Home | Tutorials | Wiki | Issues
Ask Your Question
0

libgazebo_ros_diff_drive publishes tf based on wall clock time instead of sim time

asked 2018-10-10 00:43:04 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

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 ...
(more)
edit retag flag offensive close merge delete

Comments

There's been a recent ABI breakage on ROS time, could you try upgrading to the latest Kinetic debs and try again?

chapulina gravatar imagechapulina ( 2018-10-10 13:50:41 -0500 )edit

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?

chapulina gravatar imagechapulina ( 2018-10-10 13:50:57 -0500 )edit

what version of gazebo and ROS are you using? Did the problem only started happening recently?

iche033 gravatar imageiche033 ( 2018-10-10 14:57:41 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-10-15 12:48:25 -0500

iche033 gravatar image

This should be fixed in the new release of gazebo8-ros-pkgs (2.5.17). Doing a sudo apt update and sudo apt dist-upgrade will make sure you get the latest debs. Alternatively, you can add gazebo_ros_pkgs to your workspace and build from source to bypass the ABI compatibility issue.

edit flag offensive delete link more
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2018-10-10 00:43:04 -0500

Seen: 43 times

Last updated: Oct 15