Action通信(二) MoveBase客户端和服务端的常用代码

Action通信(二) MoveBase客户端和服务端的常用代码

MoveBase.action

geometry_msgs/PoseStamped target_pose
---
---
geometry_msgs/PoseStamped base_position

target_pose是目标位姿。base_position作为feedback，是base_link在world坐标系的位姿。

MoveBase的Action实际就是另一种形式的话题，会生成7个msg文件:

MoveBaseActionFeedback.msg  
MoveBaseActionResult.msg  
MoveBaseResult.msg
MoveBaseActionGoal.msg      
MoveBaseFeedback.msg      
MoveBaseAction.msg          
MoveBaseGoal.msg

另外有个RecoveryStatus.msg，目前没有使用过。

geometry_msgs/PoseStamped pose_stamped
uint16 current_recovery_number
uint16 total_number_of_recoveries
string recovery_behavior_name

template<class ActionSpec>
bool   actionlib::SimpleActionClient< ActionSpec >::waitForServer (const ros::Duration &  timeout = ros::Duration(0,0) )  [inline]

一般花点时间，比如几百毫秒实现二者的连接，在此时间内的goal会发送失败。

• timeout Max time to block before returning. 参数为 0 is interpreted as an infinite timeout.
• 返回 True if the server connected in the allocated time. False on timeout

所以这样写代码比较优雅：

while( ! ac->isServerConnected() )
{
    ROS_INFO("Waiting for move_base action server to start");
    ac->waitForServer(ros::Duration(1) );
}

发送goal的常用程序

ac = new actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>("move_base", true);
// 注意是 MoveBaseGoal, 成员只有  geometry_msgs/PoseStamped  target_pose
move_base_msgs::MoveBaseGoal  goal;
// 一系列赋值 ......
ac->sendGoal(goal);
// ac->sendGoal(goal, &doneCb, &activeCb, &feedbackCb);

bool finished_before_timeout = ac->waitForResult(ros::Duration(30.0)); 
if (finished_before_timeout)
{
    actionlib::SimpleClientGoalState state = ac->getState();
    ROS_INFO("MoveBase Action finished: %s", state.toString().c_str());
}

• bool waitForResult (const ros::Duration &timeout = ros::Duration(0, 0))

Blocks until this goal finishes. 可以指定阻塞的时间，如果是0，则无线阻塞。

• SimpleClientGoalState getState() const

Get the state information for this goal. Possible States Are: PENDING, ACTIVE, RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST

如果在服务端的回调函数里没有对goal的状态进行设置，会有下面报警

Your executeCallback did not set the goal to a terminal status. This is a bug in your ActionServer implementation. Fix your code!
For now, the ActionServer will set this goal to aborted 因为没有设置goal的最终状态，比如使用setSucceeded和setAborted设置状态.

• cancelGoal

cancelGoal()之后，getState()的状态还是ACTIVE

如果没有goal处于ACTIVE，不要使用cancelGoal，会报错但是不会让进程退出：

我使用了一段时间后，感觉缺resetGoal和getGoal函数，不太方便。

参考:
ROS Action动作通讯编程C++
Writing a Callback Based SimpleActionClientTutorials(2f)Writing(20)a(20)Callback(20)Based(20)Simple(20)Action(20)Client.html)