Question

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?

Asked by webvenky on 2016-06-01 04:07:43 UTC

Answer 1

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.

Asked by webvenky on 2016-06-01 04:39:14 UTC

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

Comments

Answers

Comments