Extrapolation Error, Multi robots navigation

asked 2020-09-19 15:50:17 -0600

ngkhiem97vn gravatar image

Currently, I am running move_base node on multiple robots (3 robots), with each robot has these following launch file for the navigation feature:

<group ns="$(arg robot_namespace_01)">

<!-- amcl -->
<node pkg="amcl" type="amcl" name="amcl">
  <rosparam file="$(find multi_turtlebot3_environment)/param/amcl_params.yaml" command="load" />
  <param name="initial_pose_x"   value="-2.0"/>
  <param name="initial_pose_y"   value="-0.5"/>

  <param name="odom_frame_id"    value="$(arg robot_namespace_01)/odom"/> 
  <param name="base_frame_id"    value="$(arg robot_namespace_01)/base_footprint"/>

  <!-- Use the single map server -->
  <remap from="static_map" to="/static_map"/>
</node>

<!-- move_base -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
  <!-- Set planner -->
  <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/>

  <!-- Default configs form the TB3 repos -->
  <rosparam file="$(find multi_turtlebot3_environment)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
  <rosparam file="$(find multi_turtlebot3_environment)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
  <rosparam file="$(find multi_turtlebot3_environment)/param/local_costmap_params.yaml" command="load" />
  <rosparam file="$(find multi_turtlebot3_environment)/param/global_costmap_params.yaml" command="load" />
  <rosparam file="$(find multi_turtlebot3_environment)/param/move_base_params.yaml" command="load" />
  <rosparam file="$(find multi_turtlebot3_environment)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />

  <!-- Reset config -->
  <param name="global_costmap/scan/sensor_frame"                value="$(arg robot_namespace_01)/base_scan"/>
  <param name="global_costmap/obstacle_layer/scan/sensor_frame" value="$(arg robot_namespace_01)/base_scan"/>
  <param name="global_costmap/global_frame"                     value="map"/>
  <param name="global_costmap/robot_base_frame"                 value="$(arg robot_namespace_01)/base_footprint"/>
  <param name="local_costmap/scan/sensor_frame"                 value="$(arg robot_namespace_01)/base_scan"/>
  <param name="local_costmap/obstacle_layer/scan/sensor_frame"  value="$(arg robot_namespace_01)/base_scan"/>
  <param name="local_costmap/global_frame"                      value="$(arg robot_namespace_01)/odom"/>
  <param name="local_costmap/robot_base_frame"                  value="$(arg robot_namespace_01)/base_footprint"/>

  <!-- Centralized map server -->
  <remap from="map" to="/map"/>
</node>

</group>

Each robot that I have has namespace robot_namespace_01, robot_namespace_02, robot_namespace_03 respectively

When I send the navigation goal to the first robot robot_namespace_01, everything seems to be OK. However, when I send my navigation goal to the other two I got the error as follows:

Extrapolation Error: Lookup would require extrapolation into the future. Requested time 103.657000000 but the latest data is at time 103.625000000 when looking up transform from frame [turtlebot03/odom] to frame [map]

My sending goal code:

void sendGoal(const std::string& actionlib, const geometry_msgs::PoseStamped::ConstPtr& poseStamped)
{
  //tell the action client that we want to spin a thread by default
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac(actionlib, true);

  //wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  move_base_msgs::MoveBaseGoal move_base_goal;
  move_base_goal.target_pose.header.frame_id = "map";
  move_base_goal.target_pose.header.stamp    = ros::Time::now();

  move_base_goal.target_pose.pose.position.x = poseStamped->pose.position.x;
  move_base_goal.target_pose.pose.position.y = poseStamped->pose.position.y;
  move_base_goal.target_pose.pose.position.z = poseStamped->pose.position.z;
  move_base_goal.target_pose.pose.orientation.x = poseStamped->pose.orientation.x;
  move_base_goal.target_pose.pose.orientation.y = poseStamped->pose.orientation.y;
  move_base_goal.target_pose.pose.orientation.z = poseStamped->pose.orientation.z;
  move_base_goal.target_pose.pose.orientation.w = poseStamped->pose.orientation.w ...
(more)
edit retag flag offensive close merge delete

Comments

Nobody? :(

ngkhiem97vn gravatar image ngkhiem97vn  ( 2020-10-08 08:42:02 -0600 )edit