From f52745536da6e4b9381f7950a05c9291a9d86363 Mon Sep 17 00:00:00 2001
From: harleylara <harley.lara@outlook.com>
Date: Sun, 16 Feb 2025 19:23:43 +0100
Subject: [PATCH] add working demo diff drive

---
 foxy_bringup/config/controllers.yaml  | 53 +++++++++++++++++++++++++++
 foxy_bringup/config/default.rviz      | 16 +++++---
 foxy_bringup/launch/robot.launch.py   | 21 ++++++-----
 foxy_description/urdf/base.xacro      | 21 +++++++----
 foxy_description/urdf/foxy.urdf.xacro |  6 +--
 5 files changed, 92 insertions(+), 25 deletions(-)

diff --git a/foxy_bringup/config/controllers.yaml b/foxy_bringup/config/controllers.yaml
index 2042ffd..b8226d9 100644
--- a/foxy_bringup/config/controllers.yaml
+++ b/foxy_bringup/config/controllers.yaml
@@ -4,3 +4,56 @@
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
+
+    diff_drive_base_controller:
+      type: diff_drive_controller/DiffDriveController
+
+/**/diff_drive_base_controller:
+  ros__parameters:
+    left_wheel_names: ["left_wheel_joint"]
+    right_wheel_names: ["right_wheel_joint"]
+
+    wheel_separation: 0.45
+    #wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
+    wheel_radius: 0.05
+
+    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_link
+    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: false
+    enable_odom_tf: true
+
+    cmd_vel_timeout: 0.5
+    #publish_limited_velocity: true
+    use_stamped_vel: true
+    #velocity_rolling_window_size: 10
+
+    # Velocity and acceleration limits
+
+    # Whenever a min_* is unspecified, default to -max_*
+    linear.x.has_velocity_limits: true
+    linear.x.has_acceleration_limits: true
+    linear.x.has_jerk_limits: false
+    linear.x.max_velocity: 1.0
+    linear.x.min_velocity: -1.0
+    linear.x.max_acceleration: 1.0
+    linear.x.max_jerk: 0.0
+    linear.x.min_jerk: 0.0
+
+    angular.z.has_velocity_limits: true
+    angular.z.has_acceleration_limits: true
+    angular.z.has_jerk_limits: false
+    angular.z.max_velocity: 1.0
+    angular.z.min_velocity: -1.0
+    angular.z.max_acceleration: 1.0
+    angular.z.min_acceleration: -1.0
+    angular.z.max_jerk: 0.0
+    angular.z.min_jerk: 0.0
diff --git a/foxy_bringup/config/default.rviz b/foxy_bringup/config/default.rviz
index 20f2a9a..c599eaf 100644
--- a/foxy_bringup/config/default.rviz
+++ b/foxy_bringup/config/default.rviz
@@ -75,7 +75,7 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        foxy_base_link:
+        foxy/base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -107,7 +107,9 @@ Visualization Manager:
           Value: true
         caster_wheel:
           Value: true
-        foxy_base_link:
+        foxy/base_link:
+          Value: true
+        foxy/odom:
           Value: true
         left_wheel:
           Value: true
@@ -119,7 +121,7 @@ Visualization Manager:
       Show Axes: true
       Show Names: false
       Tree:
-        foxy_base_link:
+        foxy/base_link:
           base_footprint:
             {}
           caster_wheel:
@@ -128,12 +130,14 @@ Visualization Manager:
             {}
           right_wheel:
             {}
+        foxy/odom:
+          {}
       Update Interval: 0
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: foxy_base_link
+    Fixed Frame: foxy/odom
     Frame Rate: 30
   Name: root
   Tools:
@@ -212,5 +216,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1349
-  X: 368
-  Y: 66
+  X: 304
+  Y: 2
diff --git a/foxy_bringup/launch/robot.launch.py b/foxy_bringup/launch/robot.launch.py
index 57c0b6c..6437f27 100644
--- a/foxy_bringup/launch/robot.launch.py
+++ b/foxy_bringup/launch/robot.launch.py
@@ -11,15 +11,8 @@ def launch_args(context) -> list[LaunchDescriptionEntity]:
 
     declared_args = []
 
-    declared_args.append(DeclareLaunchArgument(
-        "use_sim",
-        default_value="true",
-        description="Clock source."
-    ))
-
     declared_args.append(DeclareLaunchArgument(
         "robot_name",
-        default_value="foxy",
         description="Robot name."
     ))
 
@@ -74,7 +67,8 @@ def launch_setup(context) -> list[LaunchDescriptionEntity]:
             Node(
                 package="controller_manager",
                 executable="spawner",
-                arguments=["joint_state_broadcaster"],
+                arguments=["joint_state_broadcaster", "diff_drive_base_controller"],
+                output='screen'
             ),
         ]
     )
@@ -101,12 +95,20 @@ def launch_setup(context) -> list[LaunchDescriptionEntity]:
                     "-x", "0",
                     "-y", "0",
                     "-z", "0.3"
-                ]
+                ],
+                output='screen'
             ),
         ],
         condition=LaunchConfigurationEquals("system", "gz")
     )
 
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
+        output='screen'
+    )
+
     rviz2 = Node(
         package='rviz2',
         executable='rviz2',
@@ -121,6 +123,7 @@ def launch_setup(context) -> list[LaunchDescriptionEntity]:
         robot_state_publisher_node,
         controllers,
         gz,
+        bridge,
         rviz2
     ]
 
diff --git a/foxy_description/urdf/base.xacro b/foxy_description/urdf/base.xacro
index add14b2..02dbfb8 100644
--- a/foxy_description/urdf/base.xacro
+++ b/foxy_description/urdf/base.xacro
@@ -18,13 +18,14 @@
 
     <link name="${root_frame}">
       <visual>
+        <origin xyz="0 0 0.1" rpy="0 0 0" />
         <geometry>
           <box size="0.6 0.4 0.2" />
         </geometry>
-        <origin xyz="0 0 0.1" rpy="0 0 0" />
         <material name="green" />
       </visual>
       <collision>
+        <origin xyz="0 0 0.1" rpy="0 0 0" />
         <geometry>
           <box size="0.6 0.4 0.2" />
         </geometry>
@@ -39,18 +40,20 @@
 
     <link name="left_wheel">
       <visual>
+        <origin xyz="0 0 0" rpy="1.57 0 0" />
         <geometry>
           <cylinder radius="0.1" length="0.05" />
         </geometry>
-        <origin xyz="0 0 0" rpy="1.57 0 0" />
         <material name="grey" />
       </visual>
       <collision>
+        <origin xyz="0 0 0" rpy="1.57 0 0" />
         <geometry>
           <cylinder radius="0.1" length="0.05" />
         </geometry>
       </collision>
       <inertial>
+        <origin xyz="0 0 0" rpy="1.57 0 0" />
         <mass value="1"/>
         <inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
       </inertial>
@@ -58,18 +61,20 @@
 
     <link name="right_wheel">
       <visual>
+        <origin xyz="0 0 0" rpy="1.57 0 0" />
         <geometry>
           <cylinder radius="0.1" length="0.05" />
         </geometry>
-        <origin xyz="0 0 0" rpy="1.57 0 0" />
         <material name="grey" />
       </visual>
       <collision>
+        <origin xyz="0 0 0" rpy="1.57 0 0" />
         <geometry>
           <cylinder radius="0.1" length="0.05" />
         </geometry>
       </collision>
       <inertial>
+        <origin xyz="0 0 0" rpy="1.57 0 0" />
         <mass value="1"/>
         <inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
       </inertial>
@@ -94,18 +99,20 @@
       </inertial>
     </link>
 
-    <joint name="base_left_wheel_joint" type="continuous">
+    <joint name="left_wheel_joint" type="continuous">
       <parent link="${root_frame}" />
       <child link="left_wheel" />
-      <origin xyz="-0.15 0.225 0" rpy="0 0 0" />
+      <origin xyz="-0.15 0.25 0" rpy="0 0 0" />
       <axis xyz="0 1 0"/>
+      <dynamics damping="0.2"/>
     </joint>
 
-    <joint name="base_right_wheel_joint" type="continuous">
+    <joint name="right_wheel_joint" type="continuous">
       <parent link="${root_frame}" />
       <child link="right_wheel" />
-      <origin xyz="-0.15 -0.225 0" rpy="0 0 0" />
+      <origin xyz="-0.15 -0.25 0" rpy="0 0 0" />
       <axis xyz="0 1 0"/>
+      <dynamics damping="0.2"/>
     </joint>
 
     <joint name="base_caster_wheel_joint" type="fixed">
diff --git a/foxy_description/urdf/foxy.urdf.xacro b/foxy_description/urdf/foxy.urdf.xacro
index ac0a21a..912b67d 100644
--- a/foxy_description/urdf/foxy.urdf.xacro
+++ b/foxy_description/urdf/foxy.urdf.xacro
@@ -7,7 +7,7 @@
 
   <!-- set of properties for internal use (not used by launch) -->
   <xacro:property name="namespace" default="$(arg robot_name)"/>
-  <xacro:property name="root_frame" default="${namespace}_base_link" />
+  <xacro:property name="root_frame" default="${namespace}/base_link" />
   <xacro:property name="ros2_gz_before_renamed" value="${['humble', 'galatic', 'foxy']}"/>
 
   <xacro:include filename="$(find foxy_description)/urdf/base.xacro"/>
@@ -20,7 +20,7 @@
       <hardware>
         <plugin>ign_ros2_control/IgnitionSystem</plugin>
       </hardware>
-      <joint name="base_left_wheel_joint">
+      <joint name="left_wheel_joint">
         <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>
@@ -28,7 +28,7 @@
         <state_interface name="position"/>
         <state_interface name="velocity"/>
       </joint>
-      <joint name="base_right_wheel_joint">
+      <joint name="right_wheel_joint">
         <command_interface name="velocity">
           <param name="min">-1</param>
           <param name="max">1</param>