ROS节点通信之动作

ROS计算图结构

ROS节点之间通过收发消息进行通信，消息收发机制分为话题（topic）、服务（service）和动作（action）三种。如上计算图中的节点2与节点3、节点2与节点5采用话题通信，节点2与节点4采用服务通信，节点1与节点2采用动作通信。计算图中的节点、话题、服务、动作都要有唯一名称作为标识。

ROS动作通信方式

动作通信方式是双向异步的，动作客户端向动作服务端发送目标，动作服务端要达到目标需要一个过程，动作服务端在执行目标的过程中实时地反馈消息，并在目标完成后返回结果。动作通信用于过程性的任务执行场景下，比如导航任务。

编写程序测试动作通信方式

创建一个功能包，在此功能包中设计一些节点来测试动作通信方式。

创建功能包

假设：工作空间为catkin_ws，功能包为pkg_action_example，依赖roscpp和std_msgs功能包。

cd catkin_ws/src/
catkin_create_pkg pkg_action_example roscpp std_msgs

关于创建功能包的更多信息，请参考《创建/删除ROS功能包》

示例：动作客户端节点请求动作服务端节点完成倒计时任务

目标

1. 服务器节点提供一个倒计时任务服务，并在执行任务的过程中反馈给客户端。
2. 客户端节点调用服务端的倒计时任务服务，并接受服务端的反馈。

程序设计

• nodeActionServer.cpp实现名为“NodeActionServer”的节点，提供倒计时的任务服务。
• nodeActionClient.cpp实现名为“NodeActionClient”的节点，发起倒计时的任务请求。
• 倒计时任务名称为“daniel_action_0”。

1. 新建动作类型“CountDown.action”

   mkdir -p catkin_ws/src/pkg_action_example/action
   touch countdownTimer.action
   

   CountDown.action

   #goal
   int32 initValue
   int32 step
   ---
   #result
   bool result
   ---
   #feedback
   int32 currentValue
   

2. 将动作类型CountDown.action添加至编译环境

   • 打开pkg_action_example/CMakeLists.txt，添加下面的修改：
     

     

   • 打开pkg_action_example/package.xml，添加下面的修改：
     

     本实例中CountDown.action经过编译会产生对应的*.msg和*.h文件，如图下图所示。
     在程序 中，只需要引用action_example/CountDownAction.h头文件，
     就能使用自定义动作类型以及配套的子类型。
     

3. 新建源文件nodeActionServer.cpp nodeActionClient.cpp

   cd catkin_ws/src/pkg_action_example/src/
   touch nodeActionServer.cpp nodeActionClient.cpp
   

   
   nodeActionServer.cpp

   #include "ros/ros.h"
   #include "actionlib/server/simple_action_server.h"
   #include "pkg_action_example/CountDownAction.h"
   
   #define NODE_NAME "NodeActionServer"
   #define ACTION_NAME	"daniel_action_0"
   
   static actionlib::SimpleActionServer<pkg_action_example::CountDownAction>* server = nullptr;
   
   void onRecvRequest(const pkg_action_example::CountDownGoalConstPtr& goal)
   {
   	int initVal = goal->initValue;
   	int step = goal->step;
   	int currentVal = initVal;
   	double rate = (double)1/step;
   	ROS_INFO("rate=%f\n", rate);
   	ros::Rate loopRate(rate);
   	
   	pkg_action_example::CountDownResult result;
   	pkg_action_example::CountDownFeedback feedback;
   	
   	while (ros::ok() && (0 != currentVal)) {
   		if (server->isPreemptRequested()) {
   			ROS_INFO("Preempted\n");
   			break;
   		}
   		
   		if (0 != currentVal) {
   			feedback.currentValue = currentVal;
   			server->publishFeedback(feedback);
   		}
   		
   		loopRate.sleep();
   		currentVal -= step;
   		currentVal = currentVal<0 ? 0:currentVal;
   	}
   	
   	result.result = (0 == currentVal)? true:false;
   	server->setSucceeded(result);
   }
   
   int main(int argc, char* argv[])
   {
   	ros::init(argc, argv, NODE_NAME);
   	ros::NodeHandle nodeHandle;
   	
   	server = new actionlib::SimpleActionServer<pkg_action_example::CountDownAction>(nodeHandle, ACTION_NAME, onRecvRequest, false);
   	if (nullptr == server) {
   		ROS_INFO("Create action server error\n");
   		return -1;
   	}
   	
   	server->start();
   	ros::spin();
   	
   	delete server;
   	server = nullptr;
   	
   	return 0;
   }
   

   nodeActionClient.cpp

   #include "ros/ros.h"
   #include "actionlib/client/simple_action_client.h"
   #include "actionlib/client/terminal_state.h"
   #include "pkg_action_example/CountDownAction.h"
   
   #define NODE_NAME "NodeActionClient"
   #define ACTION_NAME	"daniel_action_0"
   
   static bool lastActionDone = true;
   
   void onServerReady()
   {
   	//ROS_INFO("Action Server is ready\n", ACTION_NAME);
   }
   
   void onActionDone(const actionlib::SimpleClientGoalState& state,
   				  const pkg_action_example::CountDownResultConstPtr& result)
   {
   	ROS_INFO("Action result is %s\n", result->result? "true":"false");
   	//ros::shutdown();
   	lastActionDone = true;
   }
   
   void onRecvFeedback(const pkg_action_example::CountDownFeedbackConstPtr& feedback)
   {
   	ROS_INFO("Current value of timer is %d\n", (int)feedback->currentValue);
   }
   
   int main(int argc, char* argv[])
   {
   	ros::init(argc, argv, NODE_NAME);
   	ros::NodeHandle nodeHandle;
   	
   	actionlib::SimpleActionClient<pkg_action_example::CountDownAction> client(ACTION_NAME, true);
   	
   	ROS_INFO("Waiting for action server ...\n");
   	client.waitForServer();
   	ros::Rate loopRate(1);
   	while (ros::ok()) {
   		if (lastActionDone) {
   			int initVal, step;
   			std::cout << "Please input timer init value and step:" << std::endl;
   			std::cin >> initVal >> step;
   			
   			pkg_action_example::CountDownGoal goal;
   			goal.initValue = initVal;
   			goal.step = step;
   			
   			client.sendGoal(goal, onActionDone, onServerReady, onRecvFeedback);
   			
   			lastActionDone = false;
   			
   			ros::spinOnce();
   		}
   	}
   	
   	return 0;
   }
   

编译

1. 在pkg_action_example/CMakeLists.txt的末尾加上下面的内容：

   # node action server
   add_executable(NodeActionServer ./src/nodeActionServer.cpp)
   target_link_libraries(NodeActionServer ${catkin_LIBRARIES})
   
   # node action client
   add_executable(NodeActionClient ./src/nodeActionClient.cpp)
   target_link_libraries(NodeActionClient ${catkin_LIBRARIES})
   

   

2. 编译功能包
   在工作空间根目录中使用catkin_make，编译该工作空间中的所有功能包。
   如果需要编译指定的某个功能包（譬如：target_pkg），
   使用catkin_make -DCATKIN_WHITELIST_PACKAGES=“target_pkg”。

   cd catkin_ws
   catkin_make 或者 catkin_make -DCATKIN_WHITELIST_PACKAGES="pkg_action_example"
   

   

测试

1. 新开终端，运行主节点（如果主节点已运行，则跳过此步骤）

   roscore
   

   使用roscore运行主节点。
   

2. 运行服务器节点
   执行rosrun pkg_action_example NodeActionServer，运行服务器节点NodeActionServer。

   

3. 运行客户端节点
   执行rosrun pkg_action_example NodeActionClient，运行服务器节点NodeActionClient。