ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# Is there a way to find out the distance to goal in move_base?

I would like to send a new goal to the robot as soon as it reaches close to the goal (within a certain distance). I would like to know if there is a way to find out the distance to goal.

I do know that I can subscribe to the odometry topic and get the values. But is there any other easy way of doing it?

edit retag close merge delete

Sort by ยป oldest newest most voted

Currently I'm using this method:

double pos_x, pos_y, quat_z, quat_w, orientation_angle_x;

void poseCallback(const nav_msgs::Odometry& msgIn)
{
pos_x = msgIn.pose.pose.position.x;
pos_y = msgIn.pose.pose.position.y;
quat_z = msgIn.pose.pose.orientation.z;
quat_w = msgIn.pose.pose.orientation.w;
orientation_angle_x = 2*atan2(quat_z,quat_w);
// cout<<"Angle ====>>"<<(float)(orientation_angle_x*(180.0/3.1415))<<endl;
}

void updatePosition(ros::NodeHandle nh, string pose_str, double& temp_pos_x, double& temp_pos_y, double& temp_orientation_angle_x)
{
ros::Rate rate(5);
ros::Subscriber sub2 = nh.subscribe(pose_str, 1, &poseCallback);

pos_x = 0;
while(ros::ok()) {

ros::spinOnce();

if(pos_x!=0)
{
temp_pos_x = pos_x; temp_pos_y = pos_y;
temp_orientation_angle_x = orientation_angle_x;

cout<<"\t"<<pose_str<<"\t POS_X = "<<pos_x<<"\t POS_Y = "<<pos_y<<"\t ANGLE = "<<orientation_angle_x<<endl;
break;
}
else
cout<<"\t Updating Position: Waiting for information..."<<endl;

// Wait until it's time for another iteration.
rate.sleep();
}
}

But I think there should another easier method.

more