Ask Your Question
0

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

asked 2016-06-01 04:07:43 -0500

webvenky gravatar image

updated 2016-06-01 04:27:50 -0500

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 flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-06-01 04:39:14 -0500

webvenky gravatar image

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2016-06-01 04:07:43 -0500

Seen: 303 times

Last updated: Jun 01 '16