[feat]:修复URDF轮子bug并添加部分8.4

docs/chapt7/7.2.2动手学空间姿态描述.md

@@ -159,11 +159,12 @@ $$\begin{bmatrix}
 
 
 
-
 大家好，我是小鱼，上一节我们学习了使用numpy表示位置和姿态，并使用了numpy来进行坐标变换，本节课我们直接使用ROS2中的TF工具不写一行代码进行坐标，同时我们还可以使用RVIZ2进行直观的查看坐标关系。
 
+
+
 ## 3.tf2介绍
-TF即`变换`的英文单词`TransForm`的缩写，后面的2是版本编号。所以`ROS`和`ROS2`中的`TF`就是指和坐标变换相关的工具。
+TF即`变换`的英文单词`TransForm`的缩写。所以`ROS`和`ROS2`中的`TF`就是指和坐标变换相关的工具。
 
 > 在搞机器人当中，坐标变换经常用到，所以`ROS2`帮我们做了一个强大易用的TF工具
 
@@ -186,6 +187,38 @@ TF即`变换`的英文单词`TransForm`的缩写，后面的2是版本编号。
 - 子坐标系名称（字符串）
 - 父子之间的变换关系（平移关系和旋转关系）
 
+在终端中输入：
+
+```
+ros2 run tf2_ros static_transform_publisher 
+```
+
+可以看到
+
+```
+A command line utility for manually sending a transform.
+Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id 
+OR 
+Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id 
+```
+
+这是该CLI所提供的使用提示，可以看出
+
+使用TF发布位置和姿态时，位置的描述使用的是xyz三个参数，而姿态的描述则分两种
+
+- 第一种是四元数形式（qx qy qz qw）
+- 第二种是欧拉角形式（yaw偏航角-rz pitch俯仰角-ry roll滚转角-rx），我们这里采用的是欧拉形式，绕x轴旋转采用欧拉角中的滚转角roll来描述，注意角度单位采用弧度制。
+
+![image-20220125112120457](7.2.2动手学空间姿态描述/imgs/image-20220125112120457.png)
+
+>  关于欧拉角和四元数的区别我们放到了姿态的多种表示章节来讲。
+
+
+
+
+
+#### 发布B到C的位姿
+
 比如针对上面的手眼转换，广播机械臂坐标系{B}和相机坐标系{C}之间的关系。
 
 父坐标系的名字就是B,子坐标系的名字是C，父子之间的平移关系是`0 0 3`,旋转关系是绕x轴旋转180度
@@ -199,8 +232,6 @@ ros2 run tf2_ros static_transform_publisher 0 0 3 0 0 3.14 B C
 
 ![发布位姿关系](7.2.2动手学空间姿态描述/imgs/6b0d8be89ab04fbd8d82e9cea3d882c8.png)
 
-使用TF发布位置和姿态时，位置的描述使用的是xyz三个参数，而姿态的描述则分两种，第一种是四元数形式，第二种是欧拉角形式，我们这里采用的是欧拉形式，绕x轴旋转采用欧拉角中的滚转角roll来描述(其余两个为：偏航角与俯仰角)，旋转180度则换算成弧度约为3.14，所以上述的姿态采用的描述为`0,0,3.14`
-
 
 #### 发布C到P的位姿
 
@@ -212,7 +243,10 @@ ros2 run tf2_ros static_transform_publisher 0 0 3 0 0 3.14 B C
 ros2 run tf2_ros static_transform_publisher 2 1 2 0 0 0 C P
 ```
 
+
+
 ### 3.2 可视化坐标变换
+
 打开终端输入rviz2，打开rviz2，我们尝试在rviz2中直观的看到坐标之间的关系
 
 #### 3.2.1 设置默认坐标系
@@ -227,7 +261,10 @@ ros2 run tf2_ros static_transform_publisher 2 1 2 0 0 0 C P
 
 设置完成后，错误也没有了，因为此时的ROS2的TF中确确实实找到了一个叫做B的坐标系。
 
+
+
 #### 3.2.2 添加TF插件
+
 即使没有错误，现在我们还是看不到坐标系，这是为什么呢？在小鱼的动手学ROS2课程当中，小鱼讲过，RVIZ2是一个插件化的软件，所以我们要添加TF相关的插件才能看到TF数据。 
 
 点击左下角的Add，在弹出的窗口中选择TF点击OK
@@ -259,7 +296,11 @@ ros2 run tf2_ros tf2_echo B P
 至于旋转部分采用的是四元数表示，关于这部分姿态的表示，小鱼在第六节会讲，大家不必纠结。
 
 除了使用TF获取关系外，ros2还提供很多工具来查看坐标之间的关系，大家可以在终端中输入下面的命令自行尝试
-#### tf2_monitor
+
+#### 其他工具
+
+##### tf2_monitor
+
 查看所有的发布者和频率。
 ```
 ros2 run tf2_ros tf2_monitor 
@@ -284,7 +325,7 @@ Node: <no authority available> 5029.14 Hz, Average Delay: 1871.74 Max Delay: 300
 
 ```
 
-#### view_frames.py
+##### view_frames.py
 可以生成TF的pdf，目前也有在线的实时查看工具。
 ```
 ros2 run tf2_tools view_frames.py 

docs/chapt8/8.3动手创建一个移动机器人.md

@@ -65,11 +65,11 @@ IMU传感器和透明度与颜色修改是上节课，作业，小鱼先带你
   </joint>
 ```
 
-## 2.添加左轮
+## 2.添加右轮
 
 ### 2.1 添加关节
 
-关节名称为`left_wheel_link`,小鱼在做ros2小车的时候采用的轮子如下图：
+关节名称为`right_wheel_link`,小鱼在做ros2小车的时候采用的轮子如下图：
 
 ![image-20220117193835801](8.3动手创建一个移动机器人/imgs/image-20220117193835801.png)
 
@@ -94,7 +94,7 @@ IMU传感器和透明度与颜色修改是上节课，作业，小鱼先带你
 颜色换黑色，可以得到下面的配置：
 
 ```xml
-  <link name="left_wheel_link">
+  <link name="right_wheel_link">
       <visual>
         <origin xyz="0 0 0" rpy="1.57079 0 0"/>
         <geometry>
@@ -119,10 +119,10 @@ IMU传感器和透明度与颜色修改是上节课，作业，小鱼先带你
 
 - z表示child相对parent的z轴上的关系，想将轮子固定在机器人的下表面,所以`origin`的z向下偏移0.12/2=0.06m(向下符号为负)
 
-- y表示child相对parent的y轴上的关系，base_link的半径是0.10,所以我们让轮子的y轴向正方向偏移0.10m(向左符号为正)
+- y表示child相对parent的y轴上的关系，base_link的半径是0.10,所以我们让轮子的y轴向负方向偏移0.10m(向左符号为负)
 - x表示child相对parent的x轴上的关系，向后偏移则是x轴向后进行偏移，我们用个差不多的值0.02m(向后符号为负)
 
-![image-20220117005420889](8.3动手创建一个移动机器人/imgs/image-20220117005420889.png)
+![image-20220125114933059](8.3动手创建一个移动机器人/imgs/image-20220125114933059.png)
 
 **再看axis**
 
@@ -133,22 +133,22 @@ IMU传感器和透明度与颜色修改是上节课，作业，小鱼先带你
 ```
 
 ```xml
-  <joint name="left_wheel_joint" type="continuous">
+  <joint name="right_wheel_joint" type="continuous">
       <parent link="base_link" />
-      <child link="left_wheel_link" />
-      <origin xyz="-0.02 0.10 -0.06" />
+      <child link="right_wheel_link" />
+      <origin xyz="-0.02 -0.10 -0.06" />
       <axis xyz="0 1 0" />
   </joint>
 ```
 
 
 
-### 3.添加右轮
+### 3.添加左轮
 
-右轮就是左轮的映射，不再赘述
+左轮就是右轮的映射，不再赘述
 
 ```xml
-  <link name="right_wheel_link">
+  <link name="left_wheel_link">
       <visual>
         <origin xyz="0 0 0" rpy="1.57079 0 0"/>
         <geometry>
@@ -160,13 +160,12 @@ IMU传感器和透明度与颜色修改是上节课，作业，小鱼先带你
       </visual>
   </link>
     
-  <joint name="right_wheel_joint" type="continuous">
+  <joint name="left_wheel_joint" type="continuous">
       <parent link="base_link" />
-      <child link="right_wheel_link" />
-      <origin xyz="-0.02 -0.10 -0.06" />
+      <child link="left_wheel_link" />
+      <origin xyz="-0.02 0.10 -0.06" />
       <axis xyz="0 1 0" />
   </joint>
-
 ```
 
 
@@ -199,6 +198,12 @@ IMU传感器和透明度与颜色修改是上节课，作业，小鱼先带你
 
 > 最终URDF文件：https://raw.githubusercontent.com/fishros/fishbot/master/src/fishbot_description/urdf/fishbot_base.urdf
 
+
+
+
+
+
+
 ## 5.测试运行
 
 ### 5.1 编译测试
@@ -239,6 +244,8 @@ ros2 topic echo /joint_states
 
 ![image-20220117012745344](8.3动手创建一个移动机器人/imgs/image-20220117012745344.png)
 
+
+
 ### 5.3 通过joint_state_gui改变关节tf中关节角度
 
 在JointStatePublisher中,拖动滑动条,观察

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

@@ -22,17 +22,57 @@
 
 方便起见，我们就在`fishbot_describle`包中新建节点（参考李四节点代码）
 
+```shell
+cd fishbot_ws
+touch fishbot_description/fishbot_description/rotate_wheel.py
 ```
 
-```
+```python
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+
+
+class RotateWheelNode(Node):
+    def __init__(self,name):
+        super().__init__(name)
+        self.get_logger().info("rotate node init.." % name)
 
-配置以下setup.py
 
+def main(args=None):
+    """
+    ros2运行该节点的入口函数
+    1. 导入库文件
+    2. 初始化客户端库
+    3. 新建节点
+    4. spin循环节点
+    5. 关闭客户端库
+    """
+    rclpy.init(args=args) # 初始化rclpy
+    node = RotateWheelNode("rotate_fishbot_wheel")  # 新建一个节点
+    rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
+    rclpy.shutdown() # 关闭rclpy
 ```
 
+配置下setup.py
+
+```
+    entry_points={
+        'console_scripts': [
+            "rotate_wheel= fishbot_description.rotate_wheel:main"
+        ],
+    },
 ```
 
+编译运行
+
+```
+colcon build
+source install/setup.bash
+ros2 run fishbot_description rotate_wheel
+```
 
+![image-20220125120447619](8.4控制移动机器人轮子运动/imgs/image-20220125120447619.png)
 
 ## 2.创建发布者
 
@@ -44,45 +84,117 @@
 ros2 topic info /joint_states
 ```
 
+```
+Type: sensor_msgs/msg/JointState
+Publisher count: 1
+Subscription count: 1
+```
+
 接着
 
 ```
-ros2 interfaces show 
+ros2 interfaces show sensor_msgs/msg/JointState
+```
+
 ```
+# This is a message that holds data to describe the state of a set of torque controlled joints.
+#
+# The state of each joint (revolute or prismatic) is defined by:
+#  * the position of the joint (rad or m),
+#  * the velocity of the joint (rad/s or m/s) and
+#  * the effort that is applied in the joint (Nm or N).
+#
+# Each joint is uniquely identified by its name
+# The header specifies the time at which the joint states were recorded. All the joint states
+# in one message have to be recorded at the same time.
+#
+# This message consists of a multiple arrays, one for each part of the joint state.
+# The goal is to make each of the fields optional. When e.g. your joints have no
+# effort associated with them, you can leave the effort array empty.
+#
+# All arrays in this message should have the same size, or be empty.
+# This is the only way to uniquely associate the joint name with the correct
+# states.
 
-知道了话题类型，我们就可以来创建发布者了
+std_msgs/Header header
 
+string[] name
+float64[] position
+float64[] velocity
+float64[] effort
 ```
 
+知道了话题类型，我们就可以来创建发布者了参考代码[4.2.1 话题通信实现(Python)](https://fishros.com/d2lros2foxy/#/chapt4/4.2话题通信实现(Python))
+
+```python
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+# 1.导入消息类型JointState
+from sensor_msgs.msg import JointState
+
+class RotateWheelNode(Node):
+    def __init__(self,name):
+        super().__init__(name)
+        self.get_logger().info(f"node {name} init..")
+        # 2.创建并初始化发布者成员属性pub_joint_states_
+        self.pub_joint_states_ = self.create_publisher(JointState,"joint_states", 10) 
 ```
 
 
 
 ## 3.编写发布逻辑
 
-创建好发布者，我们想让话题按照某个固定的速度进行发布，可以采用ROS2中的定时神器Rate,不清楚Rate的小伙伴可以看看小鱼的这篇文章：
+创建好发布者，我们想让话题按照某个固定的速度进行发布，可以采用ROS2中的定时神器Rate,不清楚Rate的小伙伴可以看看小鱼的这篇文章：[ROS中的定频神器你会用吗](https://mp.weixin.qq.com/s/bFbTIh6rGou1k0Ach-hlqA)
+
+为了能够一直循环使用rate，我们单独开一个线程用于发布joint_states话题数据
+
+
 
 接着我们来构造发布的数据：
 
-joint_states有
+joint_states有一个头和四个数组需要赋值
 
 ```
-
+std_msgs/Header header #时间戳信息
+string[] name
+float64[] position
+float64[] velocity
+float64[] effort
 ```
 
 对应的含义为：
 
 ```
-
+# 这是一个持有数据的信息，用于描述一组扭矩控制的关节的状态。
+#
+# 每个关节（渐进式或棱柱式）的状态由以下因素定义。
+# #关节的位置（rad或m）。
+# #关节的速度（弧度/秒或米/秒）和
+# #在关节上施加的力（Nm或N）。
+#
+# 每个关节都由其名称来唯一标识
+# 头部规定了记录关节状态的时间。所有的联合状态
+# 必须是在同一时间记录的。
+#
+# 这个消息由多个数组组成，每个部分的联合状态都有一个数组。
+# 目标是让每个字段都是可选的。例如，当你的关节没有
+# 扭矩与它们相关，你可以让扭矩数组为空。
+#
+# 这个信息中的所有数组都应该有相同的大小，或者为空。
+# 这是唯一能将关节名称与正确的
+# 状态。
+string[] name #关节名称数组
+float64[] position #关节位置数组
+float64[] velocity #关节速度数组
+float64[] effort #扭矩数据
 ```
 
 >  单个轮子的转速 = (当前的位置-上一时刻位置）/ 两个时间之间的间隔
 
 所以最终有如下代码：
 
-```
 
-```