ROS Kinetic通信编程：话题、服务、动作编程

接上篇，继续学习ROS通信编程基础

一、话题编程

步骤：

• 创建发布者
  • 初始化ROS节点
  • 向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型
  • 按照一定频率循环发布消息
• 创建订阅者
  • 初始化ROS节点
  • 订阅需要的话题
  • 循环等待话题消息，接受到消息后进行回调函数
  • 回调函数中完成消息处理
• 添加编译选项
  • 设置需要编译的代码和生成的可执行文件
  • 设置链接库
  • 设置依赖
• 运行可执行程序

talker.cpp

#include<sstream>
#include"ros/ros.h"
#include"std_msgs/String.h"
int main(int argc,char **argv)
{//ROS节点初始化ros::init(argc,argv,"talker");//创建节点句柄ros::NodeHandle n;//创建一个Publisher，发布名为chatter的topic，消息类型为std_msgs::Stringros::Publisher chatter_pub=n.advertise<std_msgs::String>("chatter",1000);//设置循环的频率ros::Rate loop_rate(10);int count=0;while(ros::ok()){//初始化std_msgs::String类型的消息std_msgs::String msg;std::stringstream ss;ss<<"hello world"<<count;msg.data=ss.str();//发布消息ROS_INFO("%s",msg.data.c_str());chatter_pub.publish(msg);//循环等待回调函数ros::spinOnce();//接受循环频率延时loop_rate.sleep();++count;}return 0;
}

listener.cpp

#include"ros/ros.h"
#include"std_msgs/String.h"
//接收到订阅的消息，会进入消息的回调函数
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{//将接收到的消息打印处理ROS_INFO("I heard:{%s}",msg->data.c_str());
}
int main(int argc,char **argv)
{//初始化ROS节点ros::init(argc,argv,"listener");//创建节点句柄ros::NodeHandle n;//创建一个Subscriber，订阅名为chatter的topic，注册回调函数chatterCallbackros::Subscriber sub=n.subscribe("chatter",1000,chatterCallback);//循环等待回调函数ros::spin();return 0;
}

在CMakeLists.txt末尾添加编译选项

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

编译

cd catkin_ws
catkin_make

运行程序

# 以下是对于Ubantu 16.04的操作，其他版本的也许操作会简洁很多
roscore
#打开新终端
cd ~/catkin_ws
#下面这一步是为了保证rosrun命令能够找到相应的功能包，有可以省去这一步骤的方法，各位可以自行查找
source ~/catkin_ws/devel/setup.bash
rosrun learning_communication talker
#打开新终端
cd ~/catkin_ws
source ~/catkin_ws/devel/setup.bash
rosrun learning_communication listener

如图，发送了hello world的同时接收了hello world。

二、服务编程

定义服务请求与应答的方式

• 定义srv文件

   mkdir ~/catkin_ws/src/learning_communication/srvsudo nano AddTwoInts.srv
  

• AddTwoInts.srv

  int64 a
  int64 b
  ---
  int64 sum
  

• 用gedit打开package.xml，在里面添加功能包依赖

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>
  

• 在CMakeLists.txt添加编译选项
  
  

步骤：

• 创建服务器
  • 初始化ROS节点
  • 创建Serve实例
  • 循环等待服务请求，进入回调函数
  • 在回调函数中完成服务功能的处理，并反馈应答数据
• 创建客户端
  • 初始化ROS节点
  • 创建一个Client实例
  • 发布服务请求数据
  • 等待Serve处理之后的应答结果
• 添加编译选项
  • 设置需要编译的代码和生成的可执行文件
  • 设置链接库
  • 设置依赖
• 运行可执行程序
  server.cpp

#include<ros/ros.h>
#include"learning_communication/AddTwoInts.h"
//service回调函数，输入参数req，输出参数res
bool add(learning_communication::AddTwoInts::Request &req,learning_communication::AddTwoInts::Response &res)
{//将输入的参数中的请求数据相加，结果放到应答变量中res.sum=req.a+req.b;ROS_INFO("request: x=%1d,y=%1d",(long int)req.a,(long int)req.b);ROS_INFO("sending back response:[%1d]",(long int)res.sum);return true;
}
int main(int argc,char **argv)
{//ROS节点初始化ros::init(argc,argv,"add_two_ints_server");//创建节点句柄ros::NodeHandle n;//创建一个名为add_two_ints的server,注册回调函数add()ros::ServiceServer service=n.advertiseService("add_two_ints",add);//循环等待回调函数ROS_INFO("Ready to add two ints.");ros::spin();return 0;
}

client.cpp

#include<cstdlib>
#include<ros/ros.h>
#include"learning_communication/AddTwoInts.h"
int main(int argc,char **argv)
{//ROS节点初始化ros::init(argc,argv,"add_two_ints_client");//从终端命令行获取两个加数if(argc!=3){ROS_INFO("usage:add_two_ints_client X Y");return 1;}//创建节点句柄ros::NodeHandle n;//创建一个client，请求add_two_ints_service//service消息类型是learning_communication::AddTwoIntsros::ServiceClient client=n.serviceClient<learning_communication::AddTwoInts>("add_two_ints");//创建learning_communication::AddTwoInts类型的service消息learning_communication::AddTwoInts srv;srv.request.a=atoll(argv[1]);srv.request.b=atoll(argv[2]);//发布service请求，等待加法运算的应答请求if(client.call(srv)){ROS_INFO("sum: %1d",(long int)srv.response.sum);}else{ROS_INFO("Failed to call service add_two_ints");return 1;}return 0;
}

关于编译时一直出现这样的报错，注意看是不是有些比如这个符号“_”没打。

添加编译设置

编译通过

输入指令

roscore
#打开新终端
source ~/catkin_ws/devel/setup.bash
rosrun learning_communication server
#打开新终端
source ~/catkin_ws/devel/setup.bash
rosrun learning_communication client 11 12

三、动作编程

动作是一种基于ROS消息实现的问答通信机制，它包含连续反馈，可以在任务过程中止运行。
动作（Action）的接口

练习ROS动作编程： 客户端发送一个运动坐标，模拟机器人运动到目标位置的过程。包括服务端和客户端的代码实现，要求带有实时位置反馈。

创建工作区间

#创建功能包
cd catkin_ws/src/
catkin_create_pkg learn_action std_msgs rospy roscpp
#编译功能包
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

创建action文件夹，并在里面创建TurtleMove.action文件

# Define the goal 
float64 turtle_target_x  
# Specify Turtle's　target position 
float64 turtle_target_y 
float64 turtle_target_theta 
---
# Define the result 
float64 turtle_final_x 
float64 turtle_final_y 
float64 turtle_final_theta 
--- 
# Define a feedback message 
float64 present_turtle_x 
float64 present_turtle_y 
float64 present_turtle_theta

在learn_action的src文件夹下，创建TurtleMove_server.cpp文件和TurtleMove_client.cpp文件

TurtleMove_server.cpp

  /*   　　　此程序通过通过动作编程实现由client发布一个目标位置 　　　然后控制Turtle运动到目标位置的过程  */ 
#include <ros/ros.h> 
#include <actionlib/server/simple_action_server.h> 
#include "learn_action/TurtleMoveAction.h" 
#include <turtlesim/Pose.h>  
#include <turtlesim/Spawn.h> 
#include <geometry_msgs/Twist.h>   
typedef actionlib::SimpleActionServer<learn_action::TurtleMoveAction> Server;   
struct Myturtle 
{     float x;     float y;     float theta; }turtle_original_pose,turtle_target_pose;   ros::Publisher turtle_vel;  void posecallback(const turtlesim::PoseConstPtr& msg)  {    ROS_INFO("Turtle1_position:(%f,%f,%f)",msg->x,msg->y,msg->theta);   turtle_original_pose.x=msg->x;    turtle_original_pose.y=msg->y;   turtle_original_pose.theta=msg->theta;  }   // 收到action的goal后调用该回调函数 void execute(const learn_action::TurtleMoveGoalConstPtr& goal, Server* as) {     learn_action::TurtleMoveFeedback feedback;       ROS_INFO("TurtleMove is working.");     turtle_target_pose.x=goal->turtle_target_x;     turtle_target_pose.y=goal->turtle_target_y;      turtle_target_pose.theta=goal->turtle_target_theta;          geometry_msgs::Twist vel_msgs;     float break_flag;          while(1)     {           ros::Rate r(10);                  vel_msgs.angular.z = 4.0 * (atan2(turtle_target_pose.y-turtle_original_pose.y,                                    turtle_target_pose.x-turtle_original_pose.x)-turtle_original_pose.theta);         vel_msgs.linear.x = 0.5 * sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +                                       pow(turtle_target_pose.y-turtle_original_pose.y, 2));          break_flag=sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +                                         pow(turtle_target_pose.y-turtle_original_pose.y, 2));         turtle_vel.publish(vel_msgs);feedback.present_turtle_x=turtle_original_pose.x;         feedback.present_turtle_y=turtle_original_pose.y;         feedback.present_turtle_theta=turtle_original_pose.theta;         as->publishFeedback(feedback);         ROS_INFO("break_flag=%f",break_flag);         if(break_flag<0.1) break;         r.sleep();     }         // 当action完成后，向客户端返回结果         ROS_INFO("TurtleMove is finished.");         as->setSucceeded();
}   
int main(int argc, char** argv) 
{     ros::init(argc, argv, "TurtleMove_server");     ros::NodeHandle n,turtle_node;     ros::Subscriber sub =turtle_node.subscribe("turtle1/pose",10,&posecallback);//订阅小乌龟的位置信息     turtle_vel = turtle_node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);//发布控制小乌龟运动的速度     // 定义一个服务器         Server server(n, "TurtleMove", boost::bind(&execute, _1, &server), false);        // 服务器开始运行         server.start();         ROS_INFO("server has started.");     ros::spin();       return 0;
} 

TurtleMove_client.cpp

#include <actionlib/client/simple_action_client.h> 
#include "learn_action/TurtleMoveAction.h" 
#include <turtlesim/Pose.h>  
#include <turtlesim/Spawn.h> 
#include <geometry_msgs/Twist.h>   
typedef actionlib::SimpleActionClient<learn_action::TurtleMoveAction> Client;   
struct Myturtle 
{     float x;     float y;   float theta; 
}turtle_present_pose;   
// 当action完成后会调用该回调函数一次 
void doneCb(const actionlib::SimpleClientGoalState& state,         const learn_action::TurtleMoveResultConstPtr& result) 
{     ROS_INFO("Yay! The TurtleMove is finished!");     ros::shutdown(); 
}   
// 当action激活后会调用该回调函数一次 
void activeCb() 
{     ROS_INFO("Goal just went active"); 
}   
// 收到feedback后调用该回调函数 
void feedbackCb(const learn_action::TurtleMoveFeedbackConstPtr& feedback) 
{     ROS_INFO(" present_pose : %f  %f  %f", feedback->present_turtle_x,                    feedback->present_turtle_y,feedback->present_turtle_theta); 
}   
int main(int argc, char** argv) 
{     ros::init(argc, argv, "TurtleMove_client");       // 定义一个客户端     Client client("TurtleMove", true);       // 等待服务器端     ROS_INFO("Waiting for action server to start.");     client.waitForServer();     ROS_INFO("Action server started, sending goal.");      // 创建一个action的goal     learn_action::TurtleMoveGoal goal;    goal.turtle_target_x = 1;     goal.turtle_target_y = 1;     goal.turtle_target_theta = 0;       // 发送action的goal给服务器端，并且设置回调函数     client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);       ros::spin();      return 0; 
} 

在package.xml里面添加依赖

<build_depend>message_generation</build_depend>  
<build_depend>actionlib</build_depend>  
<build_depend>actionlib_msgs</build_depend>
<exec_depend>message_runtime</exec_depend>  
<exec_depend>actionlib</exec_depend>  
<exec_depend>actionlib_msgs</exec_depend> 

添加完就是这样

修改learn_action里面的CMakeLists.txt，添加代码




添加编译选项

add_executable(TurtleMove_client src/TurtleMove_client.cpp)
target_link_libraries(TurtleMove_client ${catkin_LIBRARIES})
add_dependencies(TurtleMove_client ${PROJECT_NAME}_gencpp)  add_executable(TurtleMove_server src/TurtleMove_server.cpp)
target_link_libraries(TurtleMove_server ${catkin_LIBRARIES})
add_dependencies(TurtleMove_server ${PROJECT_NAME}_gencpp) 

编译

roscore
＃开一个新终端窗口
source ./devel/setup.bash
rosrun turtlesim turtlesim.node
＃新终端
source ./devel/setup.bash
rosrun learn_action TurtleMove_server
＃新终端
source ./devel/setup.bash
rosrun learn_action TurtleMove_client

运行结果如下