[ROS]通信机制实操

通过ROS内置的turtlesim案例，结合已经介绍ROS命令获取节点、话题、话题消息、服务、服务消息与参数的信息，最终再以编码的方式实现乌龟运动的控制、乌龟位姿的订阅、乌龟生成与乌龟窗体背景颜色的修改。

1.实操_话题发布

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

实现流程：

        1.通过计算图结合ros命令获取话题与消息信息

        2.编码实现运动控制节点

        3.启动roscore、turtlesim_node以及自定义的控制节点，查看运行结果

改变乌龟的Twist中的线速度x(m/s)和角速度z(rad/s)可以实现转圈目的

补充资料：

        弧度

                单位弧度定义为圆弧长度等于半径时的圆心角

        偏航、翻滚、俯仰

                绕Z轴（黄）转动是偏航，绕Y轴（紫）转动是俯仰，绕X轴（红）转动是翻滚。

话题发布

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

/*
需求：发布速度消息
    话题：/turtle1/cmd_vel
    消息：geometry_msgs/Twist

    1.包含头文件
    2.初始化ROS节点
    3.创建节点句柄
    4.创建发布对象
    5.发布逻辑
    6.spinOnce();
*/

int main(int argc, char *argv[])
{
    /* code */
    // 2.初始化ROS节点
    ros::init(argc,argv,"my_control");
    // 3.创建节点句柄
    ros::NodeHandle nh;
    // 4.创建发布对象
    ros::Publisher pub=nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);
    // 5.发布逻辑
    ros::Rate rate(10);//设置发布频率
    //组织被发布的消息
    geometry_msgs::Twist twist;
    twist.linear.x=1.0;
    twist.linear.y=0.0;
    twist.linear.z=0.0;
    twist.angular.x=0.0;
    twist.angular.y=0.0;
    twist.angular.z=1.0;
    //循环发布
    while (ros::ok())
    {
        pub.publish(twist);
        //休眠
        rate.sleep();
        //回头
        ros::spinOnce();
    }

    return 0;
}

2.实操_话题订阅

已知turtlesim的乌龟显示节点，会发布当前乌龟的位子（窗体中乌龟的坐标及朝向），要求控制乌龟运动，并实时打印当前乌龟的位姿。

实现流程：

        1.通过ros命令获取话题与消息信息。

        2.编码实现位姿获取节点。

        3.启动roscore、turtlesim_node、控制节点以及位姿订阅节点，控制乌龟运动并输出乌龟的位姿。

话题订阅

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

/*
需求：订阅乌龟的位姿信息，并输出到控制台
    1.包含头文件
    2.初始化ROS节点
    3.创建节点句柄
    4.创建订阅对象
    5.处理订阅到的数据（回调函数）
    6.spin();
*/

//处理订阅到的数据
void doPose(const turtlesim::Pose::ConstPtr &msg){
    ROS_INFO("乌龟的位姿信息:坐标（%.2f,%.2f),朝向(%.2f),线速度:%.2f,角速度:%.2f",msg->x,msg->y,msg->theta,msg->linear_velocity,msg->angular_velocity);
}

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

3.实操_服务调用

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

实现流程：

        1.通过ros命令获取服务与服务消息信息。

        2.编码实现服务请求节点。

        3.启动roscore、turtlesim_node、乌龟生成节点、生成新的乌龟。

服务调用

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

/*
需求：向服务端发送请求，生成一只新乌龟
    话题：/spawn
    消息：turtlesim/Spawn

    1.包含头文件
    2.初始化ROS节点
    3.创建节点句柄
    4.创建客户端对象
    5.组织数据并发送
    6.处理响应
*/

int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL,"");
    // 2.初始化ROS节点
    ros::init(argc,argv,"service_call");
    // 3.创建节点句柄
    ros::NodeHandle nh;
    // 4.创建客户端对象
    ros::ServiceClient client=nh.serviceClient<turtlesim::Spawn>("spawn");
    // 5.组织数据并发送
    //5-1.组织请求数据
    turtlesim::Spawn spawn;
    spawn.request.x=1.0;
    spawn.request.y=4.0;
    spawn.request.theta=1.57;
    spawn.request.name="turtle2";
    //5-2。发送请求
    //判断服务器状态
    //ros::service::waitForService("spawn");
    client.waitForExistence();
    bool flag=client.call(spawn);//flag是用来接收响应状态，响应结果也会被设置进spawn对象
    if(flag){
        ROS_INFO("乌龟生成成功，新乌龟叫：%s",spawn.response.name.c_str());
    }
    else{
        ROS_INFO("请求失败！！！");
    }
    // 6.处理响应
    return 0;
}

4.实操_参数设置

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

实现流程：

        1.通过ros命令获取参数。

        2.编码实现参数设置节点。

        3.启动roscore、turtlesim_node与参数设置节点，查看运行结果。

参数设置

#include "ros/ros.h"

/*
需求：修改参数服务器中turtlesim背景色相关的参数
    1.初始化ROS节点
    2.不一定需要创建节点句柄（和后续API有关）
    3.修改参数。
*/

int main(int argc, char *argv[])
{
    /* code */
    // 1.初始化ROS节点
    ros::init(argc,argv,"chang_bgColor");
    // 2.不一定需要创建节点句柄（和后续API有关）
    // ros::NodeHandle nh("turtlesim");
    // nh.setParam("background_r",255);
    // nh.setParam("background_g",255);
    // nh.setParam("background_b",255);

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

    // 3.修改参数。
    //如果调用ros::param不需要创建节点句柄
    // ros::param::set("turtlesim/background_r",0);
    // ros::param::set("turtlesim/background_g",0);
    // ros::param::set("turtlesim/background_b",0);
    
    return 0;
}

启动节点是，直接设置参数

$rosrun turtlesim turtlesim_ode _background_r:=100 _background_g:=0 _background_b:=0