Issue: TF_OLD_DATA ignoring data from the past for frame

asked 2019-01-27 20:44:46 -0500

Tawfiq Chowdhury gravatar image

updated 2019-01-27 21:59:28 -0500

I am receiving this error while executing my python code. I am using moveit interface and transforming a fixed pose value from PR2's head_plate_frame to base_frame. Here is my code:

#!/usr/bin/env python

    import sys
    import copy
    import rospy
    import moveit_commander
    import moveit_msgs.msg
    import geometry_msgs.msg
    from geometry_msgs.msg import PoseStamped
    import tf
    import roslib
    import traceback

    from std_msgs.msg import String

    def move_group_python_interface_tutorial():
      ## Initialize moveit commander
      print "============ Starting setup"
      moveit_commander.roscpp_initialize(sys.argv)
      rospy.init_node('move_group_python_interface_tutorial',
                      anonymous=True)

      robot = moveit_commander.RobotCommander()

      scene = moveit_commander.PlanningSceneInterface()

      group = moveit_commander.MoveGroupCommander("left_arm")

      display_trajectory_publisher = rospy.Publisher(
                                          '/move_group/display_planned_path',
                                          moveit_msgs.msg.DisplayTrajectory)

      print "============ Waiting for RVIZ..."
      rospy.sleep(10)

      print "============ Generating plan 1"

      br = tf.TransformBroadcaster()
      R = rospy.Rate(150)
      while not rospy.is_shutdown():
        br.sendTransform((0.7, -0.05, 1.1), (-0.0260971551735, 0.966530312455, 0.030156799172, 0.253433740969), rospy.Time(), 'head_plate_frame','base_link')      
        R.sleep()

      listener = tf.TransformListener()

      while not rospy.is_shutdown():
        try:
          now = rospy.Time.now()
          listener.waitForTransform('/base_link','/head_plate_frame',now, rospy.Duration(10.0))
          (position, quaternion) = listener.lookupTransform('/base_link', '/head_plate_frame', now)

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
          traceback.print_exc()

      pose_source = geometry_msgs.msg.Pose()
      pose_source.orientation.x = quaternion[0]
      pose_source.orientation.y = quaternion[1]
      pose_source.orientation.z = quaternion[2]
      pose_source.orientation.w = quaternion[3]
      pose_source.position.x = position[0]
      pose_source.position.y = position[1]
      pose_source.position.z = position[2]

      group.set_pose_target(pose_source)

      plan1 = group.plan()

      print "============ Waiting while RVIZ displays plan1..."
      rospy.sleep(5)

      group.clear_pose_targets()

      moveit_commander.roscpp_shutdown()

      print "============ STOPPING"


    if __name__=='__main__':
      try:
        move_group_python_interface_tutorial()
      except rospy.ROSInterruptException:
        pass

However, I am getting the following error message:

TF_OLD_DATA ignoring data from the past for frame head_plate_frame at time 0 according to authority unknown_publisher
Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained

Then I found this and I put the following line in my launch file

 <param name ="/use_sim_time" value="true"/>

After that, the rviz did not launch and I got the following error;

Reason given for shutdown: [new node registered with same name]

Here is my launch file:

<launch>
  <param name ="/use_sim_time" value="true"/>

  <node name="move_group_python_interface_tutorial" pkg="pr2_moveit_tutorials" type="move_group_python_interface_tutorial.py" respawn="false" output="screen">
  </node>


  <arg name="db" default="false" />
  <arg name="debug" default="false" />

  <include file="$(find pr2_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <node pkg="tf" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 odom_combined base_footprint 100" />

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="false"/>
    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
  </node>

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

  <include file="$(find pr2_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <include file="$(find pr2_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <include file="$(find pr2_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"/>

</launch>

Any suggestions?

edit retag flag offensive close merge delete