ROS学习笔记(七)

roscpp介绍（一）

1.Client Library 简介

ROS提供了不同语言的编程接口，C++接口为roscpp，Python接口为rospy，Java接口为rosjava。这些接口都可以用来创建topic、service、param，实现通信功能，把一些常用的基本功能做了封装。

Client Library	介绍
roscpp	ROS的C++库，是目前最广泛应用的ROS客户端库，执行效率高
rospy	ROS的Python库，开发效率高，通常用在对运行时间没有太大要求的场合，例如配置、初始化等操作
roslisp	ROS的LISP库
roscs	Mono/.NET.库，可用任何Mono/.NET语言，包括C#，Iron Python， Iron Ruby等
rosgo	ROS Go语言库
rosjava	ROS Java语言库
rosnodejs	Javascript客户端库
…	…

最常用的是roscpp和rospy。
ROS框架：

2.roscpp

roscpp位于/opt/ros/kinetic下，用C+ +实现ROS通信。C++代码通过catkin编译构建，roscpp可以理解为一个C++库，创建一个CMake工程，在其中include roscpp等ROS的libraries，在工程中即可使用ROS提供的函数。

#include	//调用C++接口

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

以上功能可以分为以下几类：

Initialization and Shutdown 初始与关闭
Topics 话题
Services 服务
Parameter Server 参数服务器
Timers 定时器
NodeHandles 节点句柄
Callbacks and Spinning 回调和自旋
Logging 日志
Names and Node Information 名称管理
Time 时钟
Exception 异常

基本操作：

初始化节点：

1. 调用ros::init()函数，初始化节点的名称和其他信息，一般ROS程序以此开始。
2. 创建ros::NodeHandle对象（节点的句柄），它可以创建Publisher、Subscriber等。（NodeHandle是对节点资源的描述，可以用它操作节点，比如监听某个topic上的消息，为程序提供服务，访问修改param等）。

关闭节点：
在终端直接按Ctrl+C即可关闭一个节点，系统会自动触发SIGINT句柄来关闭进程。也可以通过调用ros::shotdown()来收订关闭节点（一般很少这样做）。
例：

#include 
int main(int argc,char **argv)
{
	ros::init(argc,argv,"your_node_name");
	ros::NodeHandle nh;
	 //....节点功能
    //....
    ros::spin();//用于触发topic、service的响应队列,阻塞式
    return 0;
}

启动节点，获取句柄，系统自动完成关闭工作（也可以自定义）。
在ROS中，NodeHandle是一个定义好的类，通过include创建和使用它的成员函数。

//创建话题的publisher 
ros::Publisher advertise(const string &topic,uint32_t queue_size,bool latch=false);
//第一个参数为发布话题的名称
//第二个是消息队列的最大长度，如果发布的消息超过这个长度而没有被接收，那么就的消息就会出队。通常设为一个较小的数即可。
//第三个参数是是否锁存。某些话题并不是会以某个频率发布，比如/map这个topic，只有在初次订阅或者地图更新这两种情况下，/map才会发布消息。这里就用到了锁存。

//创建话题的subscriber
ros::Subscriber subscriber(const string &topic,, uint32_t queue_size, void(*)(M));
//第一个参数是订阅话题的名称
//第二个参数是订阅队列的长度，如果受到的消息都没来得及处理，那么新消息入队，就消息就会出队
//第三个参数是回调函数指针，指向回调函数来处理接收到的消息

//创建服务的client
ros::ServiceClient ServiceClient(const string &service_name, bool persistent=false);
//第一个函数式service名称
//第二个参数用于设置服务的连接是否持续，如果为true，client将会保持与远程主机的连接，这样后续的请求会快一些。通常我们设为flase

//创建服务的server，提供服务
ros::ServiceServer advertiseService(const string &service, bool(*srv_func)(Mreq &, Mres &));
//第一个参数是service名称
//第二个参数是服务函数的指针，指向服务函数。指向的函数应该有两个参数，分别接受请求和响应。

//查询某个参数的值
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);
//从参数服务器上获取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

3.Topic通信

Topic是异步通信的模型，节点间分工明确，有的只负责发送，有的只负责接收处理。对于绝大多数的机器人应用场景，比如传感器数据收发，速度控制指令的收发，Topic模型是最适合的通信方式。
例：
自定义一个类型为gps的消息（包括位置x，y和工作状态state信息），一个node以一定频率发布模拟的gps消息，另一个node接收并处理，算出到原点的距离。

1. 创建gps消息

package/msg/gps.msg

string state
flost32 x
flost32 y

类似于C语言中的结构体

struct gps
{
	string state;
	float32 x;
	float32 y;
}

在程序中对一个gps消息进行创建修改的方法和对结构体操作相同。

2. 修改CMakeLists.txt和package.xml

CMakeLists.txt:

...
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
message_generation	#此处为需要添加的地方
)
...
add_messages_files(FILES gps.msg)//指定从哪个消息文件生成

generate_messages(DEPENDENCIES std_msgs)//用于生成消息
//DEPENDENCIES后面指定生成msg需要依赖其他什么消息，由于gps.msg用到了flaot32这种ROS标准消息，因此需要再把std_msgs作为依赖
...

package.xml

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

回到工作空间进行编译，编译完成之后会在devel路径下生成gps.msg对应的头文件，头文件按照C++的语法规则定义了test::gps类型的数据。

要在代码中使用自定义消息类型，只要#include ，然后声明，按照对结构体操作的方式修改内容即可。

topic_demo::gps mygpsmsg;
mygpsmsg.x = 1.6;
mygpsmsg.y = 5.5;
mygpsmsg.state = "working";

3. 消息发布节点

文件位置：test/src/topic_talker.cpp

#include
#include

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "topic_talker");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<test::gps>("topic_gps", 1);
    ros::Rate loop_rate(1.0);

    test::gps mygps;
    mygps.state = "working";
    mygps.x = 1.0;
    mygps.y = 2.0;

    while (ros::ok)
    {
        pub.publish(mygps);
        loop_rate.sleep();
    }
       
    return 0;
}

4. 消息接收节点

#include
#include
#include

void gpsCallback(test::gps::ConstPtr &msg)
{
    std_msgs::Float32 distance;
    distance.data = sqrt(pow(msg->x,2)+pow(msg->y,2));
    ROS_INFO("listener : distance to origin = %f,state = %s",distance.data,msg->state.c_str());
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "topic_listener");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("topic_gps", 1, gpsCallback);
    ros::spin();
    return 0;
}

回调(CallBack)：预先给话题传来的消息准备一个回调函数，定义好对消息的操作（本例中是计算到原点的距离）。只有当有消息来时，回调函数才会被触发执行，类似单片机的中断服务程序。具体去触发的命令就是ros::spin()，它会反复的查看有没有消息来，如果有就会让回调函数去处理。
消息来时不是系统自动触发，必须由ros::spin()或者ros::spinOnce()触发，使回调函数生效。

5. 继续修改CMakeLists.txt

add_executable(topic_talker src/topic_talker.cpp) //生成可执行文件topic_talker
add_dependencies(topic_talker test_generate_messages_cpp)
//表明在编译topic_talker前，必须先生编译完成自定义消息
//必须添加add_dependencies，否则找不到自定义的msg产生的头文件
target_link_libraries(talker ${catkin_LIBRARIES}) //链接

add_executable(topic_listener src/topic_listener.cpp ) //声称可执行文件topic_listener
add_dependencies(topic_listener test_generate_messages_cpp)
target_link_libraries(topic_listener ${catkin_LIBRARIES})//链接

之后经过catkin_make，一个自定义消息+发布接收的基本模型就完成了。

cd ~/catkin_ws
source devel/setup.bash
catkin_make
roscore
rosrun topic_talker
rosrun topic_listener

回调函数与spin()方法

回调函数作为参数被传入到了另一个函数中（在本例中传递的是函数指针）当有新的message到达，就会立即执行。Subscriber接收到消息，实际上是先把消息放到一个队列中去，当有spin函数执行，就会去处理消息队列中队首的消息。

spin方法	阻塞	线程
ros::spin()	阻塞	单线程
ros::spinOnce()	非阻塞	单线程
ros::MultiThreadedSpin()	阻塞	多线程
ros::AsyncMultiThreadedSpin()	非阻塞	多线程

常用的spin()、spinOnce()是单个线程逐个处理回调队列里的数据。
需要用到多线程分别处理，则可以用到MultiThreadedSpin()、AsyncMultiThreadedSpin()。