【移动机器人运动规划（ROS）】03_ROS话题-服务-动作

ROS话题发布

创建一个发布者

初始化ROS节点
创建句柄
向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型以及队列长度
创建并且初始化消息数据
按照一定频率循环发送消息

复制代码

 /**
 * 在功能包learn_topic的src文件夹,创建一个C++文件（文件后缀为.cpp），命名为turtle_velocity_publisher.cpp
 * 创建一个小海龟的速度发布者
 */
 #include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "turtle_velocity_publisher");//ROS节点初始化
     ros::NodeHandle n;//这里是创建句柄
     //创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
     ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>
     ("/turtle1/cmd_vel", 10);
     ros::Rate loop_rate(10);//设置循环的频率
     while (ros::ok())
     {
         //初始化需要发布的消息，类型需与Publisher一致
         geometry_msgs::Twist turtle_vel_msg;
         turtle_vel_msg.linear.x = 0.8;
         turtle_vel_msg.angular.z = 0.6;
         turtle_vel_pub.publish(turtle_vel_msg);// 发布速度消息
         //打印发布的速度内容
         ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]",
         turtle_vel_msg.linear.x, turtle_vel_msg.angular.z);
         loop_rate.sleep();//按照循环频率延时
     }
     return 0;
 }

修改CMakelist.txt

复制代码

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

编译运行

复制代码

 cd ~/ros_ws
 catkin_make
 cd ~/ros_ws
 source devel/setup.bash
 roscore
 rosrun turtlesim turtlesim_node
 rosrun learn_topic turtle_velocity_publisher
 rosnode list

Python版本

复制代码

 # 在功能包learn_topic下的scripts文件夹下新建一个python文件（文件后缀.py），命名为turtle_velocity_publisher.py，把下边的程序代码复制粘贴到turtle_velocity_publisher.py文件中，
 
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
 # 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
 import rospy
 from geometry_msgs.msg import Twist
 def turtle_velocity_publisher():
  rospy.init_node('turtle_velocity_publisher', anonymous=True) # ROS节点初始化
  # 创建一个小海龟速度发布者，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，8代表消息队列长度
  turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=8)
  rate = rospy.Rate(10) #设置循环的频率
  while not rospy.is_shutdown():
      # 初始化geometry_msgs::Twist类型的消息
      turtle_vel_msg = Twist()
      turtle_vel_msg.linear.x = 0.8
      turtle_vel_msg.angular.z = 0.6
      # 发布消息
      turtle_vel_pub.publish(turtle_vel_msg)
      rospy.loginfo("linear is :%0.2f m/s, angular is :%0.2f rad/s",
      turtle_vel_msg.linear.x, turtle_vel_msg.angular.z)
      rate.sleep()# 按照循环频率延时
 if __name__ == '__main__':
  try:
      turtle_velocity_publisher()
  except rospy.ROSInterruptException:
      pass

运行

复制代码

 # python程序不需要编译，但是需要添加可执行的权限，终端输入，
 cd ~/ros_ws/src/learn_topic/scripts
 sudo chmod a+x turtle_velocity_publisher.py
 roscore
 rosrun turtlesim turtlesim_node
 rosrun learn_topic turtle_velocity_publisher.py

ROS 话题订阅

创建一个订阅者

初始化ROS节点
创建句柄
订阅需要的话题
循环等待话题消息，接收到消息后进入回调函数
在回调函数中完成消息处理

复制代码

 // 在功能包learn_topic的src文件夹,创建一个C++文件（文件后缀为.cpp），命名为turtle_pose_subscriber.cpp
 /*创建一个小海龟的当前位姿信息接收*/
 #include <ros/ros.h>
 #include "turtlesim/Pose.h"
 // 接收消息后，会进入消息回调函数，回调函数里边会对接收到的数据进行处理
 void turtle_poseCallback(const turtlesim::Pose::ConstPtr& msg)
 {
  // 打印接收到的消息
  ROS_INFO("Turtle pose: x:%0.3f, y:%0.3f", msg->x, msg->y);
 }
 int main(int argc, char **argv)
 {
  ros::init(argc, argv, "turtle_pose_subscriber");// 初始化ROS节点
  ros::NodeHandle n;//这里是创建句柄
  // 创建一个订阅者，订阅的话题是/turtle1/pose的topic，poseCallback是回调函数
  ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10,turtle_poseCallback);
  ros::spin(); // 循环等待回调函数
  return 0;
 }

修改CMakelist.txt

复制代码

 # 在CMakelist.txt中配置，build区域下，添加如下内容，
 # add_executable说明了生成的可执行程序的文件是turtle_pose_subscriber，编译的源码是src目录下的turtle_pose_subscriber.cpp。
 # target_link_libraries说明了编译生成可执行文件是需要链接的库。
 
 add_executable(turtle_pose_subscriber src/turtle_pose_subscriber.cpp)
 target_link_libraries(turtle_pose_subscriber ${catkin_LIBRARIES})

运行

复制代码

 cd ~/ros_ws
 catkin_make
 cd ~/ros_ws
 source devel/setup.bash
 roscore
 rosrun turtlesim turtlesim_node
 rosrun turtlesim turtle_teleop_key
 rosrun learn_topic turtle_pose_subscriber
 
 # 我们可以通过rqt_graph工具来查看节点之间的通讯，终端输入，
 rqt_graph
 
 # 椭圆里边代表的是节点名称，矩形里边表示的是话题名称，箭头表示话题消息传递的方向，我们编写的订阅者节点程序turtle_pose_subscriber就是订阅了/turtle1/pose的话题消息，这一点与代码ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10,turtle_poseCallback);

ROS 服务客户端

服务service同样也是ros节点之间一种比较常用的通讯方式，与话题不一样的是，服务有服务端和客户端，客户端请求服务端提供服务，服务端服务完，需要返回服务的结果，也称回应。我们本节课程来说明，怎么写一个客户端节点程序。

创建功能包

复制代码

 # 为了与learn_topic功能包区别开来，我们重新创建一个功能包learn_service，终端输入，
 cd ~/ros_ws/src
 catkin_create_pkg learn_service std_msgs rospy roscpp geometry_msgs turtlesim

创建一个客户端

初始化ROS节点
创建句柄
创建一个Client实例
初始化并发布服务请求数据
等待Server处理之后的应答结果

复制代码

 /**
 * 在功能包learn_service的src文件夹,创建一个C++文件（文件后缀为.cpp），命名为a_new_turtle.cpp
 * 该例程将请求小海龟节点里的/spawn服务，会在规定的位置出现一只新的小海龟
 */
 #include <ros/ros.h>
 #include <turtlesim/Spawn.h>
 int main(int argc, char** argv)
 {
     ros::init(argc, argv, "a_nes_turtle");// 初始化ROS节点
     ros::NodeHandle node;
     ros::service::waitForService("/spawn"); // 等待/spawn服务
     ros::ServiceClient new_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");//创建一个服务客户端，连接名为/spawn的服务
     // 初始化turtlesim::Spawn的请求数据
     turtlesim::Spawn new_turtle_srv;
     new_turtle_srv.request.x = 6.0;
     new_turtle_srv.request.y = 8.0;
     new_turtle_srv.request.name = "turtle2";
     // 请求服务传入xy位置参数以及名字参数
     ROS_INFO("Call service to create a new turtle name is %s,at the x:%.1f,y:%.1f", new_turtle_srv.request.name.c_str(),
     new_turtle_srv.request.x,new_turtle_srv.request.y);
     new_turtle.call(new_turtle_srv); //请求服务
     ROS_INFO("Spwan turtle successfully [name:%s]",
     new_turtle_srv.response.name.c_str());// 显示服务调用结果
     return 0;
 };

复制代码

 # 在启动小海龟的节点后，再运行a_new_turtle这个程序会发现，画面中会出现另外一直小海龟，这是因为小海龟的节点提供了服务/spawn，对应的是代码中的ros::ServiceClient new_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");//创建该服务会产生另外一直小海龟turtle2，查看小海龟提供的服务可以通过rosservice list命令来查看，如下图所示，
 
 # 可以通过rosservice info /spawn，查看这个服务需要的参数
 
 # 可以看出需要有4个参数：x，y，theta，name，这四个参数在a_new_turtle.cpp里边有初始化，
 
 srv.request.x = 6.0;
 srv.request.y = 8.0;
 srv.request.name = "turtle2";
 注意：theta没有赋值，默认为0

ROS服务服务端

上节教程我们讲述了如何写一个客户端节点程序，那本节课程序来说明如何写一个服务端的节点。服务端有一个服务的回调函数，可以类比为话题订阅者的回调函数。接收到请求服务后，在服务的回调函数里，说明具体的服务内容，然后返回一个响应值。

创建一个服务端

初始化ROS节点
创建Server实例
循环等待服务请求，进入回调函数
在回调函数中完成服务的功能处理，并且反馈应答数据

复制代码

 /**
 * 在功能包learn_server的src文件夹,创建一个C++文件（文件后缀为.cpp），命名turtle_vel_command_server.cpp
 * 该例程将执行/turtle_vel_command服务，服务数据类型std_srvs/Trigger
 */
 #include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
 #include <std_srvs/Trigger.h>
 ros::Publisher turtle_vel_pub;
 bool pubvel = false;
 // service回调函数，输入参数req，输出参数res
 bool pubvelCallback(std_srvs::Trigger::Request &req,
 std_srvs::Trigger::Response &res)
 {
     pubvel = !pubvel;
     ROS_INFO("Do you want to publish the vel?: [%s]",
     pubvel==true?"Yes":"No");// 打印客户端请求数据
     // 设置反馈数据
     res.success = true;
     res.message = "The status is changed!";
     return true;
 }
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "turtle_vel_command_server");
     ros::NodeHandle n;
     // 创建一个名为/turtle_vel_command的server，注册回调函数pubvelCallback
     ros::ServiceServer command_service =
     n.advertiseService("/turtle_vel_command", pubvelCallback);
     // 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度8
     turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 8);
     ros::Rate loop_rate(10);// 设置循环的频率
     while(ros::ok())
     {
         ros::spinOnce();// 查看一次回调函数队列
     // 判断pubvel为True，则发布小海龟速度指令
         if(pubvel)
         {
             geometry_msgs::Twist vel_msg;
             vel_msg.linear.x = 0.6;
             vel_msg.angular.z = 0.8;
             turtle_vel_pub.publish(vel_msg);
         }
         loop_rate.sleep();//按照循环频率延时
     }
     return 0;
 }

运行
复制代码
```
 roscore
 rosrun turtlesim turtlesim_node
 
 # 运行服务端节点，
 rosrun learn_service turtle_vel_command_server
 
 # 调用服务，
 rosservice call /turtle_vel_command
```
程序运行说明：

首先，运行小海龟这个节点后，可以在终端输入rosservice list，查看当前的服务有哪些

然后，我们再运行turtle_vel_command_server程序，再输入rosservice list，会发现多了个turtle_vel_command，

然后，我们通过在终端输入rosservice call /turtle_vel_command调用这个服务，会发现小海龟做圆周运动，再次调用服务的话，小海龟停止了运动。这是因为在服务回调函数里边，我们把pubvel的值做了反转，然后反馈回去，主函数就会判断pubvel的值，如果是True就发布速度指令，为False则不发布指令。

ROS动作客户端

动作action是ros常用的一种通讯方式，有动作的客户端client与动作的服务端server。这里要提一下action与servcie都是具有反馈的通讯方式，那么它们之间有什么区别呢？很明显的一个区别就是，action可以按照一定的频率进行反馈当前的状态，而service只能够反馈一次，这是个很重要的点，比如在执行长时间任务的时候，用action的通信方式可以随时的查看进度，同时也可以终止请求。

action通信原理

我们看下ros-wiki的图

这里显示了client发送一个目标，也就是sendGoal，server接收到后执行相对应的程序，也就是excuteGoal。这是一个简单过程，但是其中，还有可能client按照设定的频率发送反馈的指令，server接收到后把反馈的结果发回给client。
这里有几个重要的概念需要知道：

目标goal：机器人执行一个动作，应该有明确的移动目标信息，告诉服务端"我想去哪里"或者是"我想做什么"，包括一些参数的设定，方向、角度、速度等等。从而使机器人完成动作任务。
反馈feedback：在动作进行的过程中，服务端应该把实时的状态信息反馈给客户端，告诉客户端动"现在进行到哪里了"或者是"我做到哪里了"，也方便客户端是否选择继续或者其它的一下指令。
结果result：当运动完成时，服务端把结果数据发送给客户端，告诉客户端"我现在已经到了"或者是"我现在完成了动作"，使客户端得到本次动作的全部信息，例如可能包含机器人的运动时长，最终姿势等等。

.action文件

复制代码

 # 行为规范使用.action文件。.action文件有目标goal定义，然后是结果result定义，然后是反馈feedback定义
 # 每个部分用3个连字符(---)分隔。在工作空间的src目录下，新建一个learn_action功能包，输入以下指令，
 catkin_create_pkg learn_action std_msgs rospy roscpp actionlib actionlib_msgs
 
 # 然后回到工作空间进行编译，
 cd ~/ros_ws
 catkin_make
 
 # 然后在learn_action文件夹下，新建一个文件夹，命名为action，用于存放我们定义的.action文件，终端输入，
 cd ~/ros_ws/src/learn_action
 mkdir action
 
 # 在action文件夹下，新建一个文件，命名为DoDishes.action，把以下代码，复制到里边，
 
 # Define the goal
 uint32 dishwasher_id  # Specify which dishwasher we want to use
 ---
 # Define the result
 uint32 total_dishes_cleaned
 ---
 # Define a feedback message 
 float32 percent_complete

复制代码

 # 修改CMakeLists.txt文件，修改成以下内容，
 
 find_package(catkin REQUIRED COMPONENTS
 actionlib
 actionlib_msgs
 roscpp
 rospy
 std_msgs
 genmsg
 )
 add_action_files(
 DIRECTORY
 action
 FILES
 DoDishes.action
 )
 generate_messages(
 DEPENDENCIES
 actionlib_msgs
 )

复制代码

 # 回到工作空间编译一下，终端输入，
 cd ~/ros_ws
 catkin_make
 
 # 成功编译后，对于DoDishes.action，以下消息通过genaction.py生成相关的msg文件，通过rosmsg list可以看到learn_action相关的即是
 rosmsg list

编写action客户端代码

复制代码

 // 在learn_action功能包下，新建一个文件，命名为action_client.cpp，把以下内容复制到里边，
 #include <actionlib/client/simple_action_client.h>
 #include "learn_action/DoDishesAction.h"
 
 
 typedef actionlib::SimpleActionClient<learn_action::DoDishesAction> Client;
 
 
 // 当action完成后会调用次回调函数一次
 void doneCb(const actionlib::SimpleClientGoalState& state,
      const learn_action::DoDishesResultConstPtr& result)
 {
  ROS_INFO("Yay! The dishes are now clean");
  ros::shutdown();
 }
 
 
 // 当action激活后会调用次回调函数一次
 void activeCb()
 {
  ROS_INFO("Goal just went active");
 }
 
 
 // 收到feedback后调用的回调函数, 这里说明feedback的成员percen_complete，
 // 我们可以通过rosmsg info来查询具体的成员有哪些rosmsg info learn_action/DoDishesFeedback
 // 包括上边的goal也一样，终端输入，rosmsg info learn_action/DoDishesGoal
 void feedbackCb(const learn_action::DoDishesFeedbackConstPtr& feedback)
 {
  ROS_INFO(" percent_complete : %f ", feedback->percent_complete);
 }
 
 
 int main(int argc, char** argv)
 {
  ros::init(argc, argv, "do_dishes_client");
 
 
  // 定义一个客户端, 有两个信息，第一个是客户端寻求的服务是“do_dishes”，第二个是true表示启动一个线程来为此操作的订阅提供服务。
  Client client("do_dishes", true);
 
 
  // 等待服务器端.等待"do_dishes"服务启动
  ROS_INFO("Waiting for action server to start.");
  client.waitForServer();
  ROS_INFO("Action server started, sending goal.");
 
 
  // 创建一个action的goal,goal有个成员是dishwasher_id，给它赋值成1，然后sendGoal发送目标给服务端。
  // 里边包括了goal，还有三个回调函数，分别是doneCb、activeCb和feedbackCb。
  learn_action::DoDishesGoal goal;
  goal.dishwasher_id = 1;
 
 
  // 发送action的goal给服务器端，并且设置回调函数
  client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);
 
 
  ros::spin();
 
 
  return 0;
 }

复制代码

 # 修改CMakeLists.txt文件，添加以下内容
 add_executable(action_client src/action_client.cpp)
 target_link_libraries( action_client ${catkin_LIBRARIES})
 add_dependencies(action_client ${${PROJECT_NAME}_EXPORTED_TARGETS})

复制代码

 # 回到工作空间编译，
 cd ~/ros_ws
 catkin_make
 roscore
 # 编译完成后，重新打开终端或者source一下，然后输入以下指令运行，
 rosrun learn_action action_client

ROS动作服务端

编写action客户端代码

复制代码

 // 在learn_action功能包下，新建一个文件，命名为action_server.cpp，把以下内容复制到里边，
 #include <ros/ros.h>
 #include <actionlib/server/simple_action_server.h>
 #include "learn_action/DoDishesAction.h"
 
 
 typedef actionlib::SimpleActionServer<learn_action::DoDishesAction> Server;
 
 
 // 收到action的goal后调用的回调函数
 void execute(const learn_action::DoDishesGoalConstPtr& goal, Server* as)
 {
  ros::Rate r(1);
  learn_action::DoDishesFeedback feedback;
 
 
  ROS_INFO("Dishwasher %d is working.", goal->dishwasher_id);
 
 
  // 假设洗盘子的进度，并且按照1hz的频率发布进度feedback
  for(int i=1; i<=10; i++)
  {
      feedback.percent_complete = i * 10;
      as->publishFeedback(feedback); // 把反馈发布回去给客户端，客户端收到后，就会执行feedbackCb函数里边的内容；
      r.sleep();
  }
 
 
  // 当action完成后，向客户端返回结果
  ROS_INFO("Dishwasher %d finish working.", goal->dishwasher_id);
  as->setSucceeded();            // 将活动目标的状态设置为成功。然后客户端接收到，就会执行doneCb函数里边的内容。
 }
 
 
 int main(int argc, char** argv)
 {
  ros::init(argc, argv, "do_dishes_server");
  ros::NodeHandle n;
 
 
  // 定义一个服务器
  Server server(n, "do_dishes", boost::bind(&execute, _1, &server), false);
 
  // 服务器开始运行
  server.start();
 
 
  ros::spin();
 
 
  return 0;
 }

复制代码

 # 修改CMakeLists.txt文件，添加以下内容,
 add_executable(action_server src/action_server.cpp)
 target_link_libraries( action_server ${catkin_LIBRARIES})
 add_dependencies(action_server ${${PROJECT_NAME}_EXPORTED_TARGETS})

复制代码

 # 结合上节编写的客户端程序，看一下整个DoDishes是怎么运行，首先依然是要先启动roscore，终端输入，
 roscore
 
 # 运行action客户端程序，
 rosrun learn_action action_client 
 
 # 运行action服务端程序，
 rosrun learn_action action_server
 
 # 程序启动后，客户端会按照一点的频率打印服务端发送回去的反馈信息，直到100后，整个动作完成了，显示“Yay！The dishes are now clean”，届时，服务端也会打印“Dishwasher 1 finishing working.”。

自定义话题消息类型

ROS自定义接口消息

复制代码

 # 在learn_topic功能包的目录下，新建一个文件夹msg
 cd ~/ros_ws/src/learn_topic
 mkdir msg
 
 # 切换至msg目录下，新建一个空白的msg文件，以msg为后缀表示代表是msg文件。这里我们以Information.msg为例子说明下
 string company
 string city
 
 # 在package.xml中添加功能包依赖
 <build_depend>message_generation</build_depend>
 <exec_depend>message_runtime</exec_depend>
 
 # 在CMakeLists.txt添加编译选项->在find_package里边加上message_generation
 add_message_files(FILES Information.msg)
 generate_messages(DEPENDENCIES std_msgs)
 
 # 编译文件
 cd ~/ros_ws
 catkin_make
 
 # 编译完成后，查询自定义的消息类型
 rosmsg show learn_topic/Information

C++程序使用自定义话题消息类型

我们写一个发布者和一个订阅者来使用自定义的话题消息，在功能包learn_topic的src目录下新建两个文件，分别命令为Information_publisher.cpp和Information_subscriber.cpp，把以下代码分别复制到里边去，

复制代码

 /**
 * Information_publisher.cpp
 * 该例程将发布/company_info话题，消息类型是自定义的learn_topic::Information
 */
 #include <ros/ros.h>
 #include "learn_topic/Information.h"
 int main(int argc, char **argv)
 {
     // ROS节点初始化
     ros::init(argc, argv, "company_Information_publisher");
     // 创建节点句柄
     ros::NodeHandle n;
     // 创建一个Publisher，发布名为/company_info的topic，消息类型为learn_topic::Person，队列长度10
     ros::Publisher Information_pub = n.advertise<learn_topic::Information>("/company_info", 10);
     // 设置循环的频率
     ros::Rate loop_rate(1);
     int count = 0;
     while (ros::ok())
     {
         // 初始化learn_topic::Information类型的消息
         learn_topic::Information info_msg;
         info_msg.company = "Yahboom";
         info_msg.city = "Shenzhen";
         // 发布消息
         Information_pub.publish(info_msg);
         ROS_INFO("Information: company:%s city:%s ",info_msg.company.c_str(), info_msg.city.c_str());
         loop_rate.sleep();// 按照循环频率延时
     }
     return 0;
 }

复制代码

 /**
 * Information_subscriber.cpp
 * 该例程将订阅/company_info话题，自定义消息类型learn_topic::Information
 */
 #include <ros/ros.h>
 #include "learn_topic/Information.h"
 // 接收到订阅的消息后，会进入消息回调函数处理数据
 void CompanyInfoCallback(const learn_topic::Information::ConstPtr& msg)
 {
     // 打印接收到的消息
     ROS_INFO("This is: %s in %s", msg->company.c_str(), msg->city.c_str());
 }
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "company_Information_subscriber");// 初始化ROS节点
     ros::NodeHandle n;// 这里是创建节点句柄
     // 创建一个Subscriber，订阅名话题/company_info的topic，注册回调函数CompanyInfoCallback
     ros::Subscriber person_info_sub = n.subscribe("/company_info", 10,CompanyInfoCallback);
     ros::spin();// 循环等待回调函数
     return 0;
 }

复制代码

 # 修改CMakeLists.txt文件
 add_executable(Information_publisher src/Information_publisher.cpp)
 target_link_libraries(Information_publisher ${catkin_LIBRARIES})
 add_dependencies(Information_publisher ${PROJECT_NAME}_generate_messages_cpp)
 
 
 add_executable(Information_subscriber src/Information_subscriber.cpp)
 target_link_libraries(Information_subscriber ${catkin_LIBRARIES})
 add_dependencies(Information_subscriber ${PROJECT_NAME}_generate_messages_cpp)

复制代码

 # 编译功能包
 cd ~/ros_ws
 catkin_make
 
 #编译通过后，重新打开两个终端，分别运行Information_publisher 和 Information_subscriber ，先启动roscore
 roscore
 
 # 另开一个终端
 rosrun learn_topic Information_publisher
 
 # 另开一个终端
 rosrun learn_topic Information_subscriber

自定义服务消息与使用

创建消息

复制代码

 # 在learn_service功能包的目录下，新建一个文件夹srv
 cd ~/ros_ws/src/learn_service
 mkdir srv
 
 # 切换至srv目录下，新建一个空白的srv文件，以srv为后缀表示代表是srv文件。这里我们以IntPlus.srv为例子说明下
 # 这里说明下srv文件的构成，由符号---分为上下两个部分，上边表示request下边是response
 uint8 a
 uint8 b
 ---
 uint8 result

复制代码

 # 在package.xml中添加功能包依赖
 <build_depend>message_generation</build_depend>
 <exec_depend>message_runtime</exec_depend>

复制代码

 # 在CMakeList.txt添加编译选项
 #在find_package里边加上message_generation
 add_service_files(FILES IntPlus.srv)
 generate_messages(DEPENDENCIES std_msgs)

复制代码

 cd  ~/ros_ws
 catkin_make
 
 # 编译成功后，终端输入以下指令查询是否生成了自定义的服务消息
 rossrv show learn_service/IntPlus

C++程序使用自定义话题消息类型

在功能包learn_service的src目录下新建两个文件，分别命令为IntPlus_client.cpp和IntPlus_server.cpp

复制代码

 /*
 * IntPlus_client.cpp
 * 该例程将请求/Two_Int_Plus服务，服务数据类型learn_service::IntPlus, 两个整型数相加求和
 */
 #include <ros/ros.h>
 #include "learn_service/IntPlus.h"
 #include <iostream>
 using namespace std;
 int main(int argc, char** argv)
 {
     int i,k;
     cin>>i;
     cin>>k;
     ros::init(argc, argv, "IntPlus_client");// 初始化ROS节点
     ros::NodeHandle node;// 创建节点句柄
     // 发现/Two_Int_Plus服务后，创建一个服务客户端
     ros::service::waitForService("/Two_Int_Plus");
     ros::ServiceClient IntPlus_client = node.serviceClient<learn_service::IntPlus>("/Two_Int_Plus");
     // 初始化learn_service::IntPlus的请求数据
     learn_service::IntPlus srv;
     srv.request.a = i;
     srv.request.b = k;
     ROS_INFO("Call service to plus %d and %d", srv.request.a, srv.request.b);//请求服务调用
     IntPlus_client.call(srv);
     // 显示服务调用结果
     ROS_INFO("Show the result : %d", srv.response.result);// 显示服务调用结果
     return 0;
 }

复制代码

 /**
 * IntPlus_server.cpp
 * 该例程将执行/Two_Int_Plus服务，服务数据类型learn_service::IntPlus
 */
 #include <ros/ros.h>
 #include "learn_service/IntPlus.h"
 // service回调函数，输入参数req，输出参数res
 bool IntPlusCallback(learn_service::IntPlus::Request &req,learn_service::IntPlus::Response &res)
 {
     ROS_INFO("number 1 is:%d ,number 2 is:%d ", req.a, req.b);// 显示请求数据
     res.result = req.a + req.b ;// 反馈结果为两数之和
     return res.result;
 }
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "IntPlus_server"); // ROS节点初始化
     ros::NodeHandle n;// 创建节点句柄
     // 创建一个server，注册回调函数IntPlusCallback
     ros::ServiceServer Int_Plus_service = n.advertiseService("/Two_Int_Plus",IntPlusCallback);
     // 循环等待回调函数
     ROS_INFO("Ready to caculate.");
     ros::spin();
     return 0;
 }
 ```

复制代码

 # 修改CMakeLists.txt文件
 add_executable(IntPlus_server src/IntPlus_server.cpp)
 target_link_libraries(IntPlus_server ${catkin_LIBRARIES})
 add_dependencies(IntPlus_server ${PROJECT_NAME}_generate_messages_cpp)
 
 
 add_executable(IntPlus_client src/IntPlus_client.cpp)
 target_link_libraries(IntPlus_client ${catkin_LIBRARIES})
 add_dependencies(IntPlus_client ${PROJECT_NAME}_generate_messages_cpp)

复制代码

 cd ~/ros_ws
 catkin_make
 
 roscore
 
 # 新建终端
 rosrun learn_service IntPlus_server
 
 # 新建终端
 rosrun learn_service IntPlus_client
 # client终端输入两个数字，空格间隔，回车完成