Ask Your Question
0

xy_goal_tolerance: the robot reaches the goal but doesn't trigger

asked 2014-10-19 04:50:11 -0600

Andromeda gravatar image

updated 2014-10-20 14:08:50 -0600

Hi guys,

I configured the Navigation stack as proposed in this tutorials, but I have it seems that the server doesn't trigger when reaching the designated goal. I played with yaw_goal_tolerance and xy_goal_tolerance (yaw = 3.14 and xy_tolerance = 1.0) but nothing... (maybe latch_xy_goal_tolerance is to be modified too?!?!)

As you can see from my code the client is connecting to the client like in the tutorial:

int counter = 0;

do{

    updateWaypoints();

    /* Define the next goal to be reached.
       For now just take the visualization marker as helper */
    next_goal_.target_pose.header.frame_id = "map";
    next_goal_.target_pose.header.stamp = ros::Time::now();
    next_goal_.target_pose.pose = marker_array_.markers[counter].pose;


    ROS_INFO( "Goal [%d] is sending now...", counter ); // it is now very important to send both!...
    sendGoalAsPoint( next_goal_ );        // ...since this one goes directly to the driver to calculate height and orientation
    action_goal_.sendGoal( next_goal_ );  // ...and this one goes to the server

    bool finished_within_time = action_goal_.waitForResult( ros::Duration( 120.0 ) );

    if( finished_within_time ) {

        if( action_goal_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED ) {
            ROS_INFO( "Goal reached!" );
        }
    } else {
        if( action_goal_.getState() == actionlib::SimpleClientGoalState::PENDING ) {
            action_goal_.cancelGoal();
            ROS_INFO( "Goal not reached. Deleted" );
        }
    }

    /* Select now the next Waypoint */
    counter++;

} while( counter < MAX_NUMBER_WAYPOINT );

I "heard" on the topic /move_base/goal and /odom and the robot reaches the pose and orientation sent to the drivers. Since removing the client server and implementing a "manual" routine, which sends the goals comparing the actual and planned position, works everything. That means that the server or client of /move_base stucks somewhere, but I don't know what can I do to find where. Any help?

UPDATE #1: I tried to see at the output of /move_base/feedback and /move_base/goal. As you can see there no difference:

    /move_base/goal
goal: 
  target_pose: 
    header: 
      seq: 0
      stamp: 
        secs: 1413827513
        nsecs: 677924498
      frame_id: map
    pose: 
      position: 
        x: 7.08270146189
        y: -2.97619508848
        z: 2.37645603035
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.748654179258
        w: 0.662960722727

and here rostopic echo /move_base/feedback

  status: 1
  text: This goal has been accepted by the simple action server
feedback: 
  base_position: 
    header: 
      seq: 0
      stamp: 
        secs: 1413827562
        nsecs: 543686202
      frame_id: map
    pose: 
      position: 
        x: 7.08269246838
        y: -2.97619141834
        z: 2.37645603031
      orientation: 
        x: 5.69693569291e-08
        y: 1.81862260271e-08
        z: 0.748654179258
        w: 0.662960722727

Anyway the output of /move_base/result doesn't display anything. Simply the goal will never reached. Every check like .isServerConnected() gives a positive answer. Could be a problem due to different namespaces? Or what could I check?

UPDATE #2: Thank to Hendrix' idea I've implemented such a function in my code, to see the state of the goal seen by /move_base (/rosout doesn't output any message from /move_base)

...
if( action_goal_.getState() == actionlib::SimpleClientGoalState::ACTIVE ) {
   ROS_INFO( "Goal not reached, but ACTIVE" );
}
...

Of course...I ve written the same function for every possible state listed here. For every goal I get the message that the goal is ACTIVE. So it seems that move_base doesn't recognize the reached goal. But it waits that the defined time expires ... (more)

edit retag flag offensive close merge delete

Comments

move_base is missing the map update rate and control loop targets by a factor of about 100x. Are you sure your robot is moving and reaching the goal at all? Are you running this on a real robot or a simulator?

ahendrix gravatar imageahendrix ( 2014-10-20 14:15:38 -0600 )edit

in a simulator: RViz to be precise. The goal is visualized through a small colored box, so it is easly identified. The robot flies to that point and stops there. To me is the point reached and the above coordinates say the same.

Andromeda gravatar imageAndromeda ( 2014-10-20 14:18:41 -0600 )edit

rviz is not a simulator.

ahendrix gravatar imageahendrix ( 2014-10-20 14:24:57 -0600 )edit

then I don't know what you mean with simulator. I ve not a robot at disposal. Anyway even reducing the control loop frequency to 5.0 Hz, doesn't change anything. The problem still persist.

Andromeda gravatar imageAndromeda ( 2014-10-20 14:30:35 -0600 )edit

rviz is only a visualizer; it displays your data, but it is not a physics engine or a sensor simulator. Perhaps you're using gazebo as your simulator?

ahendrix gravatar imageahendrix ( 2014-10-20 15:08:22 -0600 )edit

I m not so far. (for using Gazebo). I found RViz really useful and simple to use. I m going to investigate this problem deeply. It can't be so difficult...

Andromeda gravatar imageAndromeda ( 2014-10-20 15:19:36 -0600 )edit

It is not possible to use rviz as a simulator. There must be some other node that is generating your sensor data and simulating the motion of your robot.

ahendrix gravatar imageahendrix ( 2014-10-20 15:35:02 -0600 )edit

Ah! Ok, I didn't implement any sensor till now and the robot is moving (simulated) with a driver, written by myself. It works everything apart the navigation stack

Andromeda gravatar imageAndromeda ( 2014-10-20 15:37:44 -0600 )edit

3 Answers

Sort by » oldest newest most voted
0

answered 2014-10-21 15:55:29 -0600

Andromeda gravatar image

I got it. Sometime should one sleep before trying to find impossible theories. Anyway, here the explanation.

A robot driver publishes odom informations on a basis of the actual position and velocities, in order to use the navigation stack. I was maybe too tired and didn't realize that I wrongly put in my odom message the same position information in the velocity field.

Instead of writing correctly the following line of code:

//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;

//set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;

I put stupidly:

//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;

//set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = x;            <= Velocity HERE
odom.twist.twist.linear.y = y;            <= Velocity HERE
odom.twist.twist.angular.z = 0.0;         <=  Angular Velocity HERE

So... WHAT HAPPENED?!?!?

It happened that the controller moved the robot in the right position, same frame, always correctly, BUT the published velocities couldn't never get zero!!!. So move_base couldn't trigger the server since it thought that the position must be continuously adjusted.

Writing the right speed variables in the odom field, corrected the error.

Now the server triggers!

Many thanks to all the people who helped me for their suggestions. Regards

edit flag offensive delete link more

Comments

I think I'm having the same problem as you, but I don't quite understand your solution. What do you mean by "the position must be continuously adjusted"? Does the x and y velocity have to be zero for the server to successfully complete the move goal?

M@t gravatar imageM@t ( 2016-08-01 21:54:37 -0600 )edit

Simply don't put your position information in the velocity field. Otherwise the controller cannot reach and mantain the wanted position. But it is best that you post your code.

Andromeda gravatar imageAndromeda ( 2016-08-02 00:42:37 -0600 )edit

Ah I see. I'm definitely not touching the velocity so it shouldn't be a problem. I've documented my problem as best I can here

M@t gravatar imageM@t ( 2016-08-02 16:26:17 -0600 )edit

Using gazebo it doesn't work, the odom twist is always not zero (if this is the problem). Anyone solved it?

dottant gravatar imagedottant ( 2016-10-25 08:51:30 -0600 )edit
0

answered 2014-10-19 15:24:48 -0600

paulbovbel gravatar image

I'm not really certain what you mean by:

removing the client server and implementing a "manual" routine, which sends the goals comparing the actual and planned position

It sounds like you might be somehow trying to use the navigation stack for 3d navigation

sendGoalAsPoint( next_goal_ ); // ...since this one goes directly to the driver to calculate height and orientation

In which case I'd definitely recommend using MoveIt instead.


Assuming your client code is operating correctly:

First thing could be to set the xy_goal_tolerance and yaw_tolerance to something ridiculous like 1m and 3.14 rads respectively, and see if the goal still never succeeds.

To debug the problem further, I would clone a local copy of the navigation stack into your workspace, and add some handy debug messages in the helper functions that determine whether xy_goal_tolerance and yaw_tolerance are achieved.

edit flag offensive delete link more

Comments

Ok. about the first point we have already discussed it here. About the latter: you are right. I need to implement a 3D navigation stack. I m going to try with the 2 suggestions, you proposed...

Andromeda gravatar imageAndromeda ( 2014-10-19 15:44:12 -0600 )edit

Then, most probably, I will switch to Moveit. I must be honest and say I haven t much too much success implementing move_base in my flying robot. I supposed MoveIt is for robotic arms, so i didnÄ't spend too much time exploring that package.

Andromeda gravatar imageAndromeda ( 2014-10-19 15:46:39 -0600 )edit

I have updated the post above

Andromeda gravatar imageAndromeda ( 2014-10-19 15:49:58 -0600 )edit

I didn't make the connection to your previous question. Mentioning that you're doing a flying robot here would've saved a lot of confusion :P

paulbovbel gravatar imagepaulbovbel ( 2014-10-19 17:27:44 -0600 )edit

Yes....you are right, but I thought, it is better to keep the question as general as possible, so other people could take benefit of the question. Would you really use MoveIt for a quadrotor?

Andromeda gravatar imageAndromeda ( 2014-10-20 12:54:54 -0600 )edit

Well, which platform you are running makes a pretty big difference. Also, it's likely you're hitting an edge case somewhere, which is possibly due to you not using the nav stack in the intended way.

paulbovbel gravatar imagepaulbovbel ( 2014-10-20 14:38:56 -0600 )edit

I would definitely try Moveit instead of the nav stack, since it's designed for 3d motion planning.

paulbovbel gravatar imagepaulbovbel ( 2014-10-20 14:41:19 -0600 )edit

Thanks Paul. As Platform do you mean hardware (my PC) or the environment (MoveIt, RViz, etc...) ?

Andromeda gravatar imageAndromeda ( 2014-10-20 15:20:24 -0600 )edit
0

answered 2014-10-19 13:09:34 -0600

ahendrix gravatar image

updated 2014-10-20 13:47:47 -0600

It sounds like the callbacks within the action client aren't getting serviced properly. Are you passing true as the second argument to action client constructor so that it runs an internal thread to service callbacks, as described here ?

EDIT:

Given that the /move_base/result topic isn't publishing anything, it seems likely that move_base doesn't think it has reached the goal. There should be some messages in the console output or rosout from move_base that indicates that it has reached the goal, timed out or aborted.

EDIT:

There is logging output from move_base. You need to find it and read it in order to understand what is going on here. rqt_console may be useful.

edit flag offensive delete link more

Comments

Yes sir...

action_goal_( "move_base", true )

what yould be? If you can tell me which kind of tests can I do to check if the callback is working properly...

Andromeda gravatar imageAndromeda ( 2014-10-19 14:56:07 -0600 )edit

You could look at the /move_base/result topic to see if move_base thinks it is reaching the goal.

ahendrix gravatar imageahendrix ( 2014-10-19 17:31:00 -0600 )edit

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: 2014-10-19 04:50:11 -0600

Seen: 1,701 times

Last updated: Oct 21 '14