ROS乌龟运动控制

ROS学习笔记（一） [例]话题（Topic）通信机制

乌龟运动控制

一：分析

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

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

3. 了解了话题与消息之后，通过 C++ 或 Python 编写运动控制节点，通过指定的话题，按照一定的逻辑发布消息即可。

二：实现流程

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

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

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

（1）话题的获取与消息信息

首先，启动键盘启动小乌龟运动案例

roscore

打开第二个终端

rosrun turtlesim turtlesim_node

打开第三个终端(terminal)

rosrun turtlesim turtle_teleop_key

话题获取

打开一个终端输入：

rqt_graph

此命令显示了节点的话题为/turtle1/cmd_vel，然后，通过上面获取的话题名来获取消息type

rostopic info /turtle1/cmd_vel

获取消息格式：

rosmsg show geometry_msgs/Twist

乌龟线速度取值只有x和角速度z，

rostopic echo /turtle1/cmd_vel

可以看到只有线速度x和角速度y，因为只有上下左右四个键，上下控制线速度x，左右控制角速度z

（2）创建功能包

cd ~/catkin_ws/src
catkin_create_pkg my_turtle_package roscpp rospy std_msgs geometry_msgs turtlesim

（3）在my_turtle_package/src 中创建cpp文件

cd my_turtle_package/src/　　　　
touch draw_triangle.cpp

打开draw_triangle.cpp画三角形

gedit draw_triangle.cpp

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <math.h>

int main(int argc, char** argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "turtle_triangle");
    ros::NodeHandle nh;
    // 创建发布者对象
    // turtle1/cmd_vel为节点话题，type为geometry_msgs::Twist
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);

    //设置发送频率
    ros::Rate rate(2);

    int i=0;
    //  4.循环发布运动控制消息
    
    while(ros::ok())
    {
        //声明一个geometry_msgs::Twist 类型的对象msg，并将速度的值赋值到这个对象里面
        geometry_msgs::Twist msg;
        msg.linear.x = 1.0;
        msg.angular.z = 0.0; // 不进行旋转
        i++;
        if(i==5)
        {
            i = 0;
            msg.linear.x = 0;
            msg.angular.z = M_PI * (4.0/3);// 向左旋转 120  度
            //msg.angular.z =  M_PI * (8.0/5);// wujiaoxing
        }

        pub.publish(msg);
        rate.sleep();
    }

    return 0;
}

（4）编译节点

配置CMakeLists

cd ~/catkin_ws/src/my_turtle_package
gedit CMakeLists.txt            

添加下面内容

add_executable(draw_triangle src/draw_triangle.cpp)
target_link_libraries(draw_triangle ${catkin_LIBRARIES})

编译工程项目

cd ~/catkin_ws
catkin_make

（5）运行测试

启动ROS Master确保roscore可用，在一个窗口中并运行：

roscore

新开一个窗口运行：

rosrun turtlesim turtlesim_node

打开第三个终端(terminal)运行小乌龟走三角行

rosrun my_turtle_package draw_triangle

运行wujiaoxing

rosrun my_turtle_package draw_pentahram