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

从/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>

◇令小车在房间内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

<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>