ROS2入门：turtlesim的奇妙曲线轨迹

ROS2入门：turtlesim的奇妙曲线轨迹

---

时间：2021.10.2
（注意：需要确保tutlesim与python的安装）

---

安装turtlesim并运行一个简单节点

---

首先，在自己的环境下安装turtlesim。本文中的ROS2版本为foxy，读者可以根据自己的需求将代码中的foxy替换为自己的ROS2版本。

sudo apt update
sudo apt install ros-foxy-turtlesim

如果不确定自己的环境里是否已经安装了turtlesim，可以在终端中输入：

ros2 pkg executables turtlesim

如果安装成功，或者之前已经安装过了，就会在终端返回如下结果：

turtlesim draw_square
turtlesim mimic
turtlesim turtle_teleop_key
turtlesim turtlesim_node

要启动turtlesim，请在终端中输入以下命令：

ros2 run turtlesim turtlesim_node

模拟器窗口应该出现，中间有一个随机的海龟。

每次，出现的小乌龟都有可能不同，要是看厌了小乌龟，当然可以整活儿！

在命令下的终端中，将看到来自节点的消息：

[INFO] [1633058705.906234086] [turtlesim]: Starting turtlesim with node name /turtlesim
[INFO] [1633058705.910190716] [turtlesim]: Spawning turtle [turtle1] at x=[8.367925], y=[6.273585], theta=[0.000000]

---

通过使用turtle_teleop_key实现对turtlesim的键盘控制

---

打开一个新的终端窗口，并输入：

ros2 run turtlesim turtle_teleop_key

在该窗口下将会出现：

Reading from keyboard
---------------------------
Use arrow keys to move the turtle.
Use G|B|V|C|D|E|R|T keys to rotate to absolute orientations. 'F' to cancel a rotation.
'Q' to quit.

根据以上指示使用键盘对turtlesim进行简单的控制。

在ROS1的官网教程中有关于如何用python编写publiser使turtlesim按照直线轨迹运动，左转右转，到达固定位置的教程。在这里就不过多阐述了，感兴趣的读者可以直接去官网查看。
而在广大网友的不懈努力之下，turtlesim也整出了新活，比如说：画圆形轨迹，画矩形轨迹，画三角形轨迹等等。接下来，以圆形为例，讲解如何使用python编写publisher使turtlesim按照圆的轨迹持续运动。

---

使用python编写publisher控制turtlesim按照圆的轨迹运动

---

借助圆的几何原理：
其实，并不难发现，只要我们能指定小龟的角速度与线速度皆为定制，就可以实现圆形曲线的绘制。
代码如下：

import rclpy
import math
from rclpy.node import Node

# from std_msgs.msg import String
from geometry_msgs.msg import Twist



class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('my_pub')
        self.publisher_ = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)
        timer_period = 1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        vel = Twist()
#        vel.data = 'My Hello World: %d' % self.i
        t = self.i/10
        vel.linear.x =  2.0
        vel.angular.z =  2.0
        self.publisher_.publish(vel)
        self.get_logger().info('Publishing: 线速度x: %f 角速度z: %f' % (vel.linear.x,vel.angular.z))
#        self.get_logger().info('Publishing: z"%f"' % vel.angular.z)
        self.i += 1


def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

结果如下：

---

使用python编写publisher控制turtlesim按照椭圆的轨迹运动

---

联系上文，沿用编写圆形轨迹的publisher的思路，当小龟的角速度固定时，小龟的线速度无论是怎样的定值，小龟的轨迹总为圆。在此，需要借助椭圆与圆的几何关系。观察椭圆轨迹的绘制：

可以发现，在角速度固定的情况下，线速度随时间周期性变化，或者在线速度不变的情况下，使角速度周期性变化。就可以实现椭圆轨迹的绘制。
在此，还需要联系椭圆在平面坐标系下的方程：

以角速度固定的情况下，线速度随时间周期性变化为例：

import rclpy
from rclpy.node import Node
# from std_msgs.msg import String
from geometry_msgs.msg import Twist
class MinimalPublisher(Node):
    def __init__(self):
        super().__init__('my_pub')
        self.publisher_ = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)
        timer_period = 1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0
    def timer_callback(self):
        vel = Twist()
        t = self.i
        a = 1.5  
        b = 3 - (a * ( math.cos(t)) )
        vel.linear.x = 0.4*b
        vel.angular.z = 0.5
        self.publisher_.publish(vel)
        self.get_logger().info('Publishing: 线速度x: %f 角速度z: %f' % (vel.linear.x,vel.angular.z))
#        self.get_logger().info('Publishing: z"%f"' % vel.angular.z)
        self.i += 1
def main(args=None):
    rclpy.init(args=args)
    minimal_publisher = MinimalPublisher()
    rclpy.spin(minimal_publisher)
    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()
if __name__ == '__main__':
    main()

结果如下：

当然，通往答案的路不止一条。我们也可以将角速度设为0，而线速度x与线速度y均采取变化来实现椭圆的轨迹。

import rclpy
import math
from rclpy.node import Node
# from std_msgs.msg import String
from geometry_msgs.msg import Twist
class MinimalPublisher(Node):
    def __init__(self):
        super().__init__('my_pub')
        self.publisher_ = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)
        timer_period = 0.1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0
    def timer_callback(self):
        vel = Twist()
#        vel.data = 'My Hello World: %d' % self.i
        t = (self.i/180.0)*math.pi
        a = 1
        b = 1.5
        c = a*math.cos(t)
        d = b*math.sin(t)
        vel.linear.x = 0.2*d
        vel.linear.y = c
        self.publisher_.publish(vel)
        self.get_logger().info('Publishing: 线速度x: %f 线速度y: %f' % (vel.linear.x,vel.angular.z))
#        self.get_logger().：('Publishing: z"%f"' % vel.angular.z)
        self.i += 1
def main(args=None):
    rclpy.init(args=args)
    minimal_publisher = MinimalPublisher()
    rclpy.spin(minimal_publisher)
    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()
if __name__ == '__main__':
    main()

结果如下：

联动魔幻轨迹

---

zhangrelay老师的国庆祝福

zhangrelay老师的魔幻轨迹传送门！！！

这里是对zhangrelay老师的轨迹题型的不及格作答：

import rclpy
import math
from rclpy.node import Node

# from std_msgs.msg import String
from geometry_msgs.msg import Twist



class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('my_pub')
        self.publisher_ = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)
        timer_period = 1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        vel = Twist()
#        vel.data = 'My Hello World: %d' % self.i
        t = self.i/10
        a = 1.21111
        b = a * (2 * math.sin(t) + math.sin(2 * t))
        c = a * (2 * math.cos(t) + math.cos(2 * t))
        vel.linear.x =  0.2*b
        vel.angular.z =  0.5*c
        self.publisher_.publish(vel)
        self.get_logger().info('Publishing: 线速度x: %f 角速度z: %f' % (vel.linear.x,vel.angular.z))
#        self.get_logger().info('Publishing: z"%f"' % vel.angular.z)
        self.i += 1


def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

对ROS的学习其实是一个长期的过程，把官网上的教程简单的运行一遍，很难算真正入门。只有在理解、掌握并贯通一系列知识的基础上，才能真正地做到随心所欲。而非像本人在知识水平不达标的情况下的拙劣复刻。zhangrelay老师的博客里面还有很多经验的turtlesim轨迹案例，还有强力的辅助工具，在这里就不去引流了，请感兴趣的读者自行挖掘，如果有宝贵的建议或者妙趣横生的想法，欢迎私聊。

彩蛋

如何用turtlesim致敬IRON MAN？？？