ROS：通信机制

一、话题发布实操

1.1需求

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

1.2分析

乌龟运动控制实现，关键节点有两个，一个是乌龟运动显示节点 turtlesim_node，另一个是控制节点，二者是订阅发布模式实现通信的，乌龟运动显示节点直接调用即可，运动控制节点之前是使用的 turtle_teleop_key通过键盘控制，现在需要自定义控制节点。

控制节点自实现时，首先需要了解控制节点与显示节点通信使用的话题与消息，可以使用ros命令结合计算图来获取。

1.3实现流程

通过计算图结合ros命令获取话题与消息信息。
编码实现运动控制节点。
启动 roscore、turtlesim_node 以及自定义的控制节点，查看运行结果。

1.4实现代码

1.4.1C++版

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

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

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

int main(int argc, char *argv[])
{
    
// 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=0.5;
    //循环发布
    while(ros::ok())
    {
        pub.publish(twist);
        //休眠
        rate.sleep();
        //回头
        ros::spinOnce();
    }
// 6.spinOnce()
    return 0;
}

1.4.2Python版

#! /usr/bin/env python
"""
    编写 ROS 节点，控制小乌龟画圆

    准备工作:
        1.获取topic(已知: /turtle1/cmd_vel)
        2.获取消息类型(已知: geometry_msgs/Twist)
        3.运行前，注意先启动 turtlesim_node 节点

    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建发布者对象
        4.循环发布运动控制消息

"""

import rospy
from geometry_msgs.msg import Twist

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("control_circle_p")
    # 3.创建发布者对象
    pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=1000)
    # 4.循环发布运动控制消息
    rate = rospy.Rate(10)
    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 = 0.5

    while not rospy.is_shutdown():
        pub.publish(twist)
        rate.sleep()

1.5执行

首先，启动 roscore；

然后启动乌龟显示节点；

最后执行运动控制节点；
在这里插入图片描述

在这里插入图片描述

二、话题订阅实操

2.1需求

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

2.2分析

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

2.3流程

通过ros命令获取话题与消息信息。
编码实现位姿获取节点。
启动 roscore、turtlesim_node 、控制节点以及位姿订阅节点，控制乌龟运动并输出乌龟的位姿。

2.4实现代码

2.4.1启动无辜GUI与键盘控制节点

<!--启动无辜GUI与键盘控制节点-->
<launch>
    <!--乌龟GUI-->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />

    <!--键盘控制-->
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />

</launch>

2.4.2C++版

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

/*
需求：订阅乌龟的位姿信息


1.包含头文件 
2.初始化ros节点
3.创建节点句柄
4.创建订阅对象
5.处理订阅数据（回调函数）
6.spinOnce()
*/

void doPose(const turtlesim::Pose::ConstPtr &pose)
{
    ROS_INFO("乌龟的位姿信息：坐标(%.2f,%.2f),朝向(%.2f),线速度:%.2f,角速度:%.2f",
            pose->x,pose->y,pose->theta,pose->linear_velocity,pose->angular_velocity);
}

int main(int argc, char *argv[])
{
    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();
 
// 6.spinOnce()
    return 0;
}