《ROS理论与实践》学习笔记（二）ROS通信编程

在学习《ROS理论与实践》课程时，记录了学习过程中的编程练习，课后作业以及发现的问题，后续会对尚未解决的问题继续分析并更新，纯小白，仅供参考。
本次学习笔记关于课程中的第三讲：ROS通信编程 。主要学习了ROS的功能包创建、Topic通信编程以及Service通信编程。

---

课程内容编程练习

1.创建工作空间及功能包

创建一个工作空间命名为catkin_ws，创建一个功能包命名为learning_communication，并进行编译

#创建工作空间
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace

#编译工作空间
$ cd ~/catkin_ws
$ catkin_make

#设置环境变量
$ source devel/setup.bash

# 检查环境变量
$ echo $ROS_PACKAGE_PATH

# 创建功能包
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_communication rospy roscpp std_msgs std_srvs

# 编译功能包
$ cd ~/catkin_ws
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash

2.编程实现Topic通信

发布者代码实现流程：

1. 初始化ROS节点；
2. 向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型；
3. 创建消息数据；
4. 按照一定频率循环发布消息。

订阅者代码实现流程：

1. 初始化ROS节点；
2. 订阅需要的话题；
3. 循环等待话题消息，接收到消息后进入回调函数；
4. 在回调函数中完成消息处理。

由于使用C++编程实现，需要在CMakeLists.txt中设置需要编译的代码和可执行文件，并设置链接库，自定义话题消息通过.msg文件建立。
练习代码来自深蓝学院课程，不再贴出。

3.编程实现Service通信

服务器代码实现流程：

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

客户端代码实现流程：

1. 初始化ROS节点；
2. 创建一个Client实例；
3. 发布服务请求数据；
4. 等待Server处理之后的应答结果。

同样由于使用C++编程实现，需要在CMakeLists.txt中设置需要编译的代码和可执行文件，并设置链接库，自定义请求/应答数据通过.srv文件建立。
练习代码来自深蓝学院课程，不再贴出。

---

本讲作业

1.创建一个节点，在其中实现一个订阅者和一个发布者

• 发布者：发布海龟速度指令，让海龟圆周运动
• 订阅者：订阅海龟的位置信息，并在终端中周期打印输出

发布者代码如下：

/*该例程将发布turtle1/cmdvel话题，消息类型geometry_msgs::Twist*/
 
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>

int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "circle_publisher");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个Publisher，发布名为turtle1/cmd_vel的topic，消息类型为geometry_msgs/Twist
    ros::Publisher circle_pub = n.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
    // 设置循环的频率
    ros::Rate loop_rate(10);
    int count = 0;
    while (ros::ok())
    {
        // 初始化geometry_msgs::Twist类型的消息
        geometry_msgs::Twist speed_msg;
	    speed_msg.linear.x = 0.5;
	    speed_msg.angular.z = 0.2;
        // 发布消息
        ROS_INFO("setspeed [v:%0.2f ,a:%0.2f ]", speed_msg.linear.x,speed_msg.angular.z);
        circle_pub.publish(speed_msg);
        // 按照循环频率延时
        loop_rate.sleep();
    }
    return 0;
}

订阅者代码如下：

/*该例程将订阅turtle1/pose话题，消息类型String*/
 
#include "ros/ros.h"
#include "turtlesim/Pose.h"

// 接收到订阅的消息后，会进入消息回调函数
void chatterCallback(const turtlesim::Pose::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("turtle1's position: [%0.3f, %0.3f]", msg->x, msg->y);
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pose_subscriber");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个Subscriber，订阅名为turtle1/pose的topic，注册回调函数chatterCallback
    ros::Subscriber sub = n.subscribe("/turtle1/pose", 10, chatterCallback);
    // 循环等待回调函数
    ros::spin();
    return 0;
}

2.创建另外一个节点，在其中实现一个客户端

• 客户端：请求海龟诞生的服务，在仿真器中产生一只新的海龟

代码如下：

/*该例程将请求spawn服务，turtlesim::Spawn*/
 
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
#include <time.h>

int main(int argc, char **argv)
{
    char* name;
    srand(time(NULL));  //随机种子，用于生成随机位置
    name = argv[1];
    // ROS节点初始化
    ros::init(argc, argv, "create_client");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个client，service消息类型是spawn
    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("/spawn");
    // 创建turtlesim::Spawn类型的service消息
    turtlesim::Spawn srv;
    srv.request.x = rand() % 10;
    srv.request.y = rand() % 10;
    srv.request.name = name;
    // 发布service请求，等待应答结果
    ROS_INFO("call the service spawn at position [x:%0.2f, y:%0.2f] name:%s",srv.request.x,srv.request.y,srv.request.name.c_str());
    add_turtle.call(srv);
    // 显示服务调用结果
    ROS_INFO("spawn successfully! [name:%s]", srv.response.name.c_str());
    return 0;
}

调用方法：

$ roscore

$ rosrun turtlesim turtlesim_node

$ rosrun ch3 create_client turtle_name

3.综合运用话题与服务编程、命令行使用

• 通过命令行发送新生海龟的名字，即可在界面产生一只海龟，并且位置不重叠
• 通过命令行发送指令控制界面中任意海龟圆周运动的启动/停止，速度可通过命令行控制

生成海龟代码同2，发布速度指令代码如下：

/*该例程将发布turtle1/cmdvel话题，消息类型geometry_msgs::Twist */
 
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include <iostream>
using namespace std;
#include <string>
#include <sstream>
#include <stdlib.h>

int main(int argc, char **argv)
{
    char* turname;//所要控制的海龟名字
    char* speedxx;//设定线速度
    char* speedzz;//设定角速度
    turname = argv[1];
    speedxx = argv[2];
    speedzz = argv[3];
    // ROS节点初始化
    ros::init(argc, argv, "turtle_control");
    // 创建节点句柄
    ros::NodeHandle n;
    // 选择海龟对应的话题
    std::stringstream ss;
    ss <<'/'<<turname<<"/cmd_vel";
    // 创建一个Publisher，发布名为turtle1/cmd_vel的topic，消息类型为geometry_msgs/Twist
    ros::Publisher circle_pub = n.advertise<geometry_msgs::Twist>(ss.str(), 1000);
    // 设置循环的频率
    ros::Rate loop_rate(10);
    //将速度的字符指针转换为double型
    double speedx;
    double speedz;
    speedx = atof(speedxx);
    speedz = atof(speedzz);
    while (ros::ok())
    {
        // 初始化geometry_msgs::Twist类型的消息
        geometry_msgs::Twist speed_msg;
	    speed_msg.linear.x = speedx;
	    speed_msg.angular.z = speedz;
        // 发布消息
        ROS_INFO("setspeed [v:%0.2f ,a:%0.2f ]", speed_msg.linear.x,speed_msg.angular.z);
        circle_pub.publish(speed_msg);
        // 按照循环频率延时
        loop_rate.sleep();
    }
    return 0;
}

调用方法：

#启动turtlesim_node节点
$ roscore
$ rosrun turtlesim turtlesim_node

#产生名为turtle2和turtle3的海龟
$ rosrun ch3 create_client turtle2
$ rosrun ch3 create_client turtle3

#向turtle1和turtle2分别发布速度指令
$ rosrun ch3 turtle_control turtle1 0.1 0.5
$ rosrun ch3 turtle_control turtle2 0.2 0.3

运行效果：

结语

本讲完成了功能包创建、Topic通信及Service通信的编程实现，并在作业中进行了综合练习。
在作业中遇到了一些问题：

1. 如何通过命令行直接传入所需参数？
   可以通过argv[]的字符指针直接传入参数，具体可以参考该博文：ROS向节点传递参数的方法总结
2. 如何将传入的字符指针转换为double型变量？
   可以通过atof()函数进行转换。
3. 在向turtle节点发布速度指令时，同一时间只能使一只海龟运动，新发布的速度指令会覆盖前一次发布的速度指令，如何实现两只海龟的同时运动？
   尚未解决。