0
$\begingroup$

Rosanswers logo

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:

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

and then outputs the state of the goal. Furthermore move_base doesn't seem to output any helpful information:

image description

I could implement the routine on my onw, but then I would miss the functionylities of move_base and of the navigation stack implemented in ROS. Regards

Originally posted by Andromeda on ROS Answers with karma: 893 on 2014-10-19

Post score: 0

Original comments

Comment by ahendrix on 2014-10-20:
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?

Comment by Andromeda on 2014-10-20:
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.

Comment by ahendrix on 2014-10-20:
rviz is not a simulator.

Comment by Andromeda on 2014-10-20:
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.

Comment by ahendrix on 2014-10-20:
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?

Comment by Andromeda on 2014-10-20:
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...

Comment by ahendrix on 2014-10-20:
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.

Comment by Andromeda on 2014-10-20:
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

Comment by ahendrix on 2014-10-20:
Are you using your own local planner or one of the ones that is built into move_base?

Comment by Andromeda on 2014-10-20:
neither...nor...the driver listens to the /cmd_vel outputted by move_base but it is only subscribing to that topic since the driver (written on my own) will move the robot to the goal (it receives on a separate topic the coordinates) and publishes the odom position. Now it is very late here...

Comment by Andromeda on 2014-10-20:
...tomorrow I m going to investigate much deeper. My intuition says me, that the problem is somewhere else. Bye!

Comment by ahendrix on 2014-10-20:
That's a completely inappropriate way to use move_base, and violates almost all of the implicit software contract that it provides. You might as well be writing your own navigation system.

Comment by ahendrix on 2014-10-20:
You've also left out so many important details about your system that I'm no longer willing to spend time answering your questions.

Comment by Andromeda on 2014-10-21:
Sad that you feel a little bit upset. I think there has been a misunderstanding due to my previous post regarding "my controller". It is a simple robot_driver copied from another existing one. Taking a look to this navigation tutorial:

Comment by Andromeda on 2014-10-21:
Link it says: "..and the blue components must be created for each robot platform." And this is what I did.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

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

Originally posted by Andromeda with karma: 893 on 2014-10-21

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by M@t on 2016-08-01:
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?

Comment by Andromeda on 2016-08-02:
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.

Comment by M@t on 2016-08-02:
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

Comment by dottant on 2016-10-25:
Using gazebo it doesn't work, the odom twist is always not zero (if this is the problem). Anyone solved it?

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.