wip: updated sim launchfile

src/sherpa/config/gz_bridge.yaml

@@ -0,0 +1,35 @@
+- ros_topic_name: "/clock"
+  gz_topic_name: "clock"
+  ros_type_name: "rosgraph_msgs/msg/Clock"
+  gz_type_name: "gz.msgs.Clock"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "camera/camera_info"
+  gz_topic_name: "camera/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/tf"
+  gz_topic_name: "tf"
+  ros_type_name: "tf2_msgs/msg/TFMessage"
+  gz_type_name: "gz.msgs.Pose_V"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/joint_states"
+  gz_topic_name: "model/sherpa/joint_states"
+  ros_type_name: "sensor_msgs/msg/JointState"
+  gz_type_name: "gz.msgs.Model"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/velodyne_points"
+  gz_topic_name: "velodyne_points/points"
+  ros_type_name: "sensor_msgs/msg/PointCloud2"
+  gz_type_name: "gz.msgs.PointCloudPacked"
+  direction: GZ_TO_ROS  
+  
+- ros_topic_name: "/imu"
+  gz_topic_name: "imu"
+  ros_type_name: "sensor_msgs/msg/Imu"
+  gz_type_name: "gz.msgs.IMU"
+  direction: GZ_TO_ROS

src/sherpa/config/mecanum_controller.yaml

@@ -0,0 +1,32 @@
+
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+    # use_sim_time: true
+
+    mecanum_controller:
+      type: "mecanum_drive_controller/MecanumDriveController"
+
+    joint_state_broadcaster:
+      type: "joint_state_broadcaster/JointStateBroadcaster"
+
+
+mecanum_controller:
+  ros__parameters:
+
+    front_left_wheel_command_joint_name: "wheel_fl_joint"
+    front_right_wheel_command_joint_name: "wheel_fr_joint"
+    rear_left_wheel_command_joint_name: "wheel_bl_joint"
+    rear_right_wheel_command_joint_name: "wheel_br_joint"
+
+    kinematics:
+      base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 }
+      wheels_radius: 0.4
+      sum_of_robot_center_projection_on_X_Y_axis: 1.0
+
+    base_frame_id: "base_link"
+    odom_frame_id: "map"
+    enable_odom_tf: false
+    twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+    pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+    use_sim_time: True

src/sherpa/launch/simulator.launch.py

@@ -46,10 +46,27 @@ def generate_launch_description():
         }]
     )
 
-    joint_pub = Node(
+    # Load controller manager
-        package='joint_state_publisher',
+    #control_manager = Node(
-        executable='joint_state_publisher',
+    #    package='controller_manager',
-        name='joint_state_publisher',
+    #    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',
+        arguments=['mecanum_controller'],
+        output='screen'
+    )
+
+    joint_state_broadcaster = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+        parameters=[{'use_sim_time': True}],
         output='screen'
     )
 
@@ -60,8 +77,8 @@ def generate_launch_description():
         arguments=[
             '0', '0', '0',
             '0', '0', '0',
-            'lidar_link',
+            'velodyne',
-            'camera_init',
+            'base_link',
         ]          
     )
 
@@ -76,17 +93,13 @@ def generate_launch_description():
     )
 
     gz_bridge = Node(
-        package='ros_gz_bridge',
+        package="ros_gz_bridge",
-        executable='parameter_bridge',
+        executable="parameter_bridge",
         arguments=[
-            '/velodyne_points/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked',
+            "--ros-args",
-            '/model/sherpa/joint_states@gz.msgs.Model_JointState@ros2/sensor_msgs/msg/JointState',
+            "-p",
-        ],
+            f"config_file:={os.path.join(get_package_share_directory('sherpa'), 'config', 'gz_bridge.yaml')}",
-        remappings=[
+        ]
-            ('/velodyne_points/points', '/velodyne_points'),
-            ('/model/sherpa/joint_states', '/joint_states'),
-        ],
-        output='screen'
     )
 
 
@@ -99,10 +112,12 @@ def generate_launch_description():
     ld.add_action(gz_env)
     ld.add_action(gz)
     ld.add_action(state_publisher)
-    ld.add_action(joint_pub)
     ld.add_action(static_tf)
     ld.add_action(robot)
     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

src/sherpa/package.xml

@@ -12,6 +12,25 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <depend>xacro</depend>
+
+  <!-- Dependencies for the robot control system -->
+  <depend>rclcpp</depend>
+  <depend>ros2_control</depend>
+  <depend>controller_manager</depend>
+  <depend>hardware_interface</depend>
+
+  <!-- Dependencies for gazebo -->
+  <depend>gazebo_ros2_control</depend>
+  <depend>gazebo_msgs</depend>
+  <depend>ros_gz_bridge</depend>
+
+  <!-- Dependencies for slam -->
+  <depend>velodyne_driver</depend>
+  <depend>velodyne_pointcloud</depend>
+  <depend>witmotion_ros</depend>
+  <depend>fast_lio</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/sherpa/urdf/components/imu.xacro

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="imu" params="name parent xyz rpy frequency">
+
+        <link name="${name}_link">
+            <inertial>
+                <mass value="0.01"/>
+                <inertia 
+                    ixx="0.001828"
+                    ixy="0.0"
+                    ixz="0.0"
+                    iyy="0.001828"
+                    iyz="0.0"
+                    izz="0.003528"
+                />
+            </inertial>
+        </link>
+        <joint name="${name}_joint" type="fixed">
+            <parent link="${parent}_link"/>
+            <child link="${name}_link"/>
+            <origin xyz="${xyz}" rpy="${rpy}"/>
+        </joint>
+        <gazebo reference="${name}_link">
+            <sensor name="${name}" type="imu">
+                <always_on>1</always_on>
+                <update_rate>${frequency}</update_rate>
+                <gz_frame_id>${name}_link</gz_frame_id>
+                <visualize>true</visualize>
+                <topic>/imu</topic>
+            </sensor>
+        </gazebo>
+    </xacro:macro>
+</robot>

src/sherpa/urdf/components/mecanum_wheel.xacro

@@ -13,11 +13,11 @@
 
         <!-- get left or right wheel -->
         <xacro:if value="${direction == 'l'}">
-            <xacro:property name="mesh" value="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL"/>
+            <xacro:property name="mesh" value="package://sherpa/urdf/meshes//mecanum_wheel_left.STL"/>
             <xacro:property name="friction_dir" value="-1"/>
         </xacro:if>
         <xacro:if value="${direction == 'r'}">
-            <xacro:property name="mesh" value="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL"/>
+            <xacro:property name="mesh" value="package://sherpa/urdf/meshes//mecanum_wheel_right.STL"/>
             <xacro:property name="friction_dir" value="1"/>
         </xacro:if>
 
@@ -46,18 +46,18 @@
                 <material name="mecanum_wheel_material"/>
             </visual>
             <collision name='collision'>
-            <geometry>
+                <geometry>
-                <sphere radius="${radius}"/>
+                    <sphere radius="${radius}"/>
-            </geometry>
+                </geometry>
-            <surface>
+                <surface>
-                <friction>
+                    <friction>
-                <ode>
+                    <ode>
-                    <mu>1.2</mu>
+                        <mu>1.2</mu>
-                    <mu2>0</mu2>
+                        <mu2>0</mu2>
-                    <fdir1 gz:expressed_in="${parent}">1 ${friction_dir} 0</fdir1>
+                        <fdir1 gz:expressed_in="${parent}_link">1 ${friction_dir} 0</fdir1>
-                </ode>
+                    </ode>
-                </friction>
+                    </friction>
-            </surface>
+                </surface>
             </collision>
         </link>
         <joint name='${name}_joint' type='continuous'>

src/sherpa/urdf/components/ouster_lidar.xacro

@@ -140,11 +140,6 @@
         </joint>
         <gazebo reference="${name}_link">
             <sensor name="${name}" type="gpu_lidar">
-                <plugin
-                    filename="gz-sim-sensors-system"
-                    name="gz::sim::systems::Sensors">
-                    <render_engine>ogre2</render_engine>
-                </plugin>
                 <update_rate>${frequency}</update_rate>
                 <topic>/velodyne_points</topic>
                 <gz_frame_id>${name}_link</gz_frame_id>

src/sherpa/urdf/sherpa.urdf

@@ -131,7 +131,7 @@
           <ode>
             <mu>1.2</mu>
             <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base">1 1 0</fdir1>
+            <fdir1 gz:expressed_in="base_link">1 1 0</fdir1>
           </ode>
         </friction>
       </surface>
@@ -165,7 +165,7 @@
           <ode>
             <mu>1.2</mu>
             <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base">1 -1 0</fdir1>
+            <fdir1 gz:expressed_in="base_link">1 -1 0</fdir1>
           </ode>
         </friction>
       </surface>
@@ -199,7 +199,7 @@
           <ode>
             <mu>1.2</mu>
             <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base">1 -1 0</fdir1>
+            <fdir1 gz:expressed_in="base_link">1 -1 0</fdir1>
           </ode>
         </friction>
       </surface>
@@ -233,7 +233,7 @@
           <ode>
             <mu>1.2</mu>
             <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base">1 1 0</fdir1>
+            <fdir1 gz:expressed_in="base_link">1 1 0</fdir1>
           </ode>
         </friction>
       </surface>
@@ -246,17 +246,25 @@
     <limit effort="100000" velocity="5"/>
     <origin rpy="0 0 0" xyz="-0.11  0.11 0"/>
   </joint>
-  <gazebo>
+  <ros2_control name="SherpaSystem" type="system">
-    <plugin filename="gz-sim-mecanum-drive-system" name="gz::sim::systems::MecanumDrive">
+    <hardware>
-      <front_left_joint>wheel_fl_joint</front_left_joint>
+      <plugin>ros2_control/GenericSystemInterface</plugin>
-      <front_right_joint>wheel_fr_joint</front_right_joint>
+    </hardware>
-      <back_left_joint>wheel_bl_joint</back_left_joint>
+    <joint name="wheel_fl_joint">
-      <back_right_joint>wheel_br_joint</back_right_joint>
+      <command_interface name="velocity"/>
-      <wheel_separation>0.22</wheel_separation>
+      <state_interface name="velocity"/>
-      <wheelbase>0.22</wheelbase>
+    </joint>
-      <wheel_radius>0.04</wheel_radius>
+    <joint name="wheel_fr_joint">
-      <min_acceleration>-5</min_acceleration>
+      <command_interface name="velocity"/>
-      <max_acceleration>5</max_acceleration>
+      <state_interface name="velocity"/>
-    </plugin>
+    </joint>
-  </gazebo>
+    <joint name="wheel_bl_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="wheel_br_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="velocity"/>
+    </joint>
+  </ros2_control>
 </robot>

src/sherpa/urdf/sherpa.xacro

@@ -1,5 +1,11 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sherpa">   
+<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>
+        </plugin>
+    </gazebo>
+
     <link name="base_link">
         <inertial>
             <mass value="15"/>
@@ -14,9 +20,11 @@
         </inertial>
     </link>
 
+    <xacro:include filename="components/imu.xacro"/>
     <xacro:include filename="components/mecanum_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.11 -0.11 0" rpy="0 0 0" direction="r" radius="0.04" mass="0.25"/>
@@ -24,17 +32,33 @@
     <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"/>
 
-    <gazebo>
+    <ros2_control name="SherpaSystem" type="system">
-        <plugin filename="gz-sim-mecanum-drive-system" name="gz::sim::systems::MecanumDrive">
+        <hardware>
-            <front_left_joint>wheel_fl_joint</front_left_joint>
+            <plugin>gz_ros2_control/GazeboSimSystem</plugin>
-            <front_right_joint>wheel_fr_joint</front_right_joint>
+        </hardware>
-            <back_left_joint>wheel_bl_joint</back_left_joint>
+        
-            <back_right_joint>wheel_br_joint</back_right_joint>
+        <joint name="wheel_fl_joint">
-            <wheel_separation>0.22</wheel_separation>
+            <command_interface name="velocity"/>
-            <wheelbase>0.22</wheelbase>
+            <state_interface name="velocity"/>
-            <wheel_radius>0.04</wheel_radius>
+            <state_interface name="position"/>
-            <min_acceleration>-5</min_acceleration>
+        </joint>
-            <max_acceleration>5</max_acceleration>
+
-        </plugin>
+        <joint name="wheel_fr_joint">
-    </gazebo>
+            <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"/>
+            <state_interface name="position"/>
+        </joint>
+
+        <joint name="wheel_br_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+            <state_interface name="position"/>
+        </joint>
+    </ros2_control>
 </robot>

src/sherpa/worlds/empty.world

@@ -1,6 +1,31 @@
 <?xml version="1.0" ?>
 <sdf version="1.5">
   <world name="empty">
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <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>
       <diffuse>0.8 0.8 0.8 1.0</diffuse>