sim with small drift

src/sherpa/config/ackermann_controller.yaml

@@ -0,0 +1,34 @@
+
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+
+    ackermann_controller:
+      type: ackermann_steering_controller/AckermannSteeringController
+  
+    joint_state_broadcaster:
+        type: "joint_state_broadcaster/JointStateBroadcaster"
+
+
+ackermann_controller:  
+  ros__parameters:
+    position_feedback: false
+
+    # Interfaces
+    rear_wheels_names: ["wheel_bl_joint", "wheel_br_joint"]
+    front_wheels_names:  ["steer_fl_joint", "steer_fr_joint"]
+
+    # Physical parameters
+    wheelbase: 0.6                # Distance between front and rear axles [m]
+    front_wheel_track: 0.4        # Distance between front wheels [m]
+    rear_wheel_track: 0.5         # Distance between rear wheels [m]
+    front_wheels_radius: 0.15  # Radius of wheels [m]
+    rear_wheels_radius: 0.15  # Radius of wheels [m]
+
+    # Limits
+    max_steering_angle: 0.785  # [rad] ~45 degrees
+    max_speed: 10.0            # [m/s]
+    min_speed: -5.0            # [m/s]
+
+    # Frame config
+    reference_frame: base_link

src/sherpa/launch/simulator.launch.py

@@ -49,7 +49,7 @@ def generate_launch_description():
     controller = Node(
         package='controller_manager',
         executable='spawner',
-        arguments=['mecanum_controller'],
+        arguments=['ackermann_controller'],
         output='screen'
     )
 
@@ -89,7 +89,7 @@ def generate_launch_description():
         arguments=[
             "--ros-args",
             "-p",
-            f"config_file:={os.path.join(get_package_share_directory('sherpa'), 'config', 'gz_bridge.yaml')}",
+            f"config_file:={os.path.join(pkg_share, 'config', 'gz_bridge.yaml')}",
         ]
     )
 

src/sherpa/urdf/components/wheel.xacro

@@ -0,0 +1,84 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <material name="wheel_rim_material">
+        <ambient>0.05 0.05 0.2 05</ambient>
+        <diffuse>0.9 0.9 0.9 1</diffuse>
+        <specular>0.1 0.1 0.1 1</specular>
+        <emissive>0 0 0 0</emissive>
+        <color rgba="0.025 0.025 0.025 1"/> 
+    </material> 
+
+    <material name="wheel_spoke_material">
+        <ambient>0.2 0.2 0.2 1</ambient>
+        <diffuse>0.25 0.25 0.25 1</diffuse>
+        <specular>0.2 0.2 0.2 1</specular>
+        <emissive>0 0 0 0 1</emissive>
+        <color rgba="0.8 0.0 0.0 1"/> 
+    </material>
+
+    <xacro:macro name="wheel" params="name parent xyz rpy radius mass">
+
+        <xacro:property name="mesh_scale" value="${0.0078 * radius}"/>
+
+        <link name='${name}_link'>
+            <inertial>
+                <mass value="${mass}"/>
+                <inertia 
+                    ixx="${(13.0/48.0) * mass * radius * radius}"
+                    ixy="0.0"
+                    ixz="0.0"
+                    iyy="${0.5 * mass * radius * radius}"
+                    iyz="0.0"
+                    izz="${(13.0/48.0) * mass * radius * radius}"
+                />
+            </inertial>
+            <visual name='visual'>
+                <geometry>
+                    <mesh
+                        filename="package://sherpa/urdf/meshes/wheel_cape.stl"
+                        scale="${mesh_scale} ${mesh_scale} ${mesh_scale}"
+                    />
+                </geometry>
+                <material name="wheel_rim_material"/>
+            </visual>
+            <visual name='visual'>
+                <geometry>
+                    <mesh
+                        filename="package://sherpa/urdf/meshes/wheel_spoke.stl"
+                        scale="${mesh_scale} ${mesh_scale} ${mesh_scale}"
+                    />
+                </geometry>
+                <material name="wheel_spoke_material"/>
+            </visual>
+            <collision name='collision'>
+                <origin xyz="0 0 0" rpy="-1.5707 0 0"/>
+                <geometry>
+                    <cylinder radius="${radius}" length="${radius/1.5}"/>
+                </geometry>
+            </collision>
+        </link>
+        <xacro:element xacro:name="gazebo" reference="${name}_link">
+            <surface>
+                <friction>
+                    <ode>
+                        <mu>1.5</mu>
+                        <mu2>1.2</mu2>
+                    </ode>
+                </friction>
+                <soft_cfm>0.01</soft_cfm>  <!-- Soft contact to prevent rigid collisions -->
+                <soft_erp>0.1</soft_erp>   <!-- Error reduction parameter (makes contact softer) -->
+            </surface>
+        </xacro:element>
+        <joint name='${name}_joint' type='continuous'>
+            <parent link="${parent}_link"/>
+            <child link="${name}_link"/>
+            <axis xyz="0 1 0"/>
+            <limit 
+                velocity="35"
+                effort="350"
+            />
+            <origin xyz="${xyz}" rpy="${rpy}"/>
+        </joint>
+    </xacro:macro>
+</robot>

src/sherpa/urdf/sherpa.xacro

@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sherpa">  
     <gazebo>
         <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system.so">
-            <parameters>$(find sherpa)/config/mecanum_controller.yaml</parameters>
+            <parameters>$(find sherpa)/config/ackermann_controller.yaml</parameters>
         </plugin>
     </gazebo>
 
@@ -21,34 +21,66 @@
     </link>
 
     <xacro:include filename="components/imu.xacro"/>
-    <xacro:include filename="components/mecanum_wheel.xacro"/>
+    <xacro:include filename="components/wheel.xacro"/>
     <xacro:include filename="components/ouster_lidar.xacro"/>
 
     <xacro:imu name="imu" parent="base" xyz="0 0 0.156" rpy="0 0 0" frequency="300"/>
     <xacro:ouster_lidar name="lidar" parent="base" xyz="0 0 0.15" rpy="0 0 0" model="vlp16" frequency="10" topic="/points"/>
 
-    <xacro:mecanum_wheel name="wheel_fr" parent="base" xyz=" 0.31 -0.31 0" rpy="0 0 0" direction="r" radius="0.1" mass="10"/>
+    <link name="steer_fl_link">
-    <xacro:mecanum_wheel name="wheel_fl" parent="base" xyz=" 0.31  0.31 0" rpy="0 0 0" direction="l" radius="0.1" mass="10"/>
+        <inertial>
-    <xacro:mecanum_wheel name="wheel_br" parent="base" xyz="-0.31 -0.31 0" rpy="0 0 0" direction="l" radius="0.1" mass="10"/>
+            <mass value="0.1"/>
-    <xacro:mecanum_wheel name="wheel_bl" parent="base" xyz="-0.31  0.31 0" rpy="0 0 0" direction="r" radius="0.1" mass="10"/>
+            <inertia 
+                ixx="0.0"
+                ixy="0.0"
+                ixz="0.0"
+                iyy="0.0"
+                iyz="0.0"
+                izz="0.0"
+            />
+        </inertial>
+    </link>
+    <link name="steer_fr_link">
+    <inertial>
+            <mass value="0.1"/>
+            <inertia 
+                ixx="0.0"
+                ixy="0.0"
+                ixz="0.0"
+                iyy="0.0"
+                iyz="0.0"
+                izz="0.0"
+            />
+        </inertial>
+    </link>
+    
+    <xacro:wheel name="wheel_fl" parent="steer_fl" xyz="0  0.05 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
+    <xacro:wheel name="wheel_fr" parent="steer_fr" xyz="0 -0.05 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
+    <xacro:wheel name="wheel_bl" parent="base" xyz="-0.3  0.25 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
+    <xacro:wheel name="wheel_br" parent="base" xyz="-0.3 -0.25 0" rpy="0 0 0" radius="0.15" mass="2.5"/>    
+
+    <joint name="steer_fl_joint" type="revolute">
+        <parent link="base_link"/>
+        <child link="steer_fl_link"/>
+        <origin xyz="0.3 0.20 0" rpy="0 0 0"/>
+        <axis xyz="0 0 1"/>
+        <limit upper="0.3" lower="-0.3" effort="-1" velocity="-1"/>
+    </joint>
+
+    <joint name="steer_fr_joint" type="revolute">
+        <parent link="base_link"/>
+        <child link="steer_fr_link"/>
+        <origin xyz="0.3 -0.20 0" rpy="0 0 0"/>
+        <axis xyz="0 0 1"/>
+        <limit upper="0.3" lower="-0.3" effort="-1" velocity="-1"/>
+    </joint>
+
     
     <ros2_control name="SherpaSystem" type="system">
         <hardware>
             <plugin>gz_ros2_control/GazeboSimSystem</plugin>
         </hardware>
     
-        <joint name="wheel_fl_joint">
-            <command_interface name="velocity"/>
-            <state_interface name="velocity"/>
-            <state_interface name="position"/>
-        </joint>
-
-        <joint name="wheel_fr_joint">
-            <command_interface name="velocity"/>
-            <state_interface name="velocity"/>
-            <state_interface name="position"/>
-        </joint>
-
         <joint name="wheel_bl_joint">
             <command_interface name="velocity"/>
             <state_interface name="velocity"/>
@@ -60,5 +92,15 @@
             <state_interface name="velocity"/>
             <state_interface name="position"/>
         </joint>
+
+        <joint name="steer_fl_joint">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+        </joint>
+
+        <joint name="steer_fr_joint">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+        </joint>
     </ros2_control>
 </robot>

src/sherpa/worlds/empty.world

@@ -36,7 +36,7 @@
         <linear>0.05</linear>
         <quadratic>0.01</quadratic>
       </attenuation>
-      <cast_shadows>true</cast_shadows>
+      <cast_shadows>false</cast_shadows>
     </light>
 
     <model name="ground_plane">