沉默杀手HomeArchivesTagsCategories
Action通信(二) MoveBase客户端和服务端的常用代码
|ROSROS程序和源码分析|
Word count: 779|Reading time: 3 min

MoveBase.action

1
2
3
4
geometry_msgs/PoseStamped target_pose
---
---
geometry_msgs/PoseStamped base_position

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


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

1
2
3
4
5
6
7
MoveBaseActionFeedback.msg  
MoveBaseActionResult.msg  
MoveBaseResult.msg
MoveBaseActionGoal.msg      
MoveBaseFeedback.msg      
MoveBaseAction.msg          
MoveBaseGoal.msg

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

1
2
3
4
geometry_msgs/PoseStamped pose_stamped
uint16 current_recovery_number
uint16 total_number_of_recoveries
string recovery_behavior_name

客户端

1
2
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

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

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

发送goal的常用程序

1
2
3
4
5
6
7
8
9
10
11
12
13
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());
}

三个回调函数 ：当action完成后，调用回调函数doneCb一次。 当action激活后，调用回调函数activeCb一次，此时还没有获得路径。 收到feedback后，不断调用回调函数feedbackCb，一直到action结束

  • 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，会报错但是不会让进程退出：

两个回调函数

actionlib::SimpleActionServer的成员函数

1
2
3
4
5
// Allows users to register a callback to be invoked when a new goal is available.
void    registerGoalCallback (boost::function< void()> cb)

// Allows users to register a callback to be invoked when a new preempt request is available.
void    registerPreemptCallback (boost::function< void()> cb)

客户端向SimpleActionServer发送新目标时，isNewGoalAvailable()为true，isPreemptRequested()也为true。 如果客户端调用cancelGoal()，isNewGoalAvailable()为false，但isPreemptRequested()还是true.

问题

有时候我在move_base设置了setAborted之后，会出现报警 To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: 4 。 这是因为多次调用了 setAborted，state 4 其实就是状态 Aborted。

此时如果再发送goal，就会出现报警
Your executeCallback did not set the goal to a terminalstatus. This is a bug in your ActionServer implementation. Fix your code! For now, the ActionServer will set this goal to aborted ，这打乱了 Action 的状态机，必须避免这个报警。


我使用了一段时间后，感觉缺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)