[ros-users] move_base SimpleActionClient
Dejan Pangercic
dejan.pangercic at gmail.com
Sun Oct 10 00:26:07 UTC 2010
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 at cs.tum.edu
WWW: http://ias.cs.tum.edu/people/pangercic
More information about the ros-users
mailing list