ROS入门之乌龟圆周运动

学习ROS已经快一个月了，学习了ROS通信机制中的话题通信，服务通信，参数服务器。了解到之前最开始最经典的乌龟运动，原来是通过话题实现乌龟的位姿与键盘之间的通信。但是键盘控制乌龟运动是无规律的，而且运动轨迹太难看了。突发奇想就是能不能让乌龟按照一定的轨迹运行呢？例如做圆周运动，而且正好也可以利用这个机会巩固下对话题通信的理解，也作为自己ROS的第一次实战。
首先我们先运行下最开始的小乌龟：

// 启动ROS内核
roscore
//运行乌龟的GUI
rosrun turtlesim turtlesim_node
//运行键盘操控节点
rosrun turtlesim turtle_teleop_key

但是这么运动太难看了，也不好控制，既然乌龟是话题通信，我们就要知道谁是发布方谁是订阅方：

// 查看节点关系图
rqt_graph

这里我们可以看到，发布方是/teleop_turtle也就是键盘部分，订阅方是/turtlesim也就是乌龟的GUI,话题是/turtle1/cmd_vel。清楚了以上关系，我们改写下发布方的发布消息发布圆周运动的消息不就可以控制乌龟的位姿了嘛！但是此时我们还不知道发布的消息是什么类型，整型呢还是浮点呢，必须要看一下：

// 查看下现在有哪些话题
rostopic list
//查看下我们所需要的话题详细信息
rostopic info /turtle1/cmd_vel

可以看到在geometry_msgs文件包下的Twist文件，下面我们就要看下有什么吧！

//查看msg包下的详细信息
rosmsg info geometry_msgs/Twist

可以看到线速度三个参数，角速度三个参数都是浮点型的，因此我们可以改写线速度和角速度控制乌龟的位姿，但是要想乌龟做圆周运动要控制哪些参数呢？考虑到乌龟只是在二维平面运动，那不就解决了，线速度改写x轴的，角速度乌龟只有偏航角，那不就是z参数。

为了进一步验证我们的猜测，先看下只改写这两个参数的乌龟的轨迹：

//手动编写发布方，每秒发送五个
rostopic pub -r 5 /turtle1/cmd_vel geometyr_msgs/Twist tab/tab

哇塞，果真所料,接下来只要编写发布方函数实现不就可以了嘛！胜利的曙光已经开始显现！！接下来代码呈上（一定要包含geometyr_msgs包因为这个包含设置乌龟位姿的数据）：

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
/*
思路：
键盘传参,判断接受的数据个数，如果是三个
（函数包名一个，线速度x,角速度z），就将后面两个依次
赋值给线速度x和角速度z 
    1、导入包
    2、初始化ros节点
    3、初始化ros句柄
    4、创建发布方以及消息载体
    5、编写发布函数实现发布
*/
int main(int argc, char *argv[])
{
    //方便打印中文
    setlocale(LC_ALL,"");
    if(argc!=3)
    {
    ROS_INFO("输入的参数不对");
    return 1;
    }
    //初始化ros节点
    ros::init(argc,argv,"test0_turtle");
    //初始化ros句柄
    ros::NodeHandle nh;
    //创建发布方（注意数据类型）
    ros::Publisher message=nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    //创建消息载体（注意数据类型）
    geometry_msgs::Twist turtle;
    //修改线速度
    turtle.linear.x=atof(argv[1]);
    turtle.linear.y=0.0;
    turtle.linear.z=0.0;
    //修改角速度
    turtle.angular.x=0.0;
    turtle.angular.y=0.0;
    turtle.angular.z=atof(argv[2]);
    //发布速率
    ros::Rate rate(1);
    //等待上述初始化完成在发布
    ros::Duration(1).sleep();
    while(ros::ok)
    {
     //发布数据
     message.publish(turtle);
     ROS_INFO("发布的线速度数据是：\n x=%.1f\n,y=%.1f\n,z=%.1f\n",turtle.linear.x,turtle.linear.y,turtle.linear.z);
     ROS_INFO("发布的角速度数据是：\n x=%.1f\n,y=%.1f\n,z=%.1f\n",turtle.angular.x,turtle.angular.y,turtle.angular.z);
     rate.sleep();
     //处理回调函数
     ros::spinOnce();
    }
    return 0;
}

运行效果图：

终于实现了我们开始最初的目的，这样看起来是不是更美观些，做到这里真的挺开心的，第一次实战的成功给了自己更大的动力！！
最后尝试下python版本的，其实大同小异：

下面展示一些 内联代码片。

import rospy
from geometry_msgs.msg import Twist#导入乌龟参数类型包
import sys#用于键盘传参数

if __name__=="__main__":
    if len(sys.argv)!=3:
        rospy.loginfo("传入的参数不对！")
        sys.exit(1)
    #初始化ros节点
    rospy.init_node("wugui")
    #创建发布对象
    pub=rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=10)
    #创建消息载体
    turtle=Twist()
    #线速度角速度赋值
    turtle.linear.x=float(sys.argv[1])
    turtle.linear.y=0.0
    turtle.linear.z=0.0
    turtle.angular.x=0.0
    turtle.angular.y=0.0
    turtle.angular.z=float(sys.argv[2])
    #发布速率
    rate=rospy.Rate(1)
    #等待上述初始化完成
    rospy.sleep(2)
    #编写循环发布数据
    while not rospy.is_shutdown():
        pub.publish(turtle)
        rospy.loginfo("发布的线速度数据是：\n x=%.1f\n,y=%.1f\n,z=%.1f\n",turtle.linear.x,turtle.linear.y,turtle.linear.z)
        rospy.loginfo("发布的角速度数据是：\n x=%.1f\n,y=%.1f\n,z=%.1f\n",turtle.angular.x,turtle.angular.y,turtle.angular.z)
        rate.sleep()

效果图：