Errors in transformations between frames

edit

I have been trying to get multiple turtlebots inside gazebo and control them. Everything works fine except for transformations. Here's what I am doing so far:

• In my main ros script I have a subscriber registered to the /gazebo/model_states topic:

rospy.Subscriber('/gazebo/model_states', ModelStates, self.handle_turtle_pose)

The handle_turtle_pose callback:

def handle_turtle_pose(self, msg):
  tbot_arr = [j for j in msg.name if "turtlebot" in j]
  for i in tbot_arr:
    idx = msg.name.index(i)
    pos = msg.pose[idx].position
    ori = msg.pose[idx].orientation
    orientation = [ori.x, ori.y, ori.z, ori.w]
    position = [pos.x, pos.y, pos.z]
    br = tf.TransformBroadcaster()
    br.sendTransform(position, orientation, rospy.Time.now(), "/"+i+"/odom", "/world"

The tbot_arr is just created to get the turtlebot models from the model_state message.

• Inside the turtlebot.launch file I have a static_transform_publisher node running for transforming the laserScan link to the turtlebot's base_footprint (line:30)

<launch>
  <arg name="robot_name" default="turtlebot"/>
  <arg name="robotID" default="0"/>
  <arg name="x" default="0"/>
  <arg name="y" default="0"/>
  <arg name="z" default="0.1"/>
  <arg name="th" default="0"/>
  <arg name="namespace" value="$(arg robot_name)_$(arg robotID)" />
  <arg name="base"      value="$(optenv TURTLEBOT_BASE kobuki)"/> <!-- create, roomba -->
  <arg name="stacks"    value="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons --> 
  <arg name="3d_sensor" value="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>  <!-- kinect, asus_xtion_pro --> 
  <group ns="/$(arg namespace)">
    <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find turtlebot_description)/robots/$(arg base)_$(arg stacks)_$(arg 3d_sensor).urdf.xacro'" />
    <param name="robot_description" command="$(arg urdf_file)" />
    <!-- Gazebo model spawner -->
    <node name="spawn_turtlebot_model" pkg="gazebo_ros" type="spawn_model"
            args="-param robot_description
                  -urdf
                  -x $(arg x)
                  -y $(arg y)
                  -z $(arg z)
                  -model $(arg namespace)"/>
    <!-- Velocity muxer -->
     <node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
     <node pkg="nodelet" type="nodelet" name="cmd_vel_mux"
           args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
       <param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml" />
       <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
     </node>
     <node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 /$(arg namespace)/base_footprint /$(arg namespace)/sonar2_link 100" />
     <!-- Bumper/cliff to pointcloud (not working, as it needs sensors/core messages) -->
     <include file="$(find turtlebot_bringup)/launch/includes/kobuki/bumper2pc.launch.xml"/>
  </group>

</launch>

• On running rosrun rqt_tf_tree rqt_tf_tree I get the following graph, which seems okay to me. had to give a link as i'm new here and didn't have enough points: Link to image

• But here's what I get when i try to find tf's between two frames using rosrun tf tf_echo world /turtlebot_0/base_footprint :

Failure at 16528.130000000
Exception thrown:"world" passed to lookupTransform argument target_frame does not exist. 
The current list of frames is:
Failure at 16529.130000000
Exception thrown:Lookup would require extrapolation into the past.  Requested time 16529.120000000 but the earliest data is at time 1537945281.875200986, when looking up transform from frame [turtlebot_0/base_footprint] to frame [world]
The current list of frames is:
Frame turtlebot_0/odom exists with parent world.
Frame turtlebot_1/sonar2_link exists with parent turtlebot_1/base_footprint.
Frame ...