Dejan,
I'm not sure I quite understood the question. Are you seeing the message
"Error in moving to goal pose..." 5 times, just that the first time takes 15
seconds and the others happen back-to-back? If so, that may be a bug in
move_base and I'll have a look. If you're not seeing that message print 5
times then it could be a problem in actionlib, which I can also look at.
>From glancing briefly at your code, I'd expect things to work.
Let me know and hope all is well,
Eitan
On Sat, Oct 9, 2010 at 5:26 PM, Dejan Pangercic
<
dejan.pangercic@gmail.com>wrote:
> 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
> _______________________________________________
> PR2-users mailing list
> PR2-users@lists.willowgarage.com
> http://lists.willowgarage.com/cgi-bin/mailman/listinfo/pr2-users
>