robotiqsguy's profile - activity

Hi Niccer,

Because, I've run tests where I published transforms at 10Hz and 20Hz tf_monitor reports different delays. But here is an example of 20hz test.

Node: /state_filter 60.0905 Hz, Average Delay: 0.0086032 Max Delay: 0.0267848

I believe that the error happens in Time_Cache::find_closest in tf2/src/cache.cpp

To be verbose(sorry) this is the call chain

// move_base/src/move_base.cpp
        if(tc_->computeVelocityCommands(cmd_vel)){  

//dwa_local_planner/src/dwa_planner_ros.cpp
      bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
    // dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal
    if ( ! costmap_ros_->getRobotPose(current_pose_)) {
      ROS_ERROR("Could not get robot pose");
      return false;
    }
    std::vector<geometry_msgs::PoseStamped> transformed_plan;
    if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
      ROS_ERROR("Could not get local plan");
      return false;
    }   

//base_local_planner/src/local_planner_util.cpp
bool LocalPlannerUtil::getLocalPlan(tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan) {
  //get the global plan in our frame
  if(!base_local_planner::transformGlobalPlan(
      *tf_,
      global_plan_,
      global_pose,
      *costmap_,
      global_frame_,
      transformed_plan)) {
    ROS_WARN("LocalPlannerUtil::getLocalPlan - Could not transform the global plan to the frame of the controller");
    return false;
  }

//base_local_planner/src/goal_functions.cpp
  bool transformGlobalPlan(
      const tf::TransformListener& tf,
      const std::vector<geometry_msgs::PoseStamped>& global_plan,
      const tf::Stamped<tf::Pose>& global_pose,
      const costmap_2d::Costmap2D& costmap,
      const std::string& global_frame,
      std::vector<geometry_msgs::PoseStamped>& transformed_plan){
    const geometry_msgs::PoseStamped& plan_pose = global_plan[0];

    transformed_plan.clear();

    try {
      if (!global_plan.size() > 0) {
        ROS_ERROR("transformGlobalPlan - Received plan with zero length");
        return false;
      }

      // get plan_to_global_transform from plan frame to global_frame
      tf::StampedTransform plan_to_global_transform;
      tf.lookupTransform(global_frame, ros::Time(), 
          plan_pose.header.frame_id, plan_pose.header.stamp, 
          plan_pose.header.frame_id, plan_to_global_transform);

//tf/src/tf.cpp
void Transformer::lookupTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
                     const ros::Time& source_time, const std::string& fixed_frame, StampedTransform& transform) const
{
  geometry_msgs::TransformStamped output = 
    tf2_buffer_.lookupTransform(strip_leading_slash(target_frame), target_time,
                                strip_leading_slash(source_frame), source_time,
                                strip_leading_slash(fixed_frame));
  transformStampedMsgToTF(output, transform);
};

//tf2/src/buffer_core.cpp
geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame, 
                                                        const ros::Time& target_time,
                                                        const std::string& source_frame,
                                                        const ros::Time& source_time,
                                                        const std::string& fixed_frame) const
{
  validateFrameId("lookupTransform argument target_frame", target_frame);
  validateFrameId("lookupTransform argument source_frame", source_frame);
  validateFrameId("lookupTransform argument fixed_frame", fixed_frame);

  geometry_msgs::TransformStamped output;
  geometry_msgs::TransformStamped temp1 =  lookupTransform(fixed_frame, source_frame, source_time);
  geometry_msgs::TransformStamped temp2 =  lookupTransform(target_frame, fixed_frame, target_time);


//tf2/src/buffer_core.cpp
geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame,
                                                            const std::string& source_frame,
                                                            const ros::Time& time) const
{
  boost::mutex::scoped_lock lock(frame_mutex_);

  if (target_frame == source_frame) {
    geometry_msgs::TransformStamped identity;
    identity.header.frame_id = target_frame;
    identity.child_frame_id = source_frame;
    identity.transform.rotation.w = 1;

    if (time == ros::Time())
    {
      CompactFrameID target_id = lookupFrameNumber(target_frame);
      TimeCacheInterfacePtr cache = getFrame(target_id);
      if (cache)
        identity.header.stamp = cache->getLatestTimestamp();
      else
        identity.header.stamp = time;
    }
    else
      identity.header.stamp = time;

    return identity;
  }

  //Identify case does not need to be validated above
  CompactFrameID target_id = validateFrameId("lookupTransform argument target_frame", target_frame);
  CompactFrameID source_id = validateFrameId("lookupTransform argument source_frame", source_frame);

  std::string error_string;
  TransformAccum accum;
  int retval = walkToTopParent(accum, time, target_id, source_id, &error_string);
  if (retval != tf2_msgs::TF2Error::NO_ERROR)
  {
    switch (retval)
    {
    case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
      throw ConnectivityException(error_string);
    case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR ...

Hi All,

We are using Hyrdo's move_base and we continue to get an extrapolation error into the future. I believe this is causing our robot to stop and start during a traverse.

In looking the ROS source code it appears that the lookupTransform in navigation/base_local_planner/src/goal_functions.cpp line 110 in transformGlobalPlan is returning the error. We are using the dwa_local_planner. We are publishing transforms at 20hz from a separate node. The parameters to tf.lookupTransform are global_frame = "local" and the plan_pose.header.frame_id = "global", and the plan_pose.header.stamp comes from the generating the gloabl plan. I think the bug is coming from tf2/src/buffer_core.cpp in WalkToTopParent(). I still need to do more debugging to find the exact problem. I've turned the debug information on for move_base so I'm including a snippet of it, and the console log below. Any help would be appreciated. Thanks.

[DEBUG] [1386166204.375994679]: The action server has received a new goal request
[DEBUG] [1386166204.376260355]: A new goal has been recieved by the single goal action server
[DEBUG] [1386166204.376656845]: Accepting a new goal
[DEBUG] [1386166204.376895493]: Accepting goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.377491239]: Publishing feedback for goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.377705512]: Publishing feedback for goal with id: /waypoint_management_node-1-1386166204.372833387 and stamp: 1386166204.37
[DEBUG] [1386166204.378025665]: Planning...
[DEBUG] [1386166204.378168840]: Waiting for plan, in the planning state.
[DEBUG] [1386166204.378332478]: Full control cycle time: 0.000979315

[DEBUG] [1386166204.455307564]: Got Plan with 49 points!
[DEBUG] [1386166204.457263957]: Generated a plan from the base_global_planner
[DEBUG] [1386166204.458307175]: Planner thread is suspending
[DEBUG] [1386166204.477544259]: Publishing feedback for goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.477708316]: Publishing feedback for goal with id: /waypoint_management_node-1-1386166204.372833387 and stamp: 1386166204.37
[DEBUG] [1386166204.477914069]: Got a new plan...swap pointers
[DEBUG] [1386166204.478065066]: pointers swapped!
[ INFO] [1386166204.478229751]: Got new plan
[DEBUG] [1386166204.478925371]: In controlling state.
[DEBUG] [1386166204.515009849]: Got a valid command from the local planner: 0.500, 0.000, 2.000
[DEBUG] [1386166204.516475469]: Full control cycle time: 0.037907056

[DEBUG] [1386166204.578542367]: Publishing feedback for goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.579298889]: Publishing feedback for goal with id: /waypoint_management_node-1-1386166204.372833387 and stamp: 1386166204.37
[DEBUG] [1386166204.580845315]: In controlling state.
[DEBUG] [1386166204.609181880]: Got a valid command from the local planner: 0.500, 0.000, 2.000
[DEBUG] [1386166204.610400540]: Full control cycle time: 0.031062960

[DEBUG] [1386166204.677557610]: Publishing feedback for goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.677725508]: Publishing feedback for goal with id: /waypoint_management_node-1-1386166204.372833387 and stamp: 1386166204.37
[DEBUG] [1386166204.677947673]: In controlling state.
[DEBUG] [1386166204.704567051]: Got a valid command from the local planner: 0.500, 0.000, 2.000
[DEBUG] [1386166204.705445516]: Full control cycle time: 0.027409982

[DEBUG] [1386166204.777927638]: Publishing feedback for goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.779469173]: Publishing feedback for goal with id: /waypoint_management_node-1-1386166204.372833387 and stamp: 1386166204.37 ...