[人工智能]ros学习记录5 Client Library CPP

5.Client Library CPP

client library:

提供ros编程的库/接口

例如：建立node、发布消息、调用服务等

提供cpp和python两种语言支持，一般使用cpp

• roscpp

• rospy

5.1 roscpp

ros提供的使用cpp和topic,service,param等交互的接口

roscpp位于 /opt/ros/kinetic 之下， 用cpp实现了ROS通信。 在ROS中， cpp的代码是通过catkin这个编译系统（ 扩展的CMake） 来进行编译构建的。 所以简单地理解， 你也可以把roscpp就当作为一个cpp的库， 我们创建一个CMake工程， 在其中include了roscpp等ROS的libraries， 这样就可以在工程中使用ROS提供的函数了。通常我们要调用ROS的cpp接口， 首先就需要 #include <ros/ros.h> 。

roscpp的主要部分包括：

• ros::init() : 解析传入的ROS参数， 创建node第一步需要用到的函数

• ros::NodeHandle : 和topic、 service、 param等交互的公共接口

• ros::master : 包含从master查询信息的函数

• ros::this_node： 包含查询这个进程(node)的函数

• ros::service： 包含查询服务的函数

• ros::param： 包含查询参数服务器的函数， 而不需要用到NodeHandle

• ros::names： 包含处理ROS图资源名称的函数

• 具体可见： http://docs.ros.org/api/roscpp/html/index.html

5.1.1 ros::init()

void ros::init()   //解析ros参数,为本node命名

5.1.2 ros::NodeHandle Class (类)

// 创建话题的publisher
ros::Publisher advertise(const string &topic, uint32_t queue_size, bool latch=false);
//第一个参数为发布话题的名称
//第二个是消息队列的最大长度， 如果发布的消息超过这个长度而没有被接收， 那么就的消息就会出队。 通常设为一
个较小的数即可。
//第三个参数是是否锁存。 某些话题并不是会以某个频率发布， 比如/map这个topic， 只有在初次订阅或者地图更新
这两种情况下， /map才会发布消息。 这里就用到了锁存。
//创建话题的subscriber
ros::Subscriber subscribe(const string &topic, uint32_t queue_size, void(*)(M));
//第一个参数是订阅话题的名称
//第二个参数是订阅队列的长度， 如果受到的消息都没来得及处理， 那么新消息入队， 就消息就会出队
//第三个参数是回调函数指针， 指向回调函数来处理接收到的消息
//创建服务的server， 提供服务
ros::ServiceServer advertiseService(const string &service, bool(*srv_func)(Mreq &, Mre
s &));
//第一个参数是service名称
//第二个参数是服务函数的指针， 指向服务函数。 指向的函数应该有两个参数， 分别接受请求和响应。
//创建服务的client
ros::ServiceClient serviceClient(const string &service_name, bool persistent=false);
//第一个函数式service名称
//第二个参数用于设置服务的连接是否持续， 如果为true， client将会保持与远程主机的连接， 这样后续的请求会
快一些。 通常我们设为flase
//查询某个参数的值
bool getParam(const string &key, std::string &s);
bool getParam (const std::string &key, double &d) const；
bool getParam (const std::string &key, int &i) const；
//从参数服务器上获取key对应的值， 已重载了多个类型
//给参数赋值
void setParam (const std::string &key, const std::string &s) const；
void setParam (const std::string &key, const char *s) const;
void setParam (const std::string &key, int i) const;
//给key对应的val赋值， 重载了多个类型的val

5.1.3 ros::spin() 和 ros::spinOnce() 区别及详解

1 函数意义

首先要知道，这俩兄弟学名叫ROS消息回调处理函数。它俩通常会出现在ROS的主循环中，程序需要不断调用ros::spin() 或 ros::spinOnce()，两者区别在于前者调用后不会再返回，也就是你的主程序到这儿就不往下执行了，而后者在调用后还可以继续执行之后的程序。

其实消息回调处理函数的原理非常简单。我们都知道，ROS存在消息发布订阅机制，什么？不知道？不知道还不快去：http://wiki.ros.org/ROS/Tutorials （ROS官方基础教程) 瞅瞅。

好，我们继续，如果你的程序写了相关的消息订阅函数，那么程序在执行过程中，除了主程序以外，ROS还会自动在后台按照你规定的格式，接受订阅的消息，但是所接到的消息并不是立刻就被处理，而是必须要等到ros::spin()或ros::spinOnce()执行的时候才被调用，这就是消息回调函数的原理

2 区别

就像上面说的，ros::spin() 在调用后不会再返回，也就是你的主程序到这儿就不往下执行了，而 ros::spinOnce() 后者在调用后还可以继续执行之后的程序。

其实看函数名也能理解个差不多，一个是一直调用；另一个是只调用一次，如果还想再调用，就需要加上循环了。

这里一定要记住，ros::spin()函数一般不会出现在循环中，因为程序执行到spin()后就不调用其他语句了，也就是说该循环没有任何意义，还有就是spin()函数后面一定不能有其他语句(return 0 除外)，有也是白搭，不会执行的。ros::spinOnce()的用法相对来说很灵活，但往往需要考虑调用消息的时机，调用频率，以及消息池的大小，这些都要根据现实情况协调好，不然会造成数据丢包或者延迟的错误

3 常见使用方法

这里需要特别强调一下，如果大兄弟你的程序写了相关的消息订阅函数，那千万千万千万不要忘了在相应位置**加上ros::spin()或者ros::spinOnce()**函数，不然你是永远都得不到另一边发出的数据或消息的，博主血的教训，万望紧记

3.1 ros::spin()

ros::spin()函数用起来比较简单，一般都在主程序的最后，加入该语句就可。例子如下：

发送端：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
    ros::Rate loop_rate(10);
 
    int count = 0;
    while (ros::ok())
    {
        std_msgs::String msg;
        std::stringstream ss;
        ss << "hello world " << count;
        msg.data = ss.str();
        ROS_INFO("%s", msg.data.c_str());
 
        /**
         * 向 Topic: chatter 发送消息, 发送频率为10Hz（1秒发10次）；消息池最大容量1000。
         */
        chatter_pub.publish(msg);
 
        loop_rate.sleep();
        ++count;
    }
    return 0;
}

接收端

#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::init(argc, argv, "listener");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
 
    /**
     * ros::spin() 将会进入循环， 一直调用回调函数chatterCallback(),每次调用1000个数据。
     * 当用户输入Ctrl+C或者ROS主进程关闭时退出，
     */
    ros::spin();
    return 0;
}

3.2 ros::spinOnce()

对于ros::spinOnce()的使用，虽说比ros::spin()更自由，可以出现在程序的各个部位，但是需要注意的因素也更多。比如：

1 对于有些传输特别快的消息，尤其需要注意合理控制消息池大小和ros::spinOnce()执行频率; 比如消息送达频率为10Hz, ros::spinOnce()的调用频率为5Hz，那么消息池的大小就一定要大于2，才能保证数据不丢失，无延迟。

/**接收端**/
#include "ros/ros.h"
#include "std_msgs/String.h"
 
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
   /*...TODO...*/ 
}
 
int main(int argc, char **argv)
{
   ros::init(argc, argv, "listener");
   ros::NodeHandle n;
   ros::Subscriber sub = n.subscribe("chatter", 2, chatterCallback);
 
   ros::Rate loop_rate(5);
   while (ros::ok())
   {
       /*...TODO...*/ 

       ros::spinOnce();
       loop_rate.sleep();
   }
   return 0;

2. ros::spinOnce()用法很灵活，也很广泛，具体情况需要具体分析。但是对于用户自定义的周期性的函数，最好和ros::spinOnce并列执行，不太建议放在回调函数中；

/*...TODO...*/
ros::Rate loop_rate(100);
 
while (ros::ok())
{
   /*...TODO...*/
   user_handle_events_timeout(...);

   ros::spinOnce();                 
   loop_rate.sleep();
}

5.2 topic_demo

Topic是ROS里一种异步通信的模型， 一般是节点间分工明确， 有的只负责发送， 有的只负责接收处理。 对于绝大多数的机器人应用场景， 比如传感器数据收发， 速度控制指令的收发，Topic模型是最适合的通信方式。

一个消息收发的例子： 自定义一个类型为gps的消息（ 包括位置x， y和工作状态state信息）, 一个node
以一定频率发布模拟的gps消息， 另一个node接收并处理， 算出到原点的距离。

步骤：

1. 建立package
2. 自定义msg文件
3. talker.cpp
4. listener.cpp
5. CmakeList.txt & package.xml

1.创建package

cd ~/tutorial_ws/src
catkin_creat_pkg topic_demo roscpp rospy std_msgs

2. 新建自定义msg

cd topic_demo/
mkdir msg
cd msg
gedit gps.msg

catkin_make 会把*.msg文件编译成*.h文件,#include一下就可以使用了

#include "topic_demo/gps.h"
topic_demo::gps msg;

3. talk.cpp

• .pro文件的写法,主要是加入ros和msg生成的h文件

# topic_demo.pro
TEMPLATE = app
CONFIG += console cpp11
CONFIG -= app_bundle
CONFIG -= qt

SOURCES += \
    talker.cpp

LIBS += \
    -L/usr/local/lib \
    -L/opt/ros/kinetic/lib \
    -lroscpp -lrospack -lpthread -lrosconsole -lrosconsole_log4cxx\
    -lrosconsole_backend_interface -lxmlrpcpp -lroscpp_serialization -lrostime  -lcpp_common  -lroslib -lroslib


HEADERS += \


INCLUDEPATH +=\
/opt/ros/kinetic/include\
/home/swc/tutorial_ws/devel/include/     # 这个就是ros工作空间的/devel/include/

/*talker.cpp*/
#include <iostream>
#include "ros/ros.h"
#include "topic_demo/gps.h"

using namespace std;

int main(int argc,char** argv)
{
    ros::init(argc,argv,"talker");  //初始化,解析参数,命名节点为"talker"
    ros::NodeHandle nh;             //创建句柄,实例化node
    topic_demo::gps msg;            //创建gps消息
    msg.x = 1.0;                    //msg初始化
    msg.y = 1.0;
    msg.state = "working";
    ros::Publisher pub = nh.advertise<topic_demo::gps>("gps_info",1);//创建publisher
    //ros::Publisher pub = nh.advertise<消息格式>("topic名称",queue_size)
    //queue_size表示缓存队列长度,随时发送接收,设置成1这样比较小的数是可以的
    ros::Rate loop_rate(1.0);//ros::Rate是控制循环的一个类,定义循环发布的频率,这里是1Hz
    while(ros::ok())         //ros::ok() 表示只要ros没有关闭,就一直循环
    {
        //模拟数据的变化
        msg.x = 1.03 * msg.x;//以指数增长,每隔1秒
        msg.y = 1.01 * msg.y;
        //输出当前msg,ROS_INFO和printf(),cout 类似
        ROS_INFO("Talker_____:GPS:x = %f,y = %f",msg.x,msg.y); 
        pub.publish(msg);       //发布消息
        loop_rate.sleep();      //根据定义的发布频率,sleep一秒钟
    }
    return 0;
}

4.listener.cpp

/*listener.cpp*/
#include "ros/ros.h"
#include "topic_demo/gps.h"

//topic_demo::gps::ConstPtr也是一个类
//它是一个常指针,指向topic_demo::gps
//回调函数
void gpsCallback(const topic_demo::gps::ConstPtr& msg)
{
    float distance;
    float x = msg->x;
    float y = msg->y;
    distance = sqrt(pow(x,2)+pow(y,2));
    ROS_INFO("Listener_____distance to ogigin:%f,state:%s",distance,msg->state);
}

int main(int argc, char **argv)
{
    ros::init(argc,argv,"listener");
    ros::NodeHandle nh;
    /*
    创建subscribe
    ros::Subscriber sub = nh.subscribe("监听话题名称",消息队列长度,回调函数(指针);
    "监听话题名称":和publisher设置成一样
    消息队列长度,一般消息来了马上就会被处理掉,所以不要设置的太大,除非想对数据进行缓存,延迟处理
    回调函数:处理从"监听话题"上收到的消息
    */
    ros::Subscriber sub = nh.subscribe("gps_info",1,gpsCallback);
    /*
    实际上,没来一个消息,就把它放在队列里,并不是来一个就会自动处理一个,要调用一个spin()函数
    spin()函数,查看当前队列里面有没有消息,有的话,就调用回调函数进行处理,把队列清空,如果队列是空的,就不会处理
    反复调用当前可触发的回调函数,是一个阻塞的函数,执行到这一句,反复查看有没有可执行的回调函数
    还有一个ros::spinOnce()函数,只查看一次有没有可执行的回调函数,如果没有就往下执行
    */
    ros::spin();
    return 0;
}

5.修改CMakeLists.txt和package.xml

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)   # CMAKE版本
project(topic_demo)		        # 项目名称

find_package(catkin REQUIRED COMPONENTS	# 指定依赖
  roscpp
  rospy
  std_msgs
message_generation
)
				
add_message_files(			# 添加自定义的msg
   FILES
gps.msg
)

generate_messages(DEPENDENCIES std_msgs)# 生成msg对应的h文件

catkin_package(CATKIN_DEPENDS roscpp rospy std_msgs message_runtime)# 用于配置ROS和package配置文件和Cmake文件

include_directories(include ${catkin_INCLUDE_DIRS})    # 指定C/cpp的头文件路径

add_executable(talker src/talker.cpp)			# 生成可执行目标文件
add_dependencies(talker topic_demo_generate_message_cpp)# 添加依赖,必须有这句来生成msg?
target_link_libraries(talker ${catkin_LIBRARIES})		# 链接

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

package.xml 中添加两句

<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>

6.编译运行

catkin_make

rosrun topic_demo talker

rosrun topic_demo listener

也可以在qt里面编译运行listener.cpp,效果一样的

查看话题间关系:rqt_graph

7.先接收消息,处理之后发出去

/*listener.cpp*/
#include "ros/ros.h"
#include "topic_demo/gps.h"

topic_demo::gps gps_data;//转存接收到的数据

void gpsCallback(const topic_demo::gps::ConstPtr& msg)
{
    gps_data = *msg;
    gps_data.state = "received";
    float distance;
    float x = msg->x;
    float y = msg->y;
    distance = sqrt(pow(x,2)+pow(y,2));
    //ROS_INFO("Listener_____distance to ogigin:%f,state:%s",distance,msg->state.c_str());
}

int main(int argc, char **argv)
{
    ros::init(argc,argv,"listener");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<topic_demo::gps>("gps_receive",1);
    ros::Subscriber sub = nh.subscribe("gps_info",1,gpsCallback);
    ros::Rate loop_rate(1.0);
    while(ros::ok())
    {
        ros::spinOnce();//要设置成spinOnce(),不然跑不出来
        pub.publish(gps_data);//也可以在回调函数里面pub
	ROS_INFO("publish  success");
        loop_rate.sleep();
    }
    return 0;
}

/*listener2.cpp*/
#include "ros/ros.h"
#include "topic_demo/gps.h"

void gpsCallback(const topic_demo::gps::ConstPtr& msg)
{
    float distance;
    float x = msg->x;
    float y = msg->y;
    distance = sqrt(pow(x,2)+pow(y,2));
    ROS_INFO("Listener2_____distance to ogigin:%f,state:%s",distance,msg->state.c_str());
}

int main(int argc, char **argv)
{
    ros::init(argc,argv,"listener2");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("gps_receive",1,gpsCallback);
    ros::Rate loop_rate(1.0);
    while(ros::ok())
    {
        ros::spin();
	loop_rate.sleep();
    }
    return 0;
}

8.话题名称相关

以下代码显示了常用的话题的声明，我们通过修改话题名称来理解名称的用法。

int main(int argc, char** argv) 		 // 节点主函数
{
    ros::init( argc, argv, "node1"); 		 // 初始化节点
	{
    ros::NodeHandle nh; 				
    // 声明发布者,话题名 = bar
    // 声明节点句柄
    ros::Publisher node1_pub = nh.advertise<std_msg::Int32>("bar" , 10);
	}
}

? 这里的节点名称是/node1。如果您用一个没有任何字符的相对形式的bar来声明一个发布者,这个话题将和/bar具有相同的名字。

? 如果以如下所示使用斜杠(/)字符用作全局形式,话题名也是/bar。

ros::Publisher node1_pub = nh.advertise<std_msg::Int32>(“/bar”, 10);
但是,如果使用波浪号(~)字符将其声明为私有,则话题名称将变为/node1/bar。

ros::Publisher node1_pub = nh.advertise<std_msg::Int32>(“~bar”, 10);

这可以按照下表所示的各种方式使用。其中/wg意味着命名空间的修改。这在下面的描述中更详细地讨论。

5.3 service_demo

services是另一种ros通信的方式，具体介绍3.2.4 3.2.5

demo功能：

实现两个整数的相加

客户端：发送两个整数

服务端：返回求和

1.自定义srv文件

和topic的msg类似，在功能包中新建一个srv文件夹，里面保存自定义的srv文件

# 客户端发送的
int64 a
int64 b
# 客户端和服务端用---隔开
---
# 服务端返回的
int64 sum

2.生成cpp可以调用的格式

修改Cmakelists.txt package.xml

在package.xml中添加功能包依赖：

<?xml version="1.0"?>
<package format="2">
  <name>service_demo</name>
  <version>0.0.0</version>
  <description>service_demo</description>
  <maintainer email="swc@todo.todo">swc</maintainer>

  <license>TODO</license>
    
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>message_generation</build_depend>

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>  

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

在Cmakelists.txt中添加依赖选项

cmake_minimum_required(VERSION 3.0.2)
project(service_demo)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

################################################
## Declare ROS messages, services and actions ##
################################################

## Generate services in the 'srv' folder
add_service_files(
  FILES
  AddTwoInts.srv
)


## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs
)


###################################
## catkin specific configuration ##
###################################
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES service_demo
 CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
  ${catkin_INCLUDE_DIRS}
)

然后编译，生成h文件

3.客户端程序编写

/**
 * AddTwoInts Client
 */
 
#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_int service，service消息类型是learning_communication::AddTwoInts
  // 发送到的服务器提供的服务名：add_two_ints
  ros::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请求，等待加法运算的应答结果，调用服务，如果call成功，返回true，并且srv.response中的值有效，反之亦然
  if (client.call(srv))
  {
    ROS_INFO("Sum: %ld", (long int)srv.response.sum);
  }
  else
  {
    ROS_ERROR("Failed to call service add_two_ints");
    return 1;
  }

  return 0;
}

4.服务端程序编写

/**
 * AddTwoInts Server
 */
 
#include "ros/ros.h"
#include "learning_communication/AddTwoInts.h"

// service回调函数，输入参数req，输出参数res
// 回调函数的参数好像只能这么写：req；res，尝试只传AddTwoInts报错
bool add(learning_communication::AddTwoInts::Request  &req,
         learning_communication::AddTwoInts::Response &res)
{
  // 将输入参数中的请求数据相加，结果放到应答变量中
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("sending back response: [%ld]", (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，将其广播到ros
  // 注册回调函数add()
  ros::ServiceServer service = n.advertiseService("add_two_ints", add);
  
  // 循环等待回调函数
  ROS_INFO("Ready to add two ints.");
  ros::spin();

  return 0;
}