From 4d12ba589102f235a7bb0883aaf05f97899c4bdc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E9=B1=BC=E9=A6=99ROS?=
 <87068644+fishros@users.noreply.github.com>
Date: Sun, 17 Dec 2023 13:23:07 +0800
Subject: [PATCH] feat: finish chapt6

---
 .../config/fishbot_ros2_controller.yaml       | 47 ++++++++++++++++++-
 .../launch/gazebo_sim.launch.py               | 30 +++++++++++-
 .../urdf/fishbot/fishbot.ros2_control.xacro   |  4 ++
 3 files changed, 79 insertions(+), 2 deletions(-)

diff --git a/chapt6/chapt6_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml b/chapt6/chapt6_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml
index 66e6adb..8a60aeb 100644
--- a/chapt6/chapt6_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml
+++ b/chapt6/chapt6_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml
@@ -4,4 +4,49 @@ controller_manager:
     use_sim_time: true
     fishbot_joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
-      use_sim_time: true
\ No newline at end of file
+      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
\ No newline at end of file
diff --git a/chapt6/chapt6_ws/src/fishbot_description/launch/gazebo_sim.launch.py b/chapt6/chapt6_ws/src/fishbot_description/launch/gazebo_sim.launch.py
index 415e2ac..16050ca 100644
--- a/chapt6/chapt6_ws/src/fishbot_description/launch/gazebo_sim.launch.py
+++ b/chapt6/chapt6_ws/src/fishbot_description/launch/gazebo_sim.launch.py
@@ -38,10 +38,38 @@ def generate_launch_description():
         executable='spawn_entity.py',
         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],)
+            ),
     ])
diff --git a/chapt6/chapt6_ws/src/fishbot_description/urdf/fishbot/fishbot.ros2_control.xacro b/chapt6/chapt6_ws/src/fishbot_description/urdf/fishbot/fishbot.ros2_control.xacro
index 6a86970..157b5ba 100644
--- a/chapt6/chapt6_ws/src/fishbot_description/urdf/fishbot/fishbot.ros2_control.xacro
+++ b/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>