small difference between the goal received and the one calculated by the transformations in a global planner. [closed]

edit

Hello

I am trying to implement my own global planner for a specific situation. The robot must reach a goal a few meters from its current position and the trajectory must be plotted on the odometry frame (not yet implemented). To do this, I post a message in "/ move_base / goal" with frame_id = "base_footprint". However, the result I receive and has been converted from base_footprint to the global frame "map" (I believe it is the move_base node that performs this conversion and calls this method) does not correspond to the tf information (base_footprint -> odom -> map) with the same timeStamp as the goal. Here's an example

rostopic pub /move_base/goal move_base_msgs/MoveBaseAionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  target_pose:
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs: 0
      frame_id: 'base_footprint'
    pose:
      position:
        x: -1.0
        y: -1.0
        z: 0.0
      orientation:
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0"

My own global planner:

 #include <pluginlib/class_list_macros.h>
 #include "planejador_header.h"
 #include <tf/transform_listener.h>

 #include <iostream>

 //register this planner as a BaseGlobalPlanner plugin
 PLUGINLIB_EXPORT_CLASS(planejador::Planejador, nav_core::BaseGlobalPlanner)

 using namespace std;

 tf::TransformListener listenerMap_odom;
 tf::TransformListener listenerMap_BasefootPrint;
 tf::TransformListener listenerOdom_Basefootprint; 


 //Default Constructor
 namespace planejador {

 Planejador::Planejador (){

 }

 Planejador::Planejador(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
   initialize(name, costmap_ros);
 }

 void Planejador::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
 }

 bool Planejador::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,  std::vector<geometry_msgs::PoseStamped>& plan ){

    tf::StampedTransform transformMap_odom;
    try{
          listenerMap_BasefootPrint.lookupTransform("/map", "/odom",  goal.header.stamp, transformMap_odom); 
    }
    catch (tf::TransformException ex){
          ROS_ERROR("%s",ex.what());
          ros::Duration(1.0).sleep();
    }

    tf::StampedTransform transformOdom_BaseFootprint;
    try{            
          listenerOdom_Basefootprint.lookupTransform("/odom", "/base_footprint", goal.header.stamp, transformOdom_BaseFootprint);         
    }
    catch (tf::TransformException ex){
          ROS_ERROR("%s",ex.what());
          ros::Duration(1.0).sleep();
    }      

    plan.push_back(start);
    plan.push_back(goal);


    ROS_INFO("Goal -> Frame_ID:%s, TimeStamp secs: %d, TimeStamp nsecs: %d, x: %.8f, y: %.8f, quatX: %.8f, quatY: %.8f, quatZ: %.8f, quatW: %.8f ", goal.header.frame_id.c_str(), start.header.stamp.sec, start.header.stamp.nsec, goal.pose.position.x, goal.pose.position.y, goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w);

    ROS_INFO("Transform Map_odom->  TimeStamp sec: %d, TimeStamp NSec: %d,  X:%.8f, y: %.8f, quatX: %.8f, quatY: %.8f, quatZ: %.8f, quatW: %.8f ",transformMap_odom.stamp_.sec, transformMap_odom.stamp_.nsec, transformMap_odom.getOrigin().getX(), transformMap_odom.getOrigin().getY(), transformMap_odom.getRotation().getX(), transformMap_odom.getRotation().getY(), transformMap_odom.getRotation().getZ(), transformMap_odom.getRotation().getW());

    ROS_INFO("Transform Odom_BaseFootprint->  TimeStamp sec: %d, TimeStamp NSec: %d, X:%.8f, y: %.8f, quatX: %.8f, quatY: %.8f, quatZ: %.8f, quatW: %.8f ",transformOdom_BaseFootprint.stamp_.sec, transformOdom_BaseFootprint.stamp_.nsec, transformOdom_BaseFootprint.getOrigin().getX(), transformOdom_BaseFootprint.getOrigin().getY(), transformOdom_BaseFootprint.getRotation().getX(), transformOdom_BaseFootprint.getRotation().getY(), transformOdom_BaseFootprint.getRotation().getZ(), transformOdom_BaseFootprint.getRotation().getW());

    return true;
 }
 };

text output of the ROSINFO command right after publishing the goal:

 [ INFO] [1592074629.656658151, 47.916000000]: Goal -> Frame_ID:map, TimeStamp secs: 47, TimeStamp nsecs: 914000000, x: -0.97322305, y: -1.01870054, quatX: -0.00000092, quatY: -0.00000007, quatZ: -0.00302775, quatW: 0.99999542 

[ INFO] [1592074629 ...