教程 Re:Zero ROS （六）—— 获取&编写&检验 -＞ odom坐标系

其他 2021-03-08 18:21:54 阅读次数: 0 
 **前情提要：已可将模型载入gazebo与rviz，且可用按键控制 **

《教程 Re:Zero ROS （五）—— 导入模型，关节控制器》
https://blog.csdn.net/Lovely_him/article/details/107806662

教程 Re:Zero ROS （六）

—— 获取&编写&检验 -> odom坐标系

1. 获取odom坐标系

  • 1）在定位之前，还需要获取odom坐标。所谓odom坐标系即是指机器人相对运动坐标系。假设现在机器人开始启动，那儿设所处位置为原点，然后建立坐标系。根据运动的距离和方向得出相对于起始位置的坐标点。然后odom坐标系再相对map坐标系，map即是地图坐标系，我们人为规定的地图坐标。这样就可以实现机器人的定位功能了。涉及到坐标系就涉及到tf树，继续阅读前，请先掌握tf树的普通发布操作。

tf2Tutorials
http://wiki.ros.org/tf2/Tutorials

  • 2）在仿真的时候，我们可以通过仿真软件读取odom坐标。但是实际现实是不可能直接读取的。那么我们就需要通过各类传感器计算得到机器人的运动路程和方位，从而得到odom坐标。
  • 比赛时也规定不能使用gazebo节点发布的话题消息中直接获取odom，需要自己计算。我接下来打算两种方法都演示一遍，实践如何从gazebo节点话题中直接获取，还有如何通过传感器计算得到odom。为了区分launch的功能，我们这次新建一个pack软件包与launch文件，并导入之前的launch，编译无误后执行查看。最终效果如下。
  • 在这里插入图片描述

car_odom.launch

<?xml version="1.0"?>
<launch>
    <!--先启动 gazebo 并加载 模型关节消息 -->
    <include file="$(find carpack_control)/launch/car_control.launch" />
</launch>
  • 3）方法一：通过gazebo节点获取odom，代码是参考北邮学长的例程。先展示源代码，再讲解重点思路。

odom_move.py

#!/usr/bin/env python
#-*-coding:utf-8-*-

'''
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_now', 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:
            #创建msg消息对象
            arrayIndex = msg.name.index('racecar::base_link')
        except ValueError as e:
            # Wait for Gazebo to startup 等待Gazebo启动
            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
        #创建msg消息对象
        cmd = Odometry()
        cmd.header.stamp = self.last_recieved_stamp
        cmd.header.frame_id = 'map'     #建立tf的父级！！！
        cmd.child_frame_id = 'base_link' # This used to be odom
        cmd.pose.pose = self.last_received_pose
        cmd.twist.twist = self.last_received_twist
        #发送topic消息
        self.pub_odom.publish(cmd)
        #发送TF消息
        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("odom_move")#建立节点
    #调用类。在类调用类的同时调用了发送tf和topic的函数
    node = OdometryNode()
    #等待阻塞
    rospy.spin()
  • 4）根据代码可知，主要方法是从/gazebo/link_states话题中读取gazebo_msgs/LinkStates消息，再从msg.name.index中提取racecar::base_link键值对应的arrayIndex键位。使用rosmsg show指令查看该msg消息，和运行launch文件后打开rqt_graph工具查看节点话题，验证我们的想法。
  • 在这里插入图片描述
  • 在这里插入图片描述
  • 5）可以看到gazebo节点确实有发布/gazebo/link_states话题，使用rostopic echo实时打印查看。又或者是在IDE内快速查看，RoboWare IDE在启动ros后可以在线查看所有在线上的节点、话题、服务等。具体操作：在左边选项栏内打开“ROS视图”，然后在“活动话题”内单击箭头下拉查看话题消息，如果双击话题名称，即在IDE集成终端内自动输入rostopic echo指令打印话题消息。
  • 在这里插入图片描述
  • 6）由于gazebo_msgs/LinkStates消息内容过多，无法全部展示。该msg内包涵gazebo内所有关节的状态消息。msg 消息内第一个name数组即是储存关节的名称，之后2个数组分别存储着所有关节的状态。所以odom_move.py实现的功能就是找到对应名称的数组下标，然后利用下标把姿态数组内的数据提取出来。最后将数据整理打包发送至tf树与/odom_now话题（该话题为之后的功能包所用）。
  • 在这里插入图片描述
  • 7）现在还有一 （两） 个问题，就是向tf树发送的坐标消息中，父级坐标系是map，子级坐标系是base_link，这里需要改为：父级是odom，子级是base_footprint。理想情况下map与odom坐标系是会重合的，但是实际情况总由飘逸误差。因为是从gazebo中提取的坐标系消息，所以属于百分百重合的理想情况。但是我们按实际情况的流程来，保留odom坐标，另外base_footprint与base_link是投影关系，这个是车模本身已有定义了。因为tf树只能从上到下分支，base_link可改为base，base_footprint可改为base_link，这样就符合传统用法。但是我懒得改， 我觉得沿用原本的比较好。
# 源代码
        arrayIndex = msg.name.index('racecar::base_link')
		cmd.header.frame_id = 'map'     #建立tf的一二级！！！
		cmd.child_frame_id = 'base_link' # This used to be odom
# 修改后
        arrayIndex = msg.name.index('shcrobot::base_footprint')
        cmd.header.frame_id = 'odom'     #建立tf的一二级！！！
        cmd.child_frame_id = 'base_footprint' # This used to be odom

ROS中的坐标系map、odom、base_link、base_footprint、base_laser
https://blog.csdn.net/abcwoabcwo/article/details/101108477

  • 关于修改的内容，北邮学长也在古月居论坛上给出了教程。不过，因为工程的一些设置不同，所以不一样。初学者不懂，完全跟着做的话，十有八九一直无法实现效果。

智能车仿真 —— 2020室外光电创意组线上仿真赛(二)
https://www.guyuehome.com/9251

  • 8）这里再补充亿点，通过指令查看/gazebo/link_states话题发布的内容，确认我们自己的工程里的arrayIndex是不是racecar::base_link，还是其他名称。如果填写错误则无法正确生成tf树关系。
rostopic echo /gazebo/link_states/name
  • 这条指令在话题名字后加上/name表示只查看话题消息下的/name分类，这个使用方法是在用rqt_plot时发现的，需要熟练掌握。因为很多话题消息的内容很多，比如这次的LinkStates msg消息，全是数组内容。我暂停后根本查看不到第一个数组的内容，终端显示的内容有限拉到最上都无法看。这时就可以指定只显示部分内容（你甚至可以指定只显示一个数组内的某个元素的值）。效果如下：
  • 在这里插入图片描述
  • 从打印的消息可以看出，我的工程内arrayIndex值应该对应shcrobot::base_footprint名称的下标。这个shcrobot前缀，其实是我一开始定义在gazebo内生成的机器人模型的名字，即是在car_gazebo.launch，如下所示，这里我定义了名称shcrobot，所以关于机器人模型关节的名称也全都有该前缀。你也可以修改为其他名字。
<!-- 在gazebo中加载机器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
        args="-urdf -model shcrobot -param robot_description -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos)"/>
  • 9）修改.py文件、修改权限、修改CMakeLists.txt、编译无误后执行查看效果。使用工具rqt_graph与rqt_tf_tree（注意rqt_tf_tree只能用rosrun启动）查看。可以在rqt_gui内一起打开。
  • 从下图可知，/odom_move节点从/gazebo/link_states话题订阅消息然后再向tf树发布消息。同时tf树中，odom坐标系与base_footprint坐标系的关系也正确链接。实验完成，效果达到。
  • 在这里插入图片描述
    在这里插入图片描述

2.编写odom坐标系

  • 1）在编写之前先介绍个新概念——里程计。我个人理解，在ros里能起到定位反馈作用的传感器都可以笼统的算作里程计，我接触到的就是摄像头、激光雷达、编码器、陀螺仪。而之前一直提到odom坐标系，其实就是指里程计坐标系！！！

ROS里程计的学习(odometry) (一)
https://blog.csdn.net/zhu751191958/article/details/79322364

  • 里程计msg消息nav_msgs/Odometry，会被多个功能包订阅，包括定位amcl、导航move、建图slam、避障等。它的好坏直接决定了小车最终定位导航的效果，就好比人的小脑或五感，如果连自己的位置都无法确定或前后矛盾，就直接导致任务失败。
  • 2）在wiki-ros中有记载如何通过里程计消息获取odom坐标系，古月居的教程也有中文翻译+简析版。不过不能直接套用在我这个小车模型上，而且我习惯使用Python编写ros，所以需作亿点修改。建议先看懂例程再往下看。

navigation/Tutorials/Robot Setup/Odom
http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom
ROS探索总结（二十一）——如何发布里程计消息
https://www.guyuehome.com/332

  • 3）先在工程内新建.py文件，然后一顿常规操作。以下是我的编写odom坐标系程序：
  • 在这里插入图片描述
#!/usr/bin/env python
#-*-coding:utf-8-*-

import rospy
import math
import tf2_ros
from tf.transformations import euler_from_quaternion
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Imu, JointState
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped

# 定义部分参数
frame_name  = "odom"
child_frame_name = "base_footprint"
pub_name    = "odom_topic"
sub_name    = "imu_data"
sub2_name   = "/racecar/joint_states"
# 定义起始坐标
start_x     = -0.5
start_y     = 0
start_z     = 0
class odom_class:
    odom_topic = Odometry()
    odom_tf = TransformStamped()
    # Yaw Angle 偏航角
    yaw_angle = 0
    old_yaw_angle = 0
    old_position = 0
    # 微分线速度
    #velocity = 0
    def __init__(self):
        # 订阅 IMU 传感器（陀螺仪）
        rospy.Subscriber(sub_name, Imu, self.callback_imu, queue_size=1)
        # 订阅 关节
        rospy.Subscriber(sub2_name, JointState, self.callback_join, queue_size=1)
        # 发布 odom 坐标
        self.pub = rospy.Publisher(pub_name, Odometry, queue_size = 1)
        # 发布 tf odom 到 odom_footprint
        self.br = tf2_ros.TransformBroadcaster()
        # 定义时间差
        rate = rospy.Rate(20)
        #
        self.now_time_joint = rospy.Time.now().to_sec()
        self.old_time_joint = rospy.Time.now().to_sec()
        #
        self.odom_topic.pose.pose.position.x = start_x
        self.odom_topic.pose.pose.position.y = start_y
        self.odom_topic.pose.pose.position.z = start_z
        self.odom_topic.header.frame_id = frame_name
        self.odom_topic.child_frame_id = child_frame_name
        self.odom_tf.header.frame_id = self.odom_topic.header.frame_id
        self.odom_tf.child_frame_id = self.odom_topic.child_frame_id
        #

    def callback_imu(self, data):
        # 正交分解 偏航角（必须）
        #（在起始位置：以中线x轴为分界线，左正半y轴为正，右负半y轴为负，超过负半x轴直接跳变正负最大值）
        self.yaw_angle = euler_from_quaternion(
            (data.orientation.x,data.orientation.y,data.orientation.z,data.orientation.w))
        # 四元数 xy轴角度（四元数，必须）
        self.odom_topic.pose.pose.orientation.x = data.orientation.x
        self.odom_topic.pose.pose.orientation.y = data.orientation.y
        self.odom_topic.pose.pose.orientation.z = data.orientation.z
        self.odom_topic.pose.pose.orientation.w = data.orientation.w
        # 正交分解 xy轴角速度
        self.odom_topic.twist.twist.angular.x = data.angular_velocity.x
        self.odom_topic.twist.twist.angular.y = data.angular_velocity.y
        self.odom_topic.twist.twist.angular.z = data.angular_velocity.z
        #self.pub.publish(self.odom_topic)

    def callback_join(self, data):
        # 计算时间差
        self.now_time_joint = rospy.Time.now().to_sec()
        add_time_joint = self.now_time_joint - self.old_time_joint
        self.old_time_joint = self.now_time_joint
        # 记录时间
        self.odom_topic.header.stamp = rospy.Time.now()
        self.odom_tf.header.stamp = rospy.Time.now()
        #try:
        # 编码器 测速
        lrw_topic = data.name.index('left_rear_axle')
        rrw_topic = data.name.index('right_rear_axle')
        # 平均速度
        velocity = (data.velocity[lrw_topic] + data.velocity[rrw_topic])/(2 * 13.95348)
        # 平均路程
        position = velocity * add_time_joint 
        # 正交分解 xy轴坐标(位置，必须)
        # 坐标没有这个问题，因为坐标是相对map坐标的，正负方向不会因为小车的转向而改变
        self.odom_topic.pose.pose.position.x += (position*math.cos(self.yaw_angle[2]))
        self.odom_topic.pose.pose.position.y += (position*math.sin(self.yaw_angle[2]))
        self.odom_topic.pose.pose.position.z += 0
        # 正交分解 xy轴速度（线速度，必须）
        # 速度是相对小车的，往前是正的，往后是负的，但是角度是相对map的，朝左是正的，朝右是负的
        if ((self.yaw_angle[2] < 1.52) and (self.yaw_angle[2] > -1.52)):
            self.odom_topic.twist.twist.linear.x = velocity*math.cos(self.yaw_angle[2])
        else :
            self.odom_topic.twist.twist.linear.x = -velocity*math.cos(self.yaw_angle[2])
        # sin的象限特性刚好，z轴始终为零
        self.odom_topic.twist.twist.linear.y = velocity*math.sin(self.yaw_angle[2])
        self.odom_topic.twist.twist.linear.z = 0

        self.pub.publish(self.odom_topic)
        # tf 坐标关系 ：odom 到 base
        self.odom_tf.transform.translation.x = self.odom_topic.pose.pose.position.x
        self.odom_tf.transform.translation.y = self.odom_topic.pose.pose.position.y
        self.odom_tf.transform.translation.z = self.odom_topic.pose.pose.position.z
        # tf 四元数关系 ：odom 到 base
        self.odom_tf.transform.rotation.x = self.odom_topic.pose.pose.orientation.x
        self.odom_tf.transform.rotation.y = self.odom_topic.pose.pose.orientation.y
        self.odom_tf.transform.rotation.z = self.odom_topic.pose.pose.orientation.z
        self.odom_tf.transform.rotation.w = self.odom_topic.pose.pose.orientation.w
        self.br.sendTransform(self.odom_tf)

        
def main():
    rospy.init_node("odom_imu")
    node = odom_class()
    rospy.spin()

if __name__ == '__main__':
    main()
  • 4）思路，①：读取陀螺仪数据：imu_data话题，获取self.odom_topic.twist.twist.angular角速度与self.odom_topic.pose.pose.orientation角度的四元数表示方式，我们一般常用使用的是弧度制，所以我使用tf内置函数转换为self.yaw_angle三轴角度！转换后返回的是三轴的角度元组，都是我们只需要第三个z轴的角度，即偏航角；
  • ②读取编码器数据：/racecar/joint_states话题，获取两后轮的data.velocity[*rw_topic]转动圈数，2次圈数的差值再除于两次时间差就等于相对于小车x轴的线速度（相对于世界坐标系的和速度）。再结合偏航角分解为相对于世界坐标系（即odom坐标系）的分速度。分速度再乘于时间就等于路程，累加在一起就是xy轴的坐标。
  • ③ ！！注意！！ 最主要的就是xy坐标，但是这个数值却是由其他数据计算得到的，误差累计最大。对应例程的部分重要内容。
//compute odometry in a typical way given the velocities of the robot
double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;

x += delta_x;
y += delta_y;
th += delta_th;
  • 5）重点说明不同的地方。以下内容极其绕，如果一遍没看懂就别看了，自己拿笔按程序推倒一遍自然明了。
  • 第一点，th偏航角，例程内的偏航角是有角速度计算得到的单位时间的偏航角，然后累加得出最终偏航角。而我是陀螺仪数据imu_data中直接读取四元数转换得到偏航角的。原因是不知道为什么陀螺仪读回来的角速度一直接近零，或是完全没有反应正常数值，所以我临时采用四元数计算偏航角。这就又有另一个问题。计算得到的偏航角是有正负的，最大3.14（弧度），最小-3.14，以x负半轴为阶跳跃突变。所以在使用偏航角分解速度和xy坐标时需要进行额外的正负判断。
# 正交分解 偏航角（必须）
#（在起始位置：以中线x轴为分界线，左正半y轴为正，右负半y轴为负，超过负半x轴直接跳变正负最大值）
self.yaw_angle = euler_from_quaternion(
	(data.orientation.x,data.orientation.y,data.orientation.z,data.orientation.w))
  • 第二点，v线速度，在我的程序内是velocity单位时间内的平均速。上一点提到分解xy坐标和线速度时需要额外进行判断。xy 路程因为是相对世界坐标系的，本身就有正负。偏航角（也是相对于世界坐标系）的正负刚好符合计算要求，所以分解计算xy轴坐标时不需要判断正负。但是！但是！但是！velocity是相对小车的坐标系的，因为是读取编码器计算得到的速度，只要小车向前走就是正，否则反之。所以当小车朝x负半轴方向（即偏航角大于3.14/2，小于-3.14/2）运动时，就需要改变符号。
  • （比赛前调试时，没发现这个问题，在反跑时速度就极其的慢，也不知道是什么问题。考虑到正跑时没有问题，排除是定位导航等功能包的问题。检查一遍后才是odom坐标消息由问题。[毕竟就写了2个py文件，控制器没东西，只能是odom计算出问题了]）

# 正交分解 xy轴速度（线速度，必须）
# 速度是相对小车的，往前是正的，往后是负的，但是角度是相对map的，朝左是正的，朝右是负的
if ((self.yaw_angle[2] < 1.52) and (self.yaw_angle[2] > -1.52)):
    self.odom_topic.twist.twist.linear.x = velocity*math.cos(self.yaw_angle[2])
else :
    self.odom_topic.twist.twist.linear.x = -velocity*math.cos(self.yaw_angle[2])
# sin的象限特性刚好，z轴始终为零
self.odom_topic.twist.twist.linear.y = velocity*math.sin(self.yaw_angle[2])
self.odom_topic.twist.twist.linear.z = 0
  • 第三点，从编码器/racecar/joint_states话题读取的转动圈数在转换为路程时需要除于一个系数，这个系数我是随便取了一个时刻的数据与从gazebo内读取的数据作对比后计算得到的，并不太正确，都是对于跑完全程来说已经足够了。
# 编码器 测速
lrw_topic = data.name.index('left_rear_axle')
rrw_topic = data.name.index('right_rear_axle')
# 平均速度
velocity = (data.velocity[lrw_topic] + data.velocity[rrw_topic])/(2 * 13.95348)
  • 另外，在获取时间差上也下了点功夫，因为对Python函数的不熟悉，在此留下些笔记。
 # 计算时间差
 self.now_time_joint = rospy.Time.now().to_sec()  # 得到实数时间数据，而不是对象
 add_time_joint = self.now_time_joint - self.old_time_joint
 self.old_time_joint = self.now_time_joint
 # 记录时间
 self.odom_topic.header.stamp = rospy.Time.now()
 self.odom_tf.header.stamp = rospy.Time.now()

ROS时间概念总结：ros::Time、ros::Duration、定时器ros::Timer&ros::Rate
https://blog.csdn.net/u013834525/article/details/83863992
Python time time()方法
https://www.runoob.com/python/att-time-time.html

  • 6）最后启动launch查看效果。因为imu_data话题在最下面，所以没有在截图内。可以看使用odom_imu代替后，节点图和tf树图都是一样的。

car_odom.launch

<?xml version="1.0"?>
<launch>
    <!--先启动 gazebo 并加载 模型关节消息 -->
    <include file="$(find carpack_control)/launch/car_control.launch" />

    <!-- 获取odom坐标 用户编写的Python文件 -->
    <!--node name="odom_move" pkg="carpack_move" type="odom_move.py"/-->
    <!-- 获取odom坐标 用户编写的Python文件 -->
    <node name="odom_imu" pkg="carpack_move" type="odom_imu.py"/>

</launch>
  • 在这里插入图片描述在这里插入图片描述

3.校验odom坐标系

  • 1）我们知道，从gazebo节点发布的/gazebo/link_states话题中获取odom坐标消息是准确的，而自己计算通过里程计消息计算的odom坐标是有较大的误差。我们可以通过比较两者的差值来估算误差大小，从而判断odom有没有大错误，或小错误。
  • 2）思路就是在写一个.py文件创建一个节点，接受两个odom坐标消息然后作差值后再发布来，最后通过按键控制小车，将误差打印在终端，观察错误和调整odom坐标的系数。
  • 3）这就需要修改一下之前创建 odom_move.py，使其节点不会往tf树发送消息，只会发布odom话题。不然，如果同时不断发送2个数值不同对象相同的tf数据，会导致坐标反复横跳。
  • 4）复制一个 odom_move.py修改为 odom_gazebo.py，再新创建一个 odom_text.py。然后按以上思路编写以下程序。
  • 在这里插入图片描述

odom_gazebo.py

#!/usr/bin/env python
#-*-coding:utf-8-*-

'''
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_now', 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:
            #创建msg消息对象
            arrayIndex = msg.name.index('shcrobot::base_footprint')
        except ValueError as e:
            # Wait for Gazebo to startup 等待Gazebo启动
            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
        #创建msg消息对象
        cmd = Odometry()
        cmd.header.stamp = self.last_recieved_stamp
        cmd.header.frame_id = 'odom'     #建立tf的一二级！！！
        cmd.child_frame_id = 'base_footprint' # This used to be odom
        cmd.pose.pose = self.last_received_pose
        cmd.twist.twist = self.last_received_twist
        #发送topic消息
        self.pub_odom.publish(cmd)
        #发送TF消息
        # 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("odom_gazebo")#建立节点
    #调用类。在类调用类的同时调用了发送tf和topic的函数
    node = OdometryNode()
    #等待阻塞
    rospy.spin()

odom_text.py

#!/usr/bin/env python
#-*-coding:utf-8-*-

import rospy
import math
import tf2_ros
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion

pub_name    = "odom_error"
sub_name    = 'odom_now'
sub2_name   = "odom_topic"

class odom_class:
    vesc_odom  = Odometry()
    # 初始化
    def __init__(self):
        # 订阅 odom_gazebo
        rospy.Subscriber(sub_name, Odometry, self.callback_imu, queue_size=1)
        # 订阅 odom_imu
        rospy.Subscriber(sub2_name, Odometry, self.callback_gazebo, queue_size=1)
        # 发布 odom_error
        self.pub = rospy.Publisher(pub_name, Odometry, queue_size = 1)
        # 定义时间差
        rate = rospy.Rate(20)

    # 获取 gazebo 发布的 odom 坐标
    def callback_gazebo(self, data):
        self.vesc_odom = data
        # 偏航角 误差，使用四元数最后一位记录
        yaw_angle = euler_from_quaternion(
            (data.pose.pose.orientation.x,data.pose.pose.orientation.y,
             data.pose.pose.orientation.z,data.pose.pose.orientation.w))
        self.vesc_odom.pose.pose.orientation.w = yaw_angle[2]

    # 获取 imu 计算的 odom 坐标
    def callback_imu(self, data):
        error = Odometry()
        # 记录基本消息
        error.header.stamp    = rospy.Time.now()
        error.header.frame_id = sub_name
        error.child_frame_id  = sub2_name
        # xy轴坐标 误差
        error.pose.pose.position.x = self.vesc_odom.pose.pose.position.x - data.pose.pose.position.x 
        error.pose.pose.position.y = self.vesc_odom.pose.pose.position.y - data.pose.pose.position.y 
        error.pose.pose.position.z = 0 
        # xy轴线速度 误差
        error.twist.twist.linear.x = self.vesc_odom.twist.twist.linear.x - data.twist.twist.linear.x 
        error.twist.twist.linear.y = self.vesc_odom.twist.twist.linear.y - data.twist.twist.linear.y  
        error.twist.twist.linear.z = 0 
        # 偏航角 误差，使用四元数最后一位记录
        yaw_angle = euler_from_quaternion(
            (data.pose.pose.orientation.x,data.pose.pose.orientation.y,
             data.pose.pose.orientation.z,data.pose.pose.orientation.w))
        data.pose.pose.orientation.w  = yaw_angle[2]
        error.pose.pose.orientation.w = self.vesc_odom.pose.pose.orientation.w - data.pose.pose.orientation.w
        # 其他用不到就不管了
        # 发布出去
        self.pub.publish(error)
        
def main():
    rospy.init_node("odom_text")
    node = odom_class()
    rospy.spin()

if __name__ == '__main__':
    main()
  • 5）事到如今，应该都已经很熟练编写了，比较我也没写很复杂，大部分都是复制粘贴。最后修改launch、CMakeLists.txt，编译，执行。
<?xml version="1.0"?>
<launch>
    <!--先启动 gazebo 并加载 模型关节消息 -->
    <include file="$(find carpack_control)/launch/car_control.launch" />

    <!-- 获取odom坐标 用户编写的Python文件 -->
    <!--node name="odom_move" pkg="carpack_move" type="odom_move.py"/-->
    <!-- 获取odom坐标 用户编写的Python文件 -->
    <node name="odom_gazebo" pkg="carpack_move" type="odom_gazebo.py"/>
    <!-- 获取odom坐标 用户编写的Python文件 -->
    <node name="odom_imu" pkg="carpack_move" type="odom_imu.py"/>
    <!-- 计算odom误差 用户编写的Python文件 -->
    <node name="odom_text" pkg="carpack_move" type="odom_text.py"/>

</launch>
  • 6）查看节点图，符合预期要求。再打印/odom_error话题，使用按键控制查看误差。
  • 在这里插入图片描述
  • 在这里插入图片描述
  • 7）对比实验结果，可以发现偏航角、线速度 偏差都比较小，因为这些值都是直接赋值的。而xy轴坐标是累加值，可以明显看到，即是小车不动呢，也会不断得偏移。而且在斜着走（不平行xy轴）时，误差会越来越大，但是如果反方向再走回去，偏差又会变小。我写的这个融合里程计计算odom坐标的程序并不怎么完善，各位学者可自行琢磨优化，或重写一个。ros 还有很多其他融合方案和滤波器，等待你的学习。

4.总结

  • 0）光odom就讲了那么扯了那么多内容呢，我还想着能这一篇完结呢。下一篇已经就完结了，下次一定。
  • 1）入门ros教程里，有且仅有的一片需要自己写认真推导的程序。希望能从头到尾的掌握。下一篇再讲ros功能包。

《教程 Re:Zero ROS （七/完） —— 建图&定位&导航 - 代价地图&路径规划 》
https://blog.csdn.net/Lovely_him/article/details/107965294

/*********目录*********/

猜你喜欢

转载自blog.csdn.net/Lovely_him/article/details/107948765
今日推荐
周排行
Elasticsearch Log GC 日志分析详解
C++调用C出现的undefined reference 之坑------缺extern "C"
WordPress无法建立到WordPress.org的安全连接
《Spark大数据处理技术》PDF版
生成二维码功能（js前端）
day2csv
LeetCode 104. 二叉树的最大深度(C#实现)——二叉树,BFS,DFS,递归,迭代
Together
【矿工配餐_IOI2007Miners 】
HDU - 4135 Co-prime(分解质因数&容斥原理)
每日归档
更多
2025-03-21(0)
2025-03-20(0)
2025-03-19(0)
2025-03-18(0)
2025-03-17(0)
2025-03-16(0)
2025-03-15(0)
2025-03-14(0)
2025-03-13(0)
2025-03-12(0)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022