libgazebo_ros_diff_drive publishes tf based on wall clock time instead of sim time
I am using multiple turtlebot3 robots in my simulation with gazebo. The differential time plugin publishes a transform between the odom
frame and the base_footprint
frame. I also have a plugin which publishes the ground truth pose of the turtlebot3 libgazebo_ros_p3d.so
.
I want to generate transformations between multiple turtlebot3 robots and to do this I have a seperate node which broadcasts transform between the base_link
frame and the world
frame. The problem is that the base_link
to world
transformations are published with the sim_time
as I intend but for some reason the transformations between the base_footprint
and odom
frame are published with my computers clock time.
Because of this I cannot generate transformations between the odom
frame and the world
frame.
Heres what I would like my tf tree to be:
-------------------- world --------------------
tb1/base_link tb2/base_link tb3/base_link
| | |
tb1/base_footprint tb2/base_footprint tb3/base_footprint
| | |
tb1/odom tb2/odom tb3/odom
Heres the snippets of code:
The main launch file:
<launch>
<arg name="world_file" value="worlds/empty.world"/>
<arg name="paused" value="false"/>
<arg name="gui" default="false"/>
<arg name="headless" value="true"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="paused" value="false"/>
<arg name="world_name" value="$(arg world_file)"/>
<arg name="use_sim_time" value="true"/>
<arg name="debug" value="false"/>
<arg name="gui" value="$(arg gui)" />
<arg name="verbose" value="true"/>
</include>
</launch>
The gazebo plugins in the turtlebot's xacro file:
<!-- Differential drive plugin -->
<gazebo>
<plugin name="turtlebot3_waffle_controller" filename="libgazebo_ros_diff_drive.so">
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<odometrySource>world</odometrySource>
<publishOdomTF>true</publishOdomTF>
<robotBaseFrame>base_footprint</robotBaseFrame>
<publishWheelTF>false</publishWheelTF>
<publishTf>true</publishTf>
<publishWheelJointState>true</publishWheelJointState>
<legacyMode>false</legacyMode>
<updateRate>30</updateRate>
<leftJoint>wheel_left_joint</leftJoint>
<rightJoint>wheel_right_joint</rightJoint>
<wheelSeparation>0.287</wheelSeparation>
<wheelDiameter>0.066</wheelDiameter>
<wheelAcceleration>1</wheelAcceleration>
<wheelTorque>10</wheelTorque>
<rosDebugLevel>na</rosDebugLevel>
</plugin>
</gazebo>
<!-- ground truth publisher-->
<gazebo>
<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
<alwaysOn>true</alwaysOn>
<updateRate>50.0</updateRate>
<bodyName>base_link</bodyName>
<topicName>ground_truth_pose</topicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>world</frameName>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
</plugin>
</gazebo>
Turtlebot3.launch
<launch>
<arg name="model"/>
<arg name="namespace" value="$(arg model)_$(arg robotID)" />
<group ns="/$(arg namespace)">
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
<!-- Gazebo model spawner -->
<node name="spawn_burger" pkg="gazebo_ros" type="spawn_model"
args="-param robot_description
-urdf
-x $(arg x)
-y $(arg y)
-z 0.02
-model $(arg namespace)"/>
</group>
</launch>
base_link
to world
tf broadcaster:
#!/usr/bin/env python
import rospy
import sys
import tf
from nav_msgs.msg import Odometry
def update_tf(data):
transform_from = "{}/base_link".format(ns)
transform_to = "world"
pos = data.pose.pose.position
ori = data.pose.pose.orientation
orientation = [ori.x, ori.y, ori.z, ori.w]
position = [pos.x, pos.y, pos.z]
br = tf.TransformBroadcaster()
print rospy.Time.now()
br.sendTransform(position, orientation, rospy.Time.now(), transform_from, transform_to)
if __name__ == '__main__':
ns = sys.argv[1]
rospy.init_node("tf_broadcaster_{}".format(ns.split('_')[1]))
pose_subscriber = rospy.Subscriber("{}/ground_truth_pose".format(ns), Odometry, update_tf ...
There's been a recent ABI breakage on ROS time, could you try upgrading to the latest Kinetic debs and try again?
There's been a recent ABI breakage on ROS time which has been fixed, could you try upgrading to the latest Kinetic debs and try again?
what version of gazebo and ROS are you using? Did the problem only started happening recently?
Related issue: https://bitbucket.org/osrf/gazebo/issues/2536/update-gazebo8_ros_pkgs-to-fix-abi