Hi guys,
I configured the Navigation stack as proposed in this [tutorials](http://wiki.ros.org/navigation/Tutorials/SendingSimpleGoals), 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](http://docs.ros.org/indigo/api/actionlib/html/classactionlib_1_1SimpleActionClient.html#a6a2baa3674db5ab9b187783371209bd3).
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:
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
↧