ROS2 學(xué)習(xí)

ROS（機(jī)器人操作系統(tǒng)）
按照官方案例做了一遍，然后實(shí)現(xiàn)了一個小烏龜跟隨運(yùn)動，并且調(diào)用服務(wù)的案例，算是個綜合性的練習(xí)吧。(手動計算坐標(biāo)，沒有用到tf2相關(guān)模塊）
啥都不說，看效果和代碼。

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

先啟動官方的小烏龜
ros2 run turtlesim turtlesim_node
在啟動編寫的節(jié)點(diǎn)
ros2 run py_pubsub my_test_node2
代碼放在了 py_pubsub中，代碼目錄結(jié)構(gòu)如下：

└── 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)
        # 生成海龜 服務(wù)調(diào)用
        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...')
        # 生成海龜 服務(wù)調(diào)用
        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()  # 第一個烏龜?shù)牡漠?dāng)前姿勢
        self.msg = Twist()  # 第一個烏龜?shù)牡?速度，需要發(fā)布
        self.target: Target = Target()  # 第一個烏龜?shù)牡漠?dāng)前目標(biāo)
        # 新生成一個海龜
        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:
                # 如果追到目標(biāo)，服務(wù)調(diào)用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:
                # 計算速度方向，并且發(fā)布
                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:
            # 如果沒有目標(biāo)就不動。
            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  # 等待異步調(diào)用完成
        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()