Note: This tutorial assumes that you have completed the previous tutorials: 使用目标反馈方法(goal callback method)编写一个简单的行为服务器.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

编写一个简单客户端线程

Description: 本教程涵盖了使用simple_action_client库来创建一个平均行为客户端。示例程序循环一个线程,创建一个行为客户端,然后发送目标到行为服务器。

Tutorial Level: INTERMEDIATE

Next Tutorial: 使用其他节点运行一个行为服务器和客户端

  Show EOL distros: 

代码

首先,使用你最喜欢的编辑器创建actionlib_tutorials/src/averaging_client.cpp,然后使用以下内容进行填充:

   1 #include <ros/ros.h>
   2 #include <actionlib/client/simple_action_client.h>
   3 #include <actionlib/client/terminal_state.h>
   4 #include <actionlib_tutorials/AveragingAction.h>
   5 #include <boost/thread.hpp>
   6 
   7 void spinThread()
   8 {
   9   ros::spin();
  10 }
  11 
  12 int main (int argc, char **argv)
  13 {
  14   ros::init(argc, argv, "test_averaging");
  15 
  16   // 创建一个行为客户端
  17   actionlib::SimpleActionClient<actionlib_tutorials::AveragingAction> ac("averaging");
  18   boost::thread spin_thread(&spinThread);
  19 
  20   ROS_INFO("Waiting for action server to start.");
  21   ac.waitForServer();
  22 
  23   ROS_INFO("Action server started, sending goal.");
  24   // 发送目标到行为
  25   actionlib_tutorials::AveragingGoal goal;
  26   goal.samples = 100;
  27   ac.sendGoal(goal);
  28 
  29   //等待行为返回
  30   bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
  31 
  32   if (finished_before_timeout)
  33   {
  34     actionlib::SimpleClientGoalState state = ac.getState();
  35     ROS_INFO("Action finished: %s",state.toString().c_str());
  36   }
  37   else
  38     ROS_INFO("Action did not finish before the time out.");
  39 
  40   // 关闭节点,在退出前加入线程
  41   ros::shutdown();
  42   spin_thread.join();
  43 
  44   //exit
  45   return 0;
  46 }

代码解释

现在让我们分块的解释代码。

   1 #include <ros/ros.h>
   2 #include <actionlib/client/simple_action_client.h>
   3 #include <actionlib/client/terminal_state.h>
   4 

  • actionlib/server/simple_action_client.h是行为库,实现简单行为客户端。
  • actionlib/client/terminal_state.h 定义目标可能的状态。

   4 #include <actionlib_tutorials/AveragingAction.h>
   5 

这里包含从上面的Averaging.action文件中生成的消息。这个头文件自动从AveragingAction.msg生成。对于更多消息定义的信息,查看msg页面。

   5 #include <boost/thread.hpp>
   6 

这个包含boost线程库用于循环示例中的线程。

   7 void spinThread()
   8 {
   9   ros::spin();
  10 }

这是一个简单的函数用于循环线程,该函数会在代码之后用到。这个线程会在后台循环ros节点。

  12 int main (int argc, char **argv)
  13 {
  14   ros::init(argc, argv, "test_averaging");
  15 
  16   // 创建一个行为客户端
  17   actionlib::SimpleActionClient<actionlib_tutorials::AveragingAction> ac("averaging");

在行为定义中的行为客户端模板,制定和行为服务器通信的消息类型。行为客户端构造函数会加载两个参数,包括需要连接的服务名和一个可选的布尔类型用于循环一个线程。这里创建的行为客户端使用服务名和自动循环选项为false作为参数,这意味着一个必须创建一个线程。

  18   boost::thread spin_thread(&spinThread);

这里创建一个线程,然后在后台开启一个ros节点。通过使用这个方法你可以创建多个线程用于你的行为客户端,如果有必要的话。

  20   ROS_INFO("Waiting for action server to start.");
  21   ac.waitForServer();

由于行为服务器有可以没有挂起或运行,行为客户端将在继续之前等待行为服务器开启。

  23   ROS_INFO("Action server started, sending goal.");
  24   // 发送目标到行为
  25   actionlib_tutorials::AveragingGoal goal;
  26   goal.samples = 100;
  27   ac.sendGoal(goal);

这里创建一个目标消息,设置目标值并且发送到行为服务器。

  29   //等待行为返回
  30   bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

现在行为客户端在继续之前,等待目标结束。超时时间设置为30秒,这意味着函数会在30秒后如果目标没有完成,将会返回false。

  32   if (finished_before_timeout)
  33   {
  34     actionlib::SimpleClientGoalState state = ac.getState();
  35     ROS_INFO("Action finished: %s",state.toString().c_str());
  36   }
  37   else
  38     ROS_INFO("Action did not finish before the time out.");

如果在超时之前目标完成,将会报告目标状态,否则通知用户目标没有在限定时间内完成。

  40   // 关闭节点,在退出前加入线程
  41   ros::shutdown();
  42   spin_thread.join();
  43 
  44   //exit
  45   return 0;
  46 }

现在目标完成,我们报告目标状态,我们需要关闭ros节点并且子啊退出之前加入到我们的线程中。

编译 & 运行客户端

添加如下行到你的CMakeLists.txt文件:

rosbuild_add_executable(averaging_client src/averaging_client.cpp)

add_executable(averaging_client src/averaging_client.cpp)
target_link_libraries(averaging_client ${catkin_LIBRARIES})

在你运行catkin_make之后,在一个新终端开启roscore:

$ roscore

然后运行客户端:

$ rosrun actionlib_tutorials averaging_client

你会看到类似如下输出:

  • [ INFO] 1250806286.804217000: Started node [/test_averaging], pid [9414], bound on [aqy], xmlrpc port [35466], tcpros port [55866], logging to [~/ros/ros/log/test_averaging_9414.log], using [real] time
    [ INFO] 1250806287.828279000: Waiting for action server to start.

检查你的客户端运行正常,列出发布的话题:

$ rostopic list -v

你会看到类似如下输出:

  • Published topics:
     * /averaging/goal [actionlib_tutorials/AveragingActionGoal] 1 publisher
     * /averaging/cancel [actionlib/GoalID] 1 publisher
     * /rosout [roslib/Log] 1 publisher
     * /rosout_agg [roslib/Log] 1 publisher
    
    Subscribed topics:
     * /time [unknown type] 2 subscribers
     * /rosout [roslib/Log] 1 subscriber
     * /averaging/feedback [unknown type] 1 subscriber
     * /averaging/status [unknown type] 1 subscriber
     * /averaging/result [unknown type] 1 subscriber

另外,你可以查看节点:

$ rxgraph &

$ rosrun rqt_graph rqt_graph &

averaging_action_client.png

这里展示了你的客户端订阅反馈、状态和期望的结果通道,以及发布目标和期望取消通道。说明客户端运行正常。

连接服务器和客户端

为了下一步使用你的行为,你需要按下Ctrl-C关闭行为客户端,然后 使用其他节点运行行为服务器和客户端.

Wiki: cn/actionlib_tutorials/Tutorials/SimpleActionClient(Threaded) (last edited 2017-03-19 07:02:16 by Playfish)