<1>问题描述
前言-仿真实验需求:获取小车的全局坐标系下的位姿信息,因此使用gmapping建图,并使用地图服务调用发布,自己写了个节点将map坐标系添加到现有的TF树上,最终TF树见最后截图。
进入正题,想直接搭配b站赵虚左老师的教程gazebo生成的模型,学习教程时候没注意到警告,现在还债(警告见下图),连带引发的问题为:终端输入 rosrun tf tf_echo odom base_link 时候直接断断续续收不到,分析后发现是map odom base_footprint这些坐标系发布的频率不一致导致的,比如odom更新快,base_footprint还没更新就会获取不到相对位姿的情况,上解决方案。
<2>解决方案:结合网上的通用方法后有所改善但还是会零零散散出现上述报错
亲测改善成功方案如下:
关键步骤1:launch中增加下段代码,保持频率10000hz一致
<node pkg="tf" type="static_transform_publisher" name="odom_to_base_footprint" args="0.0 0.0 0.0 0 0 0.0 /odom /base_footprint 10000"/>
关键步骤2:其次 move.xacro文件修改下面三行
<publishWheelTF>false</publishWheelTF>
<publishWheelJointState>false</publishWheelJointState>
<updateRate>1</updateRate>
完整代码段如下:
<robot name="my_car_move" xmlns:xacro="http://wiki.ros.org/xacro"><!-- 传动实现:用于连接控制器与关节 --><xacro:macro name="joint_trans" params="joint_name"><!-- Transmission is important to link the joints and the controller --><transmission name="${joint_name}_trans"><type>transmission_interface/SimpleTransmission</type><joint name="${joint_name}"><hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface></joint><actuator name="${joint_name}_motor"><hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface><mechanicalReduction>1</mechanicalReduction></actuator></transmission></xacro:macro><!-- 每一个驱动轮都需要配置传动装置 --><xacro:joint_trans joint_name="left2link" /><xacro:joint_trans joint_name="right2link" /><!-- 控制器 --><gazebo><plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so"><rosDebugLevel>Debug</rosDebugLevel><publishWheelTF>false</publishWheelTF><robotNamespace>/</robotNamespace><publishTf>1</publishTf><publishOdomTF>true</publishOdomTF><publishWheelJointState>false</publishWheelJointState><alwaysOn>true</alwaysOn><updateRate>1.0</updateRate><legacyMode>true</legacyMode><leftJoint>left2link</leftJoint> <!-- 左轮 --><rightJoint>right2link</rightJoint> <!-- 右轮 --><wheelSeparation>${base_radius * 2}</wheelSeparation> <!-- 车轮间距 --><wheelDiameter>${wheel_radius * 2}</wheelDiameter> <!-- 车轮直径 --><broadcastTF>1</broadcastTF><wheelTorque>30</wheelTorque><wheelAcceleration>1\.8</wheelAcceleration><commandTopic>cmd_vel</commandTopic> <!-- 运动控制话题 --><odometryFrame>odom</odometryFrame> <odometryTopic>odom</odometryTopic> <!-- 里程计话题 --><robotBaseFrame>base_footprint</robotBaseFrame> <!-- 根坐标系 --><odometrySource>world</odometrySource> <!-- 'encoder' instead of 'world' is also possible --></plugin></gazebo></robot>
<3>运行效果:最后就可以安稳的1s一次的获取小车相对于odom、map的相对位姿信息了😄
如下图所示,终端输入rosrun tf tf_echo map base_footprint ,当前小车没有移动所以为0