ROS小实操

---

前言

提示：个人笔记，无参考价值

---

---

实操一_话题发布

需求:
编码实现乌龟运动控制，让小乌龟做圆周运动。

实现分析:

1. 乌龟运动控制实现，关键节点有两个，一个是乌龟运动显示节点
   turtlesim_node，另一个是控制节点，二者是订阅发布模式实现通信的，乌龟运动显示节点直接调用即可，运动控制节点是turtle_teleop_key，通过键盘控制，现在需要自定义控制节点。
2. 控制节点自实现时，首先需要了解控制节点与显示节点通信使用的话题与消息，可以使用ros命令结合计算图来获取。
3. 了解了话题与消息之后，通过C++ 编写运动控制节点，通过指定的话题，按照一定的逻辑发布消息即可。

1.话题与消息获取

1.1话题获取
运行turtlesim_node和turtle_teleop_key这两个节点，使用指令rqt_graph启动计算图,可以查看到这两个节点的话题为: /turtle1/cmd_vel。或者通过rostopic list指令查看所有话题。

1.2消息获取
获取消息类型
rostopic type /turtle1/cmd_vel 指令可以查看话题的详细信息，详细信息里的type字段的值为消息类型。消息类型为geometry_msgs/Twist
获取消息格式

rosmsg info geometry_msgs/Twist

#结果
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

linear(线速度) 下的xyz分别对应在x、y和z方向上的速度(单位是 m/s)；
angular(角速度)下的xyz分别对应x轴上的翻滚、y轴上俯仰和z轴上偏航的速度(单位是rad/s)。
补充资料2:偏航、翻滚与俯仰
坐标系图解:

2.实现发布节点

创建功能包需要依赖的功能包: roscpp std_msgs geometry_msgs
实现代码如下：

/*
    准备工作:
        1.获取topic(已知: /turtle1/cmd_vel)
        2.获取消息类型(已知: geometry_msgs/Twist)
        3.运行前，注意先启动 turtlesim_node 节点
    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建发布者对象
        4.循环发布运动控制消息
*/

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"control");
    ros::NodeHandle nh;
    // 3.创建发布者对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1000);
    // 4.循环发布运动控制消息
    //4-1.组织消息
    geometry_msgs::Twist msg;
    msg.linear.x = 1.0;
    msg.linear.y = 0.0;
    msg.linear.z = 0.0;

    msg.angular.x = 0.0;
    msg.angular.y = 0.0;
    msg.angular.z = 2.0;

    //4-2.设置发送频率
    ros::Rate r(10);
    //4-3.循环发送
    while (ros::ok())
    {
        pub.publish(msg);
        ros::spinOnce();
    }

    return 0;
}

3.配置文件并运行

首先，启动 roscore；
然后启动乌龟显示节点；
最后执行运动控制节点。

---

---

实操二_话题订阅

需求描述:
已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。

实现分析:

1. 首先，需要启动乌龟显示节点以及运动控制节点并控制乌龟运动。
2. 通过ROS命令，来获取乌龟位姿发布的话题以及消息。
3. 编写订阅节点，订阅并打印乌龟的位姿。

1.话题与消息获取

获取话题
rostopic list指令查看所有话题，查看到话题名称为: /turtle1/pose
获取消息类型
rostopic type /turtle1/pose指令查看该话题的详细信息，信息里的type字段为消息类型，消息类型为:turtlesim/Pose
获取消息格式

rosmsg info turtlesim/Pose
//结果
​float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

2.实现订阅节点

创建功能包需要依赖的功能包: roscpp std_msgs turtlesim

/*  
    订阅小乌龟的位姿: 时时获取小乌龟在窗体中的坐标并打印
    准备工作:
        1.获取话题名称 /turtle1/pose
        2.获取消息类型 turtlesim/Pose
        3.运行前启动 turtlesim_node 与 turtle_teleop_key 节点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅者对象
        5.回调函数处理订阅的数据
        6.spin
*/

#include "ros/ros.h"
#include "turtlesim/Pose.h"

void doPose(const turtlesim::Pose::ConstPtr& p){
    ROS_INFO("乌龟位姿信息:x=%.2f,y=%.2f,theta=%.2f,lv=%.2f,av=%.2f",
        p->x,p->y,p->theta,p->linear_velocity,p->angular_velocity
    );
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"sub_pose");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    // 5.回调函数处理订阅的数据
    // 6.spin
    ros::spin();
    return 0;
}

3.运行

首先，启动 roscore；
然后启动乌龟显示节点，执行运动控制节点；
最后启动乌龟位姿订阅节点。

---

---

实操三_服务调用

需求描述:
编码实现向 turtlesim 发送请求，在乌龟显示节点的窗体指定位置生成一乌龟，这是一个服务请求操作。

实现分析:

1. 首先，需要启动乌龟显示节点。
2. 通过ROS命令，来获取乌龟生成服务的服务名称以及服务消息类型
3. 编写服务请求节点，生成新的乌龟。

1.服务名称与服务消息获取

获取话题:/spawn

rosservice list

获取消息类型:turtlesim/Spawn

rosservice type /spawn

获取消息格式:

rossrv info turtlesim/Spawn

//响应结果:
float32 x
float32 y
float32 theta
---
string name

2.服务客户端实现

创建功能包需要依赖的功能包: roscpp std_msgs turtlesim

/*
    生成一只小乌龟
    准备工作:
        1.服务话题 /spawn
        2.服务消息类型 turtlesim/Spawn
        3.运行前先启动 turtlesim_node 节点

    实现流程:
        1.包含头文件
          需要包含 turtlesim 包下资源，注意在 package.xml 配置
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 service 客户端
        5.等待服务启动
        6.发送请求
        7.处理响应

*/

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"set_turtle");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 service 客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    // 5.等待服务启动
    // client.waitForExistence();
    ros::service::waitForService("/spawn");
    // 6.发送请求
    turtlesim::Spawn spawn;
    spawn.request.x = 1.0;
    spawn.request.y = 1.0;
    spawn.request.theta = 1.57;
    spawn.request.name = "my_turtle";
    bool flag = client.call(spawn);
    // 7.处理响应结果
    if (flag)
    {
        ROS_INFO("新的乌龟生成,名字:%s",spawn.response.name.c_str());
    } else {
        ROS_INFO("乌龟生成失败！！！");
    }

    return 0;
}

3.运行

首先，启动 roscore；
然后启动乌龟显示节点；
最后启动乌龟生成请求节点。

---

---

实操四_参数设置

需求描述:
修改turtlesim乌龟显示节点窗体的背景色，已知背景色是通过参数服务器的方式以 rgb 方式设置的。

实现分析

1. 首先，需要启动乌龟显示节点。
2. 通过ROS命令，来获取参数服务器中设置背景色的参数。
3. 编写参数设置节点，修改参数服务器中的参数值。

1.参数名获取

获取参数列表:

rosparam list

响应结果:

/turtlesim/background_b
/turtlesim/background_g
/turtlesim/background_r

2.参数修改

/*
    注意命名空间的使用。

*/
#include "ros/ros.h"


int main(int argc, char *argv[])
{
    ros::init(argc,argv,"haha");

    //ros::NodeHandle nh;

    // ros::param::set("/turtlesim/background_r",0);
    // ros::param::set("/turtlesim/background_g",0);
    // ros::param::set("/turtlesim/background_b",0);

 	ros::NodeHandle nh("turtlesim");
    nh.setParam("background_r",0);
    nh.setParam("background_g",0);
    nh.setParam("background_b",0);


    return 0;
}

3.运行

首先，启动 roscore；
然后启动背景色设置节点；
最后启动乌龟显示节点；

PS: 注意节点启动顺序，如果先启动乌龟显示节点，后启动背景色设置节点，那么颜色设置不会生效。

4.其他设置方式

方式1:修改小乌龟节点的背景色(命令行实现)

rosparam set /turtlesim/background_b 自定义数值

rosparam set /turtlesim/background_g 自定义数值

rosparam set /turtlesim/background_r 自定义数值

方式2:启动节点时，直接设置参数

rosrun turtlesim turtlesim_node _background_r=100 _background_g=0 _background_b=0