feat: finish chapt6

chapt6/chapt6_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml

@@ -5,3 +5,48 @@ controller_manager:
     fishbot_joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
       use_sim_time: true
+    fishbot_effort_controller:
+      type: effort_controllers/JointGroupEffortController
+    fishbot_diff_drive_controller:
+      type: diff_drive_controller/DiffDriveController
+
+fishbot_effort_controller:
+  ros__parameters:
+    joints:
+      - left_wheel_joint
+      - right_wheel_joint
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+      - effort
+
+
+
+fishbot_diff_drive_controller:
+  ros__parameters:
+    left_wheel_names: ["left_wheel_joint"]
+    right_wheel_names: ["right_wheel_joint"]
+
+    wheel_separation: 0.17
+    #wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
+    wheel_radius: 0.032
+
+    wheel_separation_multiplier: 1.0
+    left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
+
+    publish_rate: 50.0
+    odom_frame_id: odom
+    base_frame_id: base_footprint
+    pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
+    twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]
+
+    open_loop: true
+    enable_odom_tf: true
+
+    cmd_vel_timeout: 0.5
+    #publish_limited_velocity: true
+    use_stamped_vel: false
+    #velocity_rolling_window_size: 10

chapt6/chapt6_ws/src/fishbot_description/launch/gazebo_sim.launch.py

@@ -39,9 +39,37 @@ def generate_launch_description():
         arguments=['-topic', '/robot_description',
                    '-entity', robot_name_in_model, ])
     
+    # 加载并激活 fishbot_joint_state_broadcaster 控制器
+    load_joint_state_controller = launch.actions.ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+            'fishbot_joint_state_broadcaster'],
+        output='screen'
+    )
+
+    # 加载并激活 fishbot_effort_controller 控制器
+    load_fishbot_effort_controller = launch.actions.ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_effort_controller'], 
+        output='screen')
+    
+    load_fishbot_diff_drive_controller = launch.actions.ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_diff_drive_controller'], 
+        output='screen')
+    
     return launch.LaunchDescription([
         action_declare_arg_mode_path,
         robot_state_publisher_node,
         launch_gazebo,
-        spawn_entity_node
+        spawn_entity_node,
+        # 事件动作，当加载机器人结束后执行    
+        launch.actions.RegisterEventHandler(
+            event_handler=launch.event_handlers.OnProcessExit(
+                target_action=spawn_entity_node,
+                on_exit=[load_joint_state_controller],)
+            ),
+        # 事件动作，load_fishbot_diff_drive_controller
+        launch.actions.RegisterEventHandler(
+        event_handler=launch.event_handlers.OnProcessExit(
+            target_action=load_joint_state_controller,
+            on_exit=[load_fishbot_diff_drive_controller],)
+            ),
     ])

chapt6/chapt6_ws/src/fishbot_description/urdf/fishbot/fishbot.ros2_control.xacro

@@ -35,6 +35,10 @@
         <gazebo>
         <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
             <parameters>$(find fishbot_description)/config/fishbot_ros2_controller.yaml</parameters>
+            <ros>
+                <remapping>/fishbot_diff_drive_controller/cmd_vel_unstamped:=/cmd_vel</remapping>
+                <remapping>/fishbot_diff_drive_controller/odom:=/odom</remapping>
+            </ros>
         </plugin>
 </gazebo>
     </xacro:macro>