ROS2 小作業
===
[TOC]

# 目標

創建一個ROS2節點，使用turtlesim套件控制海龜的移動，實現以下功能:

1. 控制海龜的移動方向和速度 -> **Topic**
2. 改變海龜行徑路徑的顏色和寬度 -> **Service** & **parameter**
3. 傳輸海龜的位置和顏色 -> **Custom msg**

---

## 建立worksapce跟package

建立workspace
`mkdir -p ~/ros2_practice/src`

移動到放package的資料夾底下
`cd ~/ros2_practice/src`

建立package
`ros2 pkg create --build-type ament_python turtlesim_controller`

執行turtlesim node
`ros2 run turtlesim turtlesim_node`

## 撰寫程式 --- 控制海龜的移動方向和速度

Tips:接收烏龜移動控制的topic為**turtle1/cmd_vel**，只要將烏龜移動的參數發佈給他接收就可以改變移動速度

程式放在`~/ros2_practice/src/turtlesim_controller/turtlesim_controller`下

import rclpy
from rclpy.node import Node

from geometry_msgs.msg import Twist

class TurtlesimController(Node):

    def __init__(self):
        super().__init__('turtlesim_controller')
        #Create publisher for twist messages
        self.twistPub_ = self.create_publisher(Twist, "turtle1/cmd_vel", 10)
        #Create subscription for twist messages
        self.twistSub_ = self.create_subscription(
            Twist,
            "turtle1/move",
            self.twistSub_callback,
            10)
    def twistSub_callback(self, msg):
        self.twistPub_.publish(msg)


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

    turtlesim_controller = TurtlesimController()

    rclpy.spin(turtlesim_controller)

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


if __name__ == '__main__':
    main()

package.xml加上依賴

<exec_depend>rclpy</exec_depend>
<exec_depend>geometry_msgs</exec_depend>

setup.py加上程式進入點

entry_points={
        'console_scripts': [
            ' controller= turtlesim_controller.controller:main',
        ],
    },

Topic之間的關係透過rqt查看


透過command可以發topic出去使用，[Twist的資料結構](https://docs.ros2.org/latest/api/geometry_msgs/msg/Twist.html)

`ros2 topic pub --once /turtle1/move geometry_msgs/msg/Twist "{linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.0}}"`


## 撰寫程式 --- 改變海龜行徑路徑的顏色和寬度
Tips:透過parameter紀錄烏龜路徑的顏色跟寬度，並以一定頻率更新，其中真的修改路徑顏色跟寬度的service為**SetPen**，[SetPen的資料結構](http://docs.ros.org/en/melodic/api/turtlesim/html/srv/SetPen.html)

修改controller程式

import rclpy
from rclpy.node import Node

# from std_msgs.msg import String
from geometry_msgs.msg import Twist
from turtlesim.srv import SetPen
class TurtlesimController(Node):

    def __init__(self):
        super().__init__('turtlesim_controller')
        #Create publisher for twist messages
        self.twistPub_ = self.create_publisher(Twist, "turtle1/cmd_vel", 10)
        #Create subscription for twist messages
        self.twistSub_ = self.create_subscription(
            Twist,
            "turtle1/move",
            self.twistSub_callback,
            10)
        
        self.setPenSrv_ = self.create_client(SetPen, "turtle1/set_pen")

        self.declare_parameter('pen_rgb', [125, 125, 125])
        self.declare_parameter('pen_width', 1)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
    def twistSub_callback(self, msg):
        self.twistPub_.publish(msg)
    
    def changePenColor(self):
        req= SetPen.Request()
        r, g, b = self.get_parameter('pen_rgb').value
        req.r = r
        req.g = g
        req.b = b
        req.width = self.get_parameter('pen_width').value
        self.future = self.setPenSrv_.call_async(req)
        return self.future.result()
    def timer_callback(self):
        response = self.changePenColor()
     

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

    turtlesim_controller = TurtlesimController()

    rclpy.spin(turtlesim_controller)

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


if __name__ == '__main__':
    main()

透過指令修改路徑顏色跟寬度
`ros2 param set /turtlesim_controller pen_width 10`
`ros2 param set /turtlesim_controller pen_rgb [255, 255, 255]`


## 撰寫程式 --- 傳輸海龜的位置和顏色
Tips:建立一個新的topic資料結構包含海龜的位置跟顏色，就可以進行追蹤

新增一個新的package
`cd ~/ros2_practice/src`

`ros2 pkg create --build-type ament_cmake my_turtlesim_interfaces`

在~/ros2_pracitce/src/my_turtlesim_interfaces下新增msg資料夾
`mkdir msg`

my_turtlesim_interfaces/msg撰寫msg資料型態，==檔案名稱要大寫，拒絕任何特殊字元==，成功的檔案名稱如TurtleInfo.msg

turtlesim/Pose position
uint8[] turtle_color

CMakeList.txt新增依賴

find_package(turtlesim REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Turtle_info.msg"
  DEPENDENCIES turtlesim 
)

package.xml新增依賴

<depend>turtlesim</depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

測試我的msg是否正常
`ros2 interface show my_turtlesim_interfaces/msg/TurtleInfo`
輸出

turtlesim/Pose position
uint8[] turtle_color

修改controller

import rclpy
from rclpy.node import Node

# from std_msgs.msg import String
from geometry_msgs.msg import Twist
from turtlesim.srv import SetPen
from turtlesim.msg import Pose
from my_turtlesim_interfaces.msg import TurtleInfo
class TurtlesimController(Node):

    def __init__(self):
        super().__init__('turtlesim_controller')
        #Create publisher for twist messages
        self.twistPub_ = self.create_publisher(Twist, "turtle1/cmd_vel", 10)
        #Create subscription for twist messages
        self.twistSub_ = self.create_subscription(
            Twist,
            "turtle1/move",
            self.twistSub_callback,
            10)
        
        self.setPenSrv_ = self.create_client(SetPen, "turtle1/set_pen")
        self.declare_parameter('pen_rgb', [125, 125, 125])
        self.declare_parameter('pen_width', 1)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)

        self.turtleInfoPub_ = self.create_publisher(TurtleInfo, "turtle1/info", 10)
        self.poseSub_ = self.create_subscription(
            Pose,
            "turtle1/pose",
            self.poseSub_callback,
            10)
    def twistSub_callback(self, msg):
        print(msg)
        self.twistPub_.publish(msg)
    
    def changePenColor(self):
        req= SetPen.Request()
        r, g, b = self.get_parameter('pen_rgb').value
        req.r = r
        req.g = g
        req.b = b
        req.width = self.get_parameter('pen_width').value
        self.future = self.setPenSrv_.call_async(req)
        return self.future.result()
    def timer_callback(self):
        self.changePenColor()

    def poseSub_callback(self, msg):
        turtleInfoMsg = TurtleInfo()
        turtleInfoMsg.position = msg
        turtleInfoMsg.turtle_color = self.get_parameter('pen_rgb').value
        self.turtleInfoPub_.publish(turtleInfoMsg)

     

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

    turtlesim_controller = TurtlesimController()

    rclpy.spin(turtlesim_controller)

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


if __name__ == '__main__':
    main()

可以透過指令獲取turtleInfo

`ros2 topic echo /turtle1/info`

輸出

---
position:
  x: 5.544444561004639
  y: 5.544444561004639
  theta: 0.0
  linear_velocity: 0.0
  angular_velocity: 0.0
turtle_color:
- 125
- 125
- 125
---