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

edit

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)