[ros-users] move_base SimpleActionClient

Hi there,

I am using "move_base" SimpleActionClient and I have a problem that it
sort of randomly fails (due to unstable/hallucinated percepts).
Using below code I wanted to make it retry 5 times. It however does
not do so (It was supposed to be retrying for 15 seconds each time, it
however does so only in the first go). What am I doing wrong?
CODE:
void moveRobot(geometry_msgs::Pose goal_pose)
{
  int count = 0;
  while (count < 5)
    {
      actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>
ac("move_base", true);

      //wait for the action server to come up
      while(!ac.waitForServer(ros::Duration(5.0)))
        {
          ROS_INFO("Waiting for the move_base action server to come up");
        }
      move_base_msgs::MoveBaseGoal goal;
      goal.target_pose.header.frame_id = "/map";
      goal.target_pose.header.stamp = ros::Time::now();

      if (count == 0)
        goal.target_pose.pose = goal_pose;
      else
        {
          goal_pose.orientation.z =  goal_pose.orientation.z + 0.01;
          goal.target_pose.pose = goal_pose;
        }
      ROS_INFO("Sending goal");
      ac.sendGoal(goal);
      ac.waitForResult();

      ROS_INFO("Pose Position x: %f, y:%f, z:%f Orientation x:%f,
y:%f, z:%f, w:%f", goal.target_pose.pose.position.x,
               goal.target_pose.pose.position.y,
goal.target_pose.pose.position.z, goal.target_pose.pose.orientation.x,
               goal.target_pose.pose.orientation.y,
goal.target_pose.pose.orientation.z,
goal.target_pose.pose.orientation.w);

      actionlib::SimpleClientGoalState state = ac.getState();
      if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
          ROS_INFO("Robot has moved to the goal pose");
          count = 10;
        }
      else
        {
          count++;
          ROS_ERROR("Error in moving robot to goal pose: %s. Retrying
%d time", state.toString().c_str(), count);
          sleep(1);
        }
    }
  if (count == 5)
    {
      ROS_ERROR("Plan aborted, exiting!!!");
      exit(1);
    }
}

cheers, d.

--
MSc. Dejan Pangercic
PhD Student/Researcher
Intelligent Autonomous Systems Group
Technische Universität München
Telephone: +49 (89) 289-26908
E-Mail: dejan.pangercic@cs.tum.edu
WWW: http://ias.cs.tum.edu/people/pangercic