ROS2 实现小乌龟跟随

ROS2 学习

ROS（机器人操作系统）
按照官方案例做了一遍，然后实现了一个小乌龟跟随运动，并且调用服务的案例，算是个综合性的练习吧。(手动计算坐标，没有用到tf2相关模块）
啥都不说，看效果和代码。

GIF 2020-9-2 13-56-07.gif

先启动官方的小乌龟
ros2 run turtlesim turtlesim_node
在启动编写的节点
ros2 run py_pubsub my_test_node2
代码放在了 py_pubsub中，代码目录结构如下：

└── src
    ├── py_pubsub
    │   ├── launch
    │   │   ├── my_test.launch.py
    │   ├── package.xml
    │   ├── py_pubsub
    │   │   ├── __init__.py
    │   │   ├── my_test_node2.py

my_test_node2.py 代码如下所示：

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import math
import random
from turtlesim.srv import Spawn, Kill


class Target:
    def __init__(self):
        self.name = None
        self.is_spawn = False
        self.pose = None


class MinimalPublisher(Node):

    def __init__(self):
        super(MinimalPublisher, self).__init__('my_test_node2')

        self.publisher_ = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)
        self.subscription_ = self.create_subscription(Pose, 'turtle1/pose', self.subscription_callback, 10)
        # 生成海龟 服务调用
        self.client_spawn = self.create_client(Spawn, 'spawn', )
        while not self.client_spawn.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service /spawn not available, waiting again...')
        # 生成海龟 服务调用
        self.client_kill = self.create_client(Kill, 'kill', )
        while not self.client_spawn.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service /kill not available, waiting again...')
        self.index_spawn = 10  # 生成乌龟序号 turtle10 开始 ... turtle 101
        self.req_spawn = Spawn.Request()

        self.req_kill = Kill.Request()
        self.pose_ = Pose()  # 第一个乌龟的的当前姿势
        self.msg = Twist()  # 第一个乌龟的的 速度，需要发布
        self.target: Target = Target()  # 第一个乌龟的的当前目标
        # 新生成一个海龟
        self.spawn_target()

        # for publish
        self.timer = self.create_timer(1 / 100, self.time_callback)

    def time_callback(self):
        if self.target and self.target.is_spawn:
            if math.sqrt(math.fabs((self.pose_.x - self.target.pose.x) ** 2) +
                         math.fabs((self.pose_.y - self.target.pose.y) ** 2)) < 0.1:
                # 如果追到目标，服务调用kill掉
                self.req_kill.name = self.target.name
                self.client_kill.call_async(self.req_kill).add_done_callback(
                    self.future_kill_callback)
                self.target = None
            else:
                # 计算速度方向，并且发布
                length = math.sqrt(math.fabs((self.target.pose.x - self.pose_.x) ** 2) +
                                   math.fabs((self.target.pose.y - self.pose_.y) ** 2))
                speed = 5.0
                self.msg.linear.x = (self.target.pose.x - self.pose_.x) / length * speed
                self.msg.linear.y = (self.target.pose.y - self.pose_.y) / length * speed

                self.publisher_.publish(self.msg)
        else:
            # 如果没有目标就不动。
            self.msg.linear.x = 0.0
            self.msg.linear.y = 0.0
            self.publisher_.publish(self.msg)

    def subscription_callback(self, pose: Pose):
        """ 获取海龟位置
        :param pose:
        :return:
        """
        self.pose_ = pose

    def spawn_target(self):
        p = Pose(x=float(random.randint(1, 10)), y=float(random.randint(1, 10)))
        self.req_spawn.x = p.x
        self.req_spawn.y = p.y
        self.req_spawn.name = 'turtle' + str(self.index_spawn)
        self.index_spawn += 1

        self.target = Target()
        self.target.is_spawn = False  # 等待异步调用完成
        self.target.name = self.req_spawn.name
        self.target.pose = p

        self.client_spawn.call_async(self.req_spawn).add_done_callback(self.future_spawn_callback)

    def future_spawn_callback(self, args: rclpy.Future):
        result: Spawn.Response = args.result()
        self.target.is_spawn = True

    def future_kill_callback(self, args: rclpy.Future):
        self.spawn_target()


def main(args=None):
    rclpy.init(args=args)
    minimal_publisher = MinimalPublisher()
    try:
        rclpy.spin(minimal_publisher)
        minimal_publisher.destroy_node()
        rclpy.shutdown()
    except KeyboardInterrupt as e:
        print('exit normal .. ')


if __name__ == '__main__':
    main()