1话题发布

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

实现分析：

实现乌龟的控制，有两个关键的节点(Node)：一个是乌龟运动显示节点 turtlesim_node，另外一个是键盘控制节点，二者通过话题通信实现通信。
实现控制节点之前，需要了解节点控制所使用的话题和消息，可以使用ros的有关命令和rqt计算图来获取。
了解了话题与消息之后，通过 C++ 或 Python 编写运动控制节点，通过指定的话题，按照一定的逻辑发布消息即可。

实现流程：

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

1.1 启动tutlesim节点

启动三个终端
终端1键入：roscore
终端2键入：rosrun turtlesim turtlesim_node
终端3键盘：rosrun turtlesim turtle_teleop_key

注意： 光标必须聚焦在键盘控制的终端内，否则无法控制乌龟运动。

1.2 话题与消息获取

1.2.1 话题获取

通过计算图查看话题：

rqt_graph

或者通过 rostopic 列出话题:

rostopic list

1.2.2 消息获取

获取消息类型:

rostopic type /turtle1/cmd_vel
output：
		geometry_msgs/Twist

获取消息格式:

rosmsg info geometry_msgs/Twist

output：
		geometry_msgs/Vector3 linear         ## xyz方向的线速度
		  float64 x
		  float64 y
		  float64 z
		geometry_msgs/Vector3 angular		 ## x轴上的翻滚、y轴上的俯仰和z轴上的偏航的角速度
		  float64 x
		  float64 y
		  float64 z

1.3 实现发布节点(C++)

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

/*
    编写 ROS 节点，控制小乌龟画圆

    准备工作:
        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;
}

配置文件同ROS系列(四)：ROS通信机制系列(1)：话题通信中的发布方配置文件。

1.4 运行

运行同本文 1.1 启动tutlesim节点。

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

运行结果如下所示：
在这里插入图片描述

2 话题订阅

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

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

2.1 话题与消息获取

获取话题:/turtle1/pose：
	rostopic list
获取消息类型:turtlesim/Pose
	rostopic type  /turtle1/pose
获取消息格式:
	rosmsg info turtlesim/Pose
	结果：
	?float32 x
	float32 y
	float32 theta
	float32 linear_velocity
	float32 angular_velocity

2.2 实现订阅节点(C++)

创建功能包需要依赖的功能包: roscpp rospy 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;
}