[feat]:finished chapt8.4

docs/chapt8/8.2RVIZ2可视化移动机器人模型.md

@@ -131,7 +131,7 @@ def generate_launch_description():
 ```mermaid
 graph
 A[joint_state_publisher]--joint_states-->B
-B[robot_state_publisher]--robot_de-->C
+B[robot_state_publisher]--robot_description-->C
 C[rviz2]
 
 ```

docs/chapt8/8.4控制移动机器人轮子运动.md

@@ -36,8 +36,7 @@ from rclpy.node import Node
 class RotateWheelNode(Node):
     def __init__(self,name):
         super().__init__(name)
-        self.get_logger().info("rotate node init.." % name)
-
+        self.get_logger().info(f"node {name} init..")
 
 def main(args=None):
     """
@@ -145,18 +144,38 @@ class RotateWheelNode(Node):
 
 ## 3.编写发布逻辑
 
+### 3.1 多线程定频发布Rate
+
 创建好发布者，我们想让话题按照某个固定的速度进行发布，可以采用ROS2中的定时神器Rate,不清楚Rate的小伙伴可以看看小鱼的这篇文章：[ROS中的定频神器你会用吗](https://mp.weixin.qq.com/s/bFbTIh6rGou1k0Ach-hlqA)
 
-为了能够一直循环使用rate，我们单独开一个线程用于发布joint_states话题数据
+为了能够一直循环使用rate，我们单独开一个线程用于发布joint_states话题数据，在ROS2程序中单独开线程进行话题发布的方法为：
 
+```python
+import threading
+from rclpy.node import Node
 
+class RotateWheelNode(Node):
+    def __init__(self):
+        # 创建一个Rate和线程
+        self.pub_rate = self.create_rate(5) #5Hz
+        # 创建线程
+        self.thread_ = threading.Thread(target=self._thread_pub)
+        self.thread_.start()
+
+    def _thread_pub(self):
+        while rclpy.ok():
+        	#做一些操作，使用rate保证循环频率
+            self.pub_rate.sleep()
+```
+
+### 3.2 构造发布数据
 
 接着我们来构造发布的数据：
 
-joint_states有一个头和四个数组需要赋值
+joint_states有一个头和四个数组需要赋值（可通过ros2 interface指令查询）
 
 ```
-std_msgs/Header header #时间戳信息
+std_msgs/Header header #时间戳信息 和 frame_id
 string[] name
 float64[] position
 float64[] velocity
@@ -190,35 +209,132 @@ float64[] velocity #关节速度数组
 float64[] effort #扭矩数据
 ```
 
->  单个轮子的转速 = (当前的位置-上一时刻位置）/ 两个时间之间的间隔
 
-所以最终有如下代码：
 
+#### 3.2.1 name
+
+name是关节的名称，要与urdf中的定义的关节名称相同，根据我们的URDF定义有
+
+```python
+self.joint_states.name = ['left_wheel_joint','right_wheel_joint']
 ```
 
+#### 3.2.2 position
+
+表示关节转动的角度值，因为关节类型为`continuous`,所以其值无上下限制，初始值赋值为0.0
+
+```python
+# 关节的位置
+self.joint_states.position = [0.0,0.0]
 ```
 
+我们采用速度控制机器人轮子转动，所以机器人的位置更新则可以通过下面式子计算得出
 
+> 某一段时间内轮子转动的角度 = （当前时刻-上一时刻）*两个时刻之间的轮子转速
 
-## 4.编译测试
+```python
+delta_time =  time.time()-last_update_time
+current_position = self.joint_states.position
+self.joint_states.position[0]  += delta_time*current_position[0]
+self.joint_states.position[1]  += delta_time*current_position[1]
+```
+
+#### 3.2.3 velocity
+
+因为我们采用速度进行控制，所以对外提供一个速度更改接口。
 
+```python
+def update_speed(self,speeds):
+	self.joint_speeds = speeds
+```
 
+### 3.3 完成后代码
+
+```python
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+# 1.导入消息类型JointState
+from sensor_msgs.msg import JointState
+
+import threading
+import time
+
+class RotateWheelNode(Node):
+    def __init__(self,name):
+        super().__init__(name)
+        self.get_logger().info(f"node {name} init..")
+        # 创建并初始化发布者成员属性pub_joint_states_
+        self.joint_states_publisher_ = self.create_publisher(JointState,"joint_states", 10) 
+        # 初始化数据
+        self._init_joint_states()
+        self.pub_rate = self.create_rate(30)
+        self.thread_ = threading.Thread(target=self._thread_pub)
+        self.thread_.start()
+
+    
+    def _init_joint_states(self):
+        # 初始左右轮子的速度
+        self.joint_speeds = [0.0,0.0]
+        self.joint_states = JointState()
+        self.joint_states.header.stamp = self.get_clock().now().to_msg()
+        self.joint_states.header.frame_id = ""
+        # 关节名称
+        self.joint_states.name = ['left_wheel_joint','right_wheel_joint']
+        # 关节的位置
+        self.joint_states.position = [0.0,0.0]
+        # 关节速度
+        self.joint_states.velocity = self.joint_speeds
+        # 力 
+        self.joint_states.effort = []
+
+    def update_speed(self,speeds):
+        self.joint_speeds = speeds
+
+    def _thread_pub(self):
+        last_update_time = time.time()
+        while rclpy.ok():
+            delta_time =  time.time()-last_update_time
+            # 更新位置
+            current_position = self.joint_states.position
+            self.joint_states.position[0]  += delta_time*current_position[0]
+            self.joint_states.position[1]  += delta_time*current_position[1]
+            # 更新速度
+            self.joint_states.velocity = self.joint_speeds
+            # 发布关节数据
+            self.joint_states_publisher_.publish(self.joint_states)
+            last_update_time = time.time()
+            self.pub_rate.sleep()
+
+def main(args=None):
+    rclpy.init(args=args) # 初始化rclpy
+    node = RotateWheelNode("rotate_fishbot_wheel")  # 新建一个节点
+    node.update_speed([15.0,-15.0])
+    rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
+    rclpy.shutdown() # 关闭rclpy
+```
+
+
+
+## 4.编译测试
 
 编译程序
 
 ```
-colcon build
+colcon build --packages-select fishbot_description
 ```
 此时运行关节数据发布节点
 
 ```
-ros2 run 
+ros2 run fishbot_description rotate_wheel
 ```
 
 测试之前还需要修改下`display_rviz2.launch.py`文件，注释其`joint_state_publisher`节点
 
 ```
-
+# ld.add_action(joint_state_publisher_node)x
+ld.add_action(robot_state_publisher_node)
+ld.add_action(rviz2_node)
 ```
 
 先运行rviz和robot_state_publisher
@@ -228,12 +344,17 @@ source install/setup.bash
 ros2 launch fishbot_description display_rviz2.launch.py
 ```
 
-观察此时rviz界面
+观察此时rviz界面，可以看到轮子疯狂转动
+
+![image-20220210212327950](./8.4控制移动机器人轮子运动/imgs/image-20220210212327950.png)
 
+## 5.课后练习
 
+尝试将左右轮速度参数化，然后尝试采用rqt动态参数配置工具，实时控制轮子的转速。
 
 
 
+下一节小鱼带你一起，通过编写fishbot的正逆运动学算法。
 
 --------------