From 2b450a6d8647bf56e1b70ffe20a72ccf2baba6b9 Mon Sep 17 00:00:00 2001
From: Timo <timoschneider04@gmail.com>
Date: Fri, 18 Apr 2025 11:19:26 +0200
Subject: [PATCH] wip: testing

---
 src/sherpa/config/fastlio2_vlp16.yaml         |  4 +--
 src/sherpa/launch/simulator.launch.py         | 10 ------
 .../urdf/components/mecanum_wheel.xacro       | 34 ++++++++++---------
 src/sherpa/urdf/sherpa.xacro                  |  8 ++---
 src/sherpa/worlds/empty.world                 | 28 +++++++++++----
 5 files changed, 46 insertions(+), 38 deletions(-)

diff --git a/src/sherpa/config/fastlio2_vlp16.yaml b/src/sherpa/config/fastlio2_vlp16.yaml
index 24cf92e..dc837f5 100644
--- a/src/sherpa/config/fastlio2_vlp16.yaml
+++ b/src/sherpa/config/fastlio2_vlp16.yaml
@@ -21,7 +21,7 @@
             scan_line: 16
             scan_rate: 10                # only need to be set for velodyne, unit: Hz,
             timestamp_unit: 3            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
-            blind: 1.05
+            blind: 0.7
 
         mapping:
             acc_cov: 0.01
@@ -30,7 +30,7 @@
             b_gyr_cov: 0.00025
             fov_degree:    360.0
             det_range:     100.0
-            extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic,
+            extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
             extrinsic_T: [ 0., 0., -0.05]
             extrinsic_R: [ 1., 0., 0., 
                         0., 1., 0., 
diff --git a/src/sherpa/launch/simulator.launch.py b/src/sherpa/launch/simulator.launch.py
index ef0239a..9f5796d 100644
--- a/src/sherpa/launch/simulator.launch.py
+++ b/src/sherpa/launch/simulator.launch.py
@@ -46,15 +46,6 @@ def generate_launch_description():
         }]
     )
 
-    # Load controller manager
-    #control_manager = Node(
-    #    package='controller_manager',
-    #    executable='ros2_control_node',
-    #    parameters=[{'use_sim_time': True}, {'robot_description': '/robot_description'}],
-    #    output='screen'
-    #)
-
-    # Spawner for the mecanum controller
     controller = Node(
         package='controller_manager',
         executable='spawner',
@@ -117,7 +108,6 @@ def generate_launch_description():
     ld.add_action(gz_bridge)
     ld.add_action(joint_state_broadcaster)
     ld.add_action(controller)
-    #ld.add_action(control_manager)
     ld.add_action(rviz2)
 
     return ld
diff --git a/src/sherpa/urdf/components/mecanum_wheel.xacro b/src/sherpa/urdf/components/mecanum_wheel.xacro
index 58f5523..abc811e 100644
--- a/src/sherpa/urdf/components/mecanum_wheel.xacro
+++ b/src/sherpa/urdf/components/mecanum_wheel.xacro
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:gz="http://gazebosim.org">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
     <material name="mecanum_wheel_material">
         <ambient>0.2 0.2 0.2 1</ambient>
@@ -27,16 +27,15 @@
             <inertial>
                 <mass value="${mass}"/>
                 <inertia 
-                    ixx="${0.5 * mass * radius * radius}"
+                    ixx="${(13.0/48.0) * mass * radius * radius}"
                     ixy="0.0"
                     ixz="0.0"
                     iyy="${0.5 * mass * radius * radius}"
                     iyz="0.0"
-                    izz="${0.25 * mass * radius * radius}"
+                    izz="${(13.0/48.0) * mass * radius * radius}"
                 />
             </inertial>
             <visual name='visual'>
-                <pose>0 0 0 0 0 0</pose>
                 <geometry>
                     <mesh
                         filename="${mesh}"
@@ -46,27 +45,30 @@
                 <material name="mecanum_wheel_material"/>
             </visual>
             <collision name='collision'>
+                <origin xyz="0 0 0" rpy="-1.5707 0 0"/>
                 <geometry>
-                    <sphere radius="${radius}"/>
+                    <cylinder radius="${radius}" length="${radius}"/>
                 </geometry>
-                <surface>
-                    <friction>
-                    <ode>
-                        <mu>1.2</mu>
-                        <mu2>0</mu2>
-                        <fdir1 gz:expressed_in="${parent}_link">1 ${friction_dir} 0</fdir1>
-                    </ode>
-                    </friction>
-                </surface>
             </collision>
         </link>
+        <xacro:element xacro:name="gazebo" reference="${name}_link">
+            <surface>
+                <friction>
+                <ode>
+                    <mu>1.0</mu>
+                    <mu2>0.0</mu2>
+                    <fdir1>1 ${friction_dir} 0</fdir1>
+                </ode>
+                </friction>
+            </surface>
+        </xacro:element>
         <joint name='${name}_joint' type='continuous'>
             <parent link="${parent}_link"/>
             <child link="${name}_link"/>
             <axis xyz="0 1 0"/>
             <limit 
-                velocity="5"
-                effort="100000"
+                velocity="35"
+                effort="350"
             />
             <origin xyz="${xyz}" rpy="${rpy}"/>
         </joint>
diff --git a/src/sherpa/urdf/sherpa.xacro b/src/sherpa/urdf/sherpa.xacro
index eb3d166..0727abe 100644
--- a/src/sherpa/urdf/sherpa.xacro
+++ b/src/sherpa/urdf/sherpa.xacro
@@ -27,10 +27,10 @@
     <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.11 -0.11 0" rpy="0 0 0" direction="r" radius="0.04" mass="0.25"/>
-    <xacro:mecanum_wheel name="wheel_fl" parent="base" xyz=" 0.11  0.11 0" rpy="0 0 0" direction="l" radius="0.04" mass="0.25"/>
-    <xacro:mecanum_wheel name="wheel_br" parent="base" xyz="-0.11 -0.11 0" rpy="0 0 0" direction="l" radius="0.04" mass="0.25"/>
-    <xacro:mecanum_wheel name="wheel_bl" parent="base" xyz="-0.11  0.11 0" rpy="0 0 0" direction="r" radius="0.04" mass="0.25"/>
+    <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"/>
+    <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"/>
+    <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"/>
+    <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"/>
 
     <ros2_control name="SherpaSystem" type="system">
         <hardware>
diff --git a/src/sherpa/worlds/empty.world b/src/sherpa/worlds/empty.world
index 3549b54..26d9fdf 100644
--- a/src/sherpa/worlds/empty.world
+++ b/src/sherpa/worlds/empty.world
@@ -1,6 +1,10 @@
 <?xml version="1.0" ?>
 <sdf version="1.5">
   <world name="empty">
+    <physics type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_update_rate>100</real_time_update_rate>
+    </physics>
     <plugin
       filename="gz-sim-physics-system"
       name="gz::sim::systems::Physics">
@@ -21,10 +25,6 @@
     <plugin filename="gz-sim-imu-system"
         name="gz::sim::systems::Imu">
     </plugin>
-    <plugin
-      filename="gz-sim-contact-system"
-      name="gz::sim::systems::Contact">
-    </plugin>
 
     <light name="sun" type="directional">
       <pose>0 0 20 0 0 0</pose>
@@ -52,10 +52,11 @@
           <surface>
             <friction>
               <ode>
-                <mu>0.8</mu>
-                <mu2>0.6</mu2>
+                <mu>50</mu>
               </ode>
             </friction>
+            <bounce/>
+            <contact/>
           </surface>
         </collision>
         <visual name="visual">
@@ -73,7 +74,22 @@
             </script>
           </material>
         </visual>
+        <pose>0 0 0 0 0 0</pose>
+        <inertial>
+          <pose>0 0 0 0 0 0</pose>
+          <mass>1</mass>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
       </link>
+      <pose>0 0 0 0 0 0</pose>
+      <self_collide>false</self_collide>
     </model>
   </world>
 </sdf>
\ No newline at end of file