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

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

1 前言

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

2 配置小车里程计

◇通过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]