ROS机器人操作系统：从入门到放弃（一）节点的创建，接受与发布消息

常见的发布器与接收器

1.创建工作空间

$ mkdir -p catkin_ws/src/examples
$ cd catkin_ws/src/examples
$ catkin_create_pkg example1 std_msgs rospy roscpp
$ cd ../..
$ catkin_make

2.编写发布器

$ cd ~/catkin_ws/src/examples/example1/src
$ touch talker.cpp

进入talker.cpp，输入以下内容，保存并退出

//talker.cpp
#include<ros/ros.h> 
#include<std_msgs/String.h> //这个程序是用来发布一个String消息的，ROS针对每一个类型消息都有相应的头文件，如果要发布那个类型的消息就必须包含相应的头文件
#include<sstream>

int main(int argc, char ** argv)
{

    int count = 0;
    ros::init(argc, argv, "talker"); //初始化节点，节点名为talker，节点名必须是独一无二的
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<std_msgs::String>("message", 1000); 
    /*定义要publish的消息类型和topic的名字，类型就是std_msgs::String，topic的名字就是message。1000这个数字的意思是要缓冲的信息数量，如果信息产生的太快，接受方反应不过来，
    ROS会把发布的信息都写进缓冲区，先存1000条，然后接收的程序慢慢从缓冲区里读，只要信息不超过1000条，是可以慢慢读完的。如果超过1000条,最早的信息就直接丢掉了，第一条来的信息在序列满了的情况下会被第一个丢弃。*/
    ros::Rate loop_rate(10); //10Hz，一秒内发布10次，这个函数要和loop_rate.sleep()配合使用才能达到控制速度目的。

    while(ros::ok())//while(ros::ok()) 要是ros不OK，程序就退出了。什么时候ROS不OK呢？最常见的就是你按下ctrl+c或者在程序遇到ros::shutdown()这行命令
    {
        std_msgs::String msg; //定义了std_msgs::String的对象msg
        std::stringstream ss;
        ss << "Hello, I'm talker " << count;
        /*sstream是c++自带的头文件，可以实现利用输入输出流的方式往string里写东西，并且还可以拼接string和其他类型的变量．ss是stringstream的对象,代码中的
        std::stringstream ss;
        ss << "Hello, I'm talker " << count;
        实现了string hello world和int变量 count的拼接，形成一个新的string．即如果count是１，那么helloworld1会作为string被存放在ss当中．怎么调用这个string呢? ss.str()就可以了．*/
        msg.data = ss.str();//msg包含一个成员data，这个成员在std_msgs::String这个类中应该是被这样定义了的std::string data
        pub.publish(msg); //发布消息
        count ++;
        //ros::spinOnce();  //接受器里必须有它，它在这里没用
        loop_rate.sleep();  //保证发布的频率是10Hz
    }
    return 0;
}

3.编写接收器

$ touch listener.cpp

输入以下内容，保存并退出

//listener.cpp
#include<ros/ros.h>
#include<std_msgs/String.h>

void Callback(const std_msgs::String::ConstPtr& msg)//这个模式还是比较固定的,如果要接受的是Int8类型的消息，那么参数会变成(const std_msgs::Int8::ConstPtr& msg)。
{                                                   //ConstPtr代表一个指针。所以msg这个参数是接收到的消息的指针
    ROS_INFO("I heard: %s", msg->data.c_str());     //在Publisher中我们定义了std_msgs::String对象msg，类包含数据成员data，调用方式为msg.data。如果类的指针叫msg,那么调用该成员的方式是msg->data
}                                                   //msg->data就是一个std::string类型的量，假设有string类型的变量st要想print出来,代码就是printf("%s,", st.c_str() )（不能直接print st）

int main(int argc, char ** argv)
{
    ros::init(argc, argv, "listener"); //初始化节点，节点名称为listener
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("message", 1000, Callback); //Callback 为上面定义的回调函数，此处把接收到的消息传递给回调函数
    ros::spin(); //这个函数是用于接收器的，必须要有spinOnce()或者spin()，ROS才会检测是不是接收到信息,spinOnce就是检测一次，spin()就是一直检测
    return 0;
}

4.编写CMakeLists.txt文件

打开CMakeLists.txt文件输入以下内容，保存并退出

cmake_minimum_required(VERSION 2.8.3)
project(example1)

find_package(catkin REQUIRED COMPONENTS roscpp)

include_directories(${catkin_INCLUDE_DIRS})

catkin_package(
  INCLUDE_DIRS
  CATKIN_DEPENDS roscpp
  DEPENDS
)

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

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

5.编写package.xml

打开package.xml文件输入以下内容，保存并退出（在创建代码包的时候已经创建好了package.xml，实际上不用修改了）

<package>
  <name>example1</name>
  <version>2.4.2</version>
  <description>The example1 package</description>
  <maintainer email="[email protected]">biyueming</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>
  <run_depend>roscpp</run_depend>
  <run_depend>rospy</run_depend>
  <run_depend>std_msgs</run_depend>
</package>

6.编译并运行程序

先给两个cpp文件权限

$ cd src
$ chmod +x talker.cpp listener.cpp

编译工作空间，生成工程文件

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash

文件目录结构

运行代码

$ roscore
$ rosrun example1 talker
$ rosrun example2 listener

就可以看到listener接收到的消息

7.其它的消息类型

Int8和Float64类型

//talker.cpp
#include<ros/ros.h>
#include<std_msgs/Int8.h> //String --> Int8
//#include //String --> Float64 这里的Float64指的就是double，但是不写成double

int main(int argc, char ** argv)
{
    int count = 0;
    //float count = 3.14 //Float64
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<std_msgs::Int8>("message", 1000); //String --> Int8
    //ros::Publisher pub = n.advertise("message", 1000);
    ros::Rate loop_rate(10);

    while(ros::ok())
    {
        std_msgs::Int8 msg; //Sting --> Int8
        //std_msgs::Float64 msg;
        msg.data = count;   //直接赋值
        ROS_INFO("Hello, I'm talker: %d", msg.data);
        //ROS_INFO("Hello, I'm talker: %f", msg.data)
        pub.publish(msg);
        count ++;
        loop_rate.sleep();
    }
    return 0;
}

//listener.cpp
#include<ros/ros.h>
#include<std_msgs/Int8.h> //String --> Int8
//#include

void Callback(const std_msgs::Int8::ConstPtr& msg) //String --> Int8
{
    ROS_INFO("I heard: %d", msg->data); //msg->data.c_str() --> msg->data
    //ROS_INFO("I heard: %f", msg->data)
}

int main(int argc, char ** argv)
{
    ros::init(argc, argv, "listener");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("message", 1000, Callback);
    ros::spin();
    return 0;
}

8.NodeHandle常用成员函数

NodeHandle是Node的句柄， 用来对当前节点进行各种操作。 在ROS中， NodeHandle是一个定义好的类， 通过 include ， 我们可以创建这个类， 以及使用它的成员函数。
NodeHandle常用成员函数包括：

//创建话题的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 &, Mres &));
//第一个参数是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

独立创建msg的发布器与接收器

通信方式

自定义一个类型为gps的消息（ 包括位置x， y和工作状态state信息） ， 一个node以一定频率发布模拟的gps消息， 另一个node接收并处理， 算出到原点的距离。 源代码见 ROS-Academy-for-Beginners/topic_demo

创建gps消息

在代码中， 我们会用到自定义类型的gps消息， 因此就需要来自定义gps消息， 在msg路径下创建 gps.msg ： 见 topic_demo/msg/gps.msg

string state #工作状态
float32 x #x坐标
float32 y #y坐标

以上就定义了一个gps类型的消息， 你可以把它理解成一个C语言中的结构体， 类似

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

在程序中对一个gps消息进行创建修改的方法和对结构体的操作一样。
当创建完了msg文件， 记得修改 CMakeLists.txt 和 package.xml ， 从而让系统能够编译自定义消息。
在 CMakeLists.txt 中需要改动：

find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
message_generation #需要添加的地方
) 
add_message_files(FILES gps.msg)
#catkin在cmake之上新增的命令， 指定从哪个消息文件生成
generate_messages(DEPENDENCIES std_msgs)
#catkin新增的命令， 用于生成消息
#DEPENDENCIES后面指定生成msg需要依赖其他什么消息， 由于gps.msg用到了flaot32这种ROS标准消息， 因此需要再把std_msgs作为依赖

catkin_package(…… message_runtime)

package.xml 中需要的改动：

message_generation
message_runtime

当完成了以上所有工作， 就可以回到工作空间， 然后编译了。 编译完成之后会在 devel 路径下生成 gps.msg 对应的头文件， 头文件按照C++的语法规则定义了 topic_demo::gps 类型的数据。
要在代码中使用自定义消息类型， 只要 #include ， 然后声明， 按照对结构体操作的方式修改内容即可：

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

消息发布节点和消息接收节点

见 topic_demo/src/talker.cpp和topic_demo/src/listener.cpp。
在topic接收方，有一个比较重要的概念，就是回调(CallBack)，在本例中，回调就是预先给 gps_info 话题传来的消息准备一个回调函数，你事先定义好回调函数的操作，本例中是计算到原点的距离。只有当有消息来时，回调函数才会被触发执行。具体去触发的命令就是 ros::spin() ，它会反复的查看有没有消息来，如果有就会让回调函数去处理。
因此千万不要认为，只要指定了回调函数，系统就回去自动触发，必须 ros::spin() 或者 ros::spinOnce() 才能真正使回调函数生效。

回调函数与spin()方法

回调函数作为参数被传入到了另一个函数中（ 在本例中传递的是函数指针） ， 在未来某个时刻（ 当有新的message到达） ， 就会立即执行。 Subscriber接收到消息， 实际上是先把消息放到一个队列中去， 如图所示。 队列的长度在Subscriber构建的时候设置好了。 当有spin函数执行， 就会去处理消息队列中队首的消息。

spin具体处理的方法又可分为阻塞/非阻塞,单线程/多线程， 在ROS函数接口层面我们有4种spin的方式：

单线程与多线程的区别：


我们常用的 spin() 、 spinOnce() 是单个线程逐个处理回调队列里的数据。 有些场合需要用到多线程分别处理， 则可以用到 MultiThreadedSpin() 、 AsyncMultiThreadedSpin() 。
ros::spin()和ros::spinOnce()，两者区别在于前者调用后不会再返回，而后者在调用后还可以继续执行之后的程序。ros::spin()为阻塞，当调用ros::spin()时，程序停在此处，反复调用当前可触发的回调函数，如果队列里有可以处理的消息，则清空队列，把队列里面的每一个msg进行处理。ros::spinOnce()为非阻塞，只调用一次回调函数，如果没有可执行的回调函数则执行下面的语句。