阿克曼结构移动机器人的gazebo仿真（七）

阿克曼结构移动机器人的gazebo仿真（七）

第七章、配置小车里程计并用gmapping建图

0.前言

上一节写了用键盘控制节点控制仿真小车运动，这一节需要配置小车的里程计，并且在room_mini这个world中进行gmapping建图。

1.配置小车里程计

通过gazebo节点获取odom

从/gazebo/link_states话题中读取gazebo_msgs/LinKStates消息，再从msg.name.index中提取racebot::base_footprint位姿数据，将数据整理打包发布为里程计信息，在racebot_control/scripts中新建文件gazebo_odometry.py,代码如下：

#!/usr/bin/env python

'''
This script makes Gazebo less fail by translating gazebo status messages to odometry data.
Since Gazebo also publishes data faster than normal odom data, this script caps the update to 20hz.
Winter Guerra
'''

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose, Twist, Transform, TransformStamped
from gazebo_msgs.msg import LinkStates
from std_msgs.msg import Header
import numpy as np
import math
import tf2_ros

class OdometryNode:
    # Set publishers
    pub_odom = rospy.Publisher('/odom', Odometry, queue_size=1)

    def __init__(self):
        # init internals
        self.last_received_pose = Pose()
        self.last_received_twist = Twist()
        self.last_recieved_stamp = None

        # Set the update rate
        rospy.Timer(rospy.Duration(.05), self.timer_callback) # 20hz

        self.tf_pub = tf2_ros.TransformBroadcaster()

        # Set subscribers
        rospy.Subscriber('/gazebo/link_states', LinkStates, self.sub_robot_pose_update)

    def sub_robot_pose_update(self, msg):
        # Find the index of the racecar
        try:
            arrayIndex = msg.name.index('racebot::base_footprint')
        except ValueError as e:
            # Wait for Gazebo to startup
            pass
        else:
            # Extract our current position information
            self.last_received_pose = msg.pose[arrayIndex]
            self.last_received_twist = msg.twist[arrayIndex]
        self.last_recieved_stamp = rospy.Time.now()

    def timer_callback(self, event):
        if self.last_recieved_stamp is None:
            return

        cmd = Odometry()
        cmd.header.stamp = self.last_recieved_stamp
        cmd.header.frame_id = 'odom'
        cmd.child_frame_id = 'base_footprint'
        cmd.pose.pose = self.last_received_pose
        cmd.twist.twist = self.last_received_twist
        cmd.pose.covariance =[1e-3, 0, 0, 0, 0, 0,
						0, 1e-3, 0, 0, 0, 0,
						0, 0, 1e6, 0, 0, 0,
						0, 0, 0, 1e6, 0, 0,
						0, 0, 0, 0, 1e6, 0,
						0, 0, 0, 0, 0, 1e3]

        cmd.twist.covariance = [1e-9, 0, 0, 0, 0, 0, 
                          0, 1e-3, 1e-9, 0, 0, 0,
                          0, 0, 1e6, 0, 0, 0,
                          0, 0, 0, 1e6, 0, 0,
                          0, 0, 0, 0, 1e6, 0,
                          0, 0, 0, 0, 0, 1e-9]


        self.pub_odom.publish(cmd)

        tf = TransformStamped(
            header=Header(
                frame_id=cmd.header.frame_id,
                stamp=cmd.header.stamp
            ),
            child_frame_id=cmd.child_frame_id,
            transform=Transform(
                translation=cmd.pose.pose.position,
                rotation=cmd.pose.pose.orientation
            )
        )
        self.tf_pub.sendTransform(tf)

# Start the node
if __name__ == '__main__':
    rospy.init_node("gazebo_odometry_node")
    node = OdometryNode()
    rospy.spin()

修改racebot_control.launch,将里程计发布节点添加进去:

<?xml version='1.0'?>

<launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find racebot_control)/config/racebot_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_manager" pkg="controller_manager" type="spawner" respawn="false"
        output="screen" ns="/racebot" args="left_rear_wheel_velocity_controller right_rear_wheel_velocity_controller
                                            left_front_wheel_velocity_controller right_front_wheel_velocity_controller
                                            left_steering_hinge_position_controller right_steering_hinge_position_controller
                                            joint_state_controller"/>

	<!--运行joint_state_publisher节点，发布机器人关节状态-->
	<!--<node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "state_publisher">-->
	<node name= "robot_state_publisher" pkg= "robot_state_publisher" type= "robot_state_publisher">
		<remap from="/joint_states" to="/racebot/joint_states"/>
	</node>

  <!-- servo node -->
  <node pkg="racebot_control" type="servo_commands.py" name="servo_commands" output="screen"/>

  <!-- servo node -->
  <node pkg="racebot_control" type="transform.py" name="transform" output="screen"/>


  <!-- Allow for Gazebo to broadcast odom -->
  <node pkg="racebot_control" name="gazebo_odometry_node" type="gazebo_odometry.py"/> 


</launch>

2.令小车在房间内gmapping建图

在配置好里程计和控制节点后，可以开始建图gmapping了，先安装gmapping功能包：

sudo apt-get install ros-melodic-gmapping

在racebot/launch文件夹中创建slam_gmapping.launch：

<launch>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <param name="base_frame" value="base_footprint"/> <!--机器人底盘坐标系基框架，附带在移动底盘的框架，原点-->
      <param name="odom_frame" value="odom"/> <!--里程计坐标系里程计框架，附带在里程计的框架-->
      <param name="map_frame" value="map"/> <!--地图坐标系地图框架，附带在地图上的框架-->
      <param name="map_update_interval" value="0.01"/><!--地图更新速度，秒0.01-->
      <param name="maxUrange" value="10.0"/><!--激光最大可用距离-->
      <param name="maxRange" value="12.0"/><!--zuida juli-->	
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="3"/><!--moren:1-->
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="minimumScore" value="0"/>
      <param name="linearUpdate" value="0.05"/><!--线速度角速度在地图的更新-->
      <param name="angularUpdate" value="0.0436"/>
      <param name="temporalUpdate" value="-1"/><!--moren:-1-->
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="8"/><!--moren:30 gaicheng:8-->
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/> 
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
      <!--param name="transform_publish_period" value="0.01"/-->

    </node>
</launch>

运行一下，并打开rviz进行配置，添加雷达，添加map，添加小车模型:

roslaunch racebot_gazebo racebot.launch
roslaunch racebot_gazebo slam_gmapping.launch
rviz

用键盘控制小车在房间内跑动建图，建好图后保存：

rosrun map_server map_saver -f room_mini

将保存的地图置于racebot_gazebo/map路径下边。并将rviz设置保存于racebot_gazebo/rviz路径，并命名为gmapping.rviz，新建gmapping.launch,下一次gmapping的时候可以不用进行设置：

<launch>

    <include file="$(find racebot_gazebo)/launch/slam_gmapping.launch"/>

    <!-- 启动rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find racebot_gazebo)/rviz/new_gmapping.rviz"/>

</launch>

小结

本节内容配置小车里程计，并用gmapping让小车在房间中建立了房间的二维栅格地图，下一节用amcl定位以及teb本地规划器让小车在房间里面进行导航。

参考资料

1.古月老师的<<ROS机器人开发实践>>

2.古月学院《如何在Gazebo中实现阿克曼转向车的仿真 • 王泽恩》