From eedb38c2ffd49de17b5b231333de4591ee951e1f Mon Sep 17 00:00:00 2001
From: Ronan <ronan.bonnet42@wanadoo.fr>
Date: Sat, 28 Sep 2024 23:04:51 +0000
Subject: [PATCH 1/8] simu: Init robot description package

---
 src/robot_description/CMakeLists.txt | 12 ++++++++++++
 src/robot_description/package.xml    | 19 +++++++++++++++++++
 2 files changed, 31 insertions(+)
 create mode 100644 src/robot_description/CMakeLists.txt
 create mode 100644 src/robot_description/package.xml

diff --git a/src/robot_description/CMakeLists.txt b/src/robot_description/CMakeLists.txt
new file mode 100644
index 0000000..5b5033e
--- /dev/null
+++ b/src/robot_description/CMakeLists.txt
@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.5)
+project(robot_description)
+
+find_package(ament_cmake REQUIRED)
+find_package(urdf REQUIRED)
+
+install(
+  DIRECTORY launch urdf rviz
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
\ No newline at end of file
diff --git a/src/robot_description/package.xml b/src/robot_description/package.xml
new file mode 100644
index 0000000..bf98fc7
--- /dev/null
+++ b/src/robot_description/package.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>robot_description</name>
+    <version>0.0.0</version>
+    <description>INSA Toulouse Description Package</description>
+    <maintainer email="rbonnet@insa-toulouse.fr">Ronan Bonnet</maintainer>
+    <license>Apache 2.0</license>
+    <url type="repository">https://github.com/ClubRobotInsat/Info2025</url>
+    <url type="bugtracker">https://github.com/ClubRobotInsat/Info2025/issues</url>
+    <buildtool_depend>ament_cmake</buildtool_depend>
+    <exec_depend>joint_state_publisher</exec_depend>
+    <exec_depend>robot_state_publisher</exec_depend>
+    <exec_depend>rviz2</exec_depend>
+    <exec_depend>xacro</exec_depend>
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>
\ No newline at end of file

From cdd807082e4beae421f2b70dae7c738404515db5 Mon Sep 17 00:00:00 2001
From: Ronan <ronan.bonnet42@wanadoo.fr>
Date: Sat, 28 Sep 2024 23:05:25 +0000
Subject: [PATCH 2/8] simu: Make rectangle base for 2WD

Inertial macros are added to simplify calculations
---
 .../urdf/inertial_macros.xacro                | 40 +++++++++++++++++
 .../urdf/parts/base/base.gazebo.xacro         | 28 ++++++++++++
 .../urdf/parts/base/base.urdf.xacro           | 43 +++++++++++++++++++
 3 files changed, 111 insertions(+)
 create mode 100644 src/robot_description/urdf/inertial_macros.xacro
 create mode 100644 src/robot_description/urdf/parts/base/base.gazebo.xacro
 create mode 100644 src/robot_description/urdf/parts/base/base.urdf.xacro

diff --git a/src/robot_description/urdf/inertial_macros.xacro b/src/robot_description/urdf/inertial_macros.xacro
new file mode 100644
index 0000000..488de03
--- /dev/null
+++ b/src/robot_description/urdf/inertial_macros.xacro
@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
+
+    <!-- Specify some standard inertial calculations https://en.wikipedia.org/wiki/List_of_moments_of_inertia -->
+    <!-- These make use of xacro's mathematical functionality -->
+
+    <xacro:macro name="inertial_sphere" params="mass radius *origin">
+        <inertial>
+            <xacro:insert_block name="origin"/>
+            <mass value="${mass}" />
+            <inertia ixx="${(2/5) * mass * (radius*radius)}" ixy="0.0" ixz="0.0"
+                     iyy="${(2/5) * mass * (radius*radius)}" iyz="0.0"
+                     izz="${(2/5) * mass * (radius*radius)}" />
+        </inertial>
+    </xacro:macro>
+
+
+    <xacro:macro name="inertial_box" params="mass x y z *origin">
+        <inertial>
+            <xacro:insert_block name="origin"/>
+            <mass value="${mass}" />
+            <inertia ixx="${(1/12) * mass * (y*y+z*z)}" ixy="0.0" ixz="0.0"
+                     iyy="${(1/12) * mass * (x*x+z*z)}" iyz="0.0"
+                     izz="${(1/12) * mass * (x*x+y*y)}" />
+        </inertial>
+    </xacro:macro>
+
+
+    <xacro:macro name="inertial_cylinder" params="mass length radius *origin">
+        <inertial>
+            <xacro:insert_block name="origin"/>
+            <mass value="${mass}" />
+            <inertia ixx="${(1/12) * mass * (3*radius*radius + length*length)}" ixy="0.0" ixz="0.0"
+                     iyy="${(1/12) * mass * (3*radius*radius + length*length)}" iyz="0.0"
+                     izz="${(1/2) * mass * (radius*radius)}" />
+        </inertial>
+    </xacro:macro>
+
+
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/parts/base/base.gazebo.xacro b/src/robot_description/urdf/parts/base/base.gazebo.xacro
new file mode 100644
index 0000000..3875f27
--- /dev/null
+++ b/src/robot_description/urdf/parts/base/base.gazebo.xacro
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:macro name="base_gazebo">
+        <gazebo reference="base_link">
+            <visual>
+                <material>
+                    <ambient>
+                        0.8392 0.6314 0.0 1.0
+                    </ambient>
+                    <diffuse>
+                        0.8392 0.6314 0.0 1.0
+                    </diffuse>
+                    <specular>
+                        0.99 0.99 0.99 1.0
+                    </specular>
+                    <emissive>
+                        0.0 0.0 0.0 0.0
+                    </emissive>
+                </material>
+            </visual>
+            <selfCollide>
+                false
+            </selfCollide>
+            <mu1 value="0.05" />
+            <mu2 value="0.05" />
+        </gazebo>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/parts/base/base.urdf.xacro b/src/robot_description/urdf/parts/base/base.urdf.xacro
new file mode 100644
index 0000000..0d1d1e8
--- /dev/null
+++ b/src/robot_description/urdf/parts/base/base.urdf.xacro
@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:include filename="$(find robot_description)/urdf/inertial_macros.xacro"/>
+    <xacro:include filename="$(find robot_description)/urdf/parts/base/base.gazebo.xacro"/>
+
+    <xacro:macro name="base" params="length width height mass wheel_radius wheel_pos_z">
+        <link name="base_footprint"/>
+
+        <link name="base_link">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+                <geometry>
+                    <box size="${length} ${width} ${height}"/>
+                    <!-- <mesh filename="package://robot_description/meshes/STL/base/base.stl"/> -->
+                </geometry>
+                <material name="yellow">
+                    <color rgba="0.8784 0.6627 0.6627 1.0"/>
+                </material>
+            </visual>
+
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+                <geometry>
+                    <box size="${length} ${width} ${height}"/>
+                </geometry>
+            </collision>
+
+            <xacro:inertial_box mass="${mass}" x="${length}" y="${width}" z="${height}">
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+            </xacro:inertial_box>      
+        </link>
+
+        <!-- Insert Gazebo reference from base.gazebo.xacro file-->
+        <xacro:base_gazebo/>
+
+        <joint name="base_to_footprint" type="fixed">
+            <parent link="base_footprint"/>
+            <child link="base_link"/>
+            <origin xyz="0 0 ${wheel_radius - wheel_pos_z}" rpy="0 0 0"/>
+        </joint>
+    </xacro:macro>
+</robot>

From c85226325e47a2bd13a1152f4677c3b1b0b162d2 Mon Sep 17 00:00:00 2001
From: Ronan <ronan.bonnet42@wanadoo.fr>
Date: Sat, 28 Sep 2024 23:06:38 +0000
Subject: [PATCH 3/8] simu: Initialize 2WD model

---
 .../urdf/2wd.properties.xacro                 | 25 +++++++++++++++++++
 .../urdf/robots/2wd.urdf.xacro                | 15 +++++++++++
 2 files changed, 40 insertions(+)
 create mode 100644 src/robot_description/urdf/2wd.properties.xacro
 create mode 100644 src/robot_description/urdf/robots/2wd.urdf.xacro

diff --git a/src/robot_description/urdf/2wd.properties.xacro b/src/robot_description/urdf/2wd.properties.xacro
new file mode 100644
index 0000000..d84df2e
--- /dev/null
+++ b/src/robot_description/urdf/2wd.properties.xacro
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:property name="base_length" value="0.24" />
+  <xacro:property name="base_width" value="0.275" />
+  <xacro:property name="base_height" value="0.003" />
+  <xacro:property name="base_mass" value="1" />
+
+  <xacro:property name="wheel_radius" value="0.045" />
+  <xacro:property name="wheel_width" value="0.01" />
+  <xacro:property name="wheel_pos_x" value="0.0" />
+  <xacro:property name="wheel_pos_y" value="0.13" />
+  <xacro:property name="wheel_pos_z" value="-0.03" />
+  <xacro:property name="wheel_mass" value=".1" />
+  <xacro:property name="wheel_torque" value="20" />
+  <xacro:property name="front_caster_wheel" value="true" />
+  <xacro:property name="rear_caster_wheel" value="true" />
+
+  <xacro:property name="laser_pose">
+    <origin xyz="0.12 0 0.33" rpy="0 0 0"/>
+  </xacro:property>
+
+  <xacro:property name="depth_sensor_pose">
+    <origin xyz="0.14 0.0 0.045" rpy="0 0 0"/>
+  </xacro:property>
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/robots/2wd.urdf.xacro b/src/robot_description/urdf/robots/2wd.urdf.xacro
new file mode 100644
index 0000000..b75b74b
--- /dev/null
+++ b/src/robot_description/urdf/robots/2wd.urdf.xacro
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<robot name="robot_2wd" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find robot_description)/urdf/2wd.properties.xacro" />
+  <xacro:include filename="$(find robot_description)/urdf/parts/base/base.urdf.xacro" />
+
+  <xacro:base 
+    length="${base_length}" 
+    width="${base_width}" 
+    height="${base_height}" 
+    mass="${base_mass}"
+    wheel_radius="${wheel_radius}"
+    wheel_pos_z="${wheel_pos_z}"
+/>
+</robot>
\ No newline at end of file

From 3b6221a66a8e5afbcb53a31e4971230fd976ac70 Mon Sep 17 00:00:00 2001
From: Ronan <ronan.bonnet42@wanadoo.fr>
Date: Sat, 28 Sep 2024 23:07:06 +0000
Subject: [PATCH 4/8] simu: Make launch file

The type of robot will be chosen by an environment variable. Do not
forget to set it to change robots.
---
 .../launch/description.launch.py              |  79 ++++++++++
 src/robot_description/rviz/description.rviz   | 142 ++++++++++++++++++
 2 files changed, 221 insertions(+)
 create mode 100644 src/robot_description/launch/description.launch.py
 create mode 100644 src/robot_description/rviz/description.rviz

diff --git a/src/robot_description/launch/description.launch.py b/src/robot_description/launch/description.launch.py
new file mode 100644
index 0000000..5cdde4c
--- /dev/null
+++ b/src/robot_description/launch/description.launch.py
@@ -0,0 +1,79 @@
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution, EnvironmentVariable
+from launch.conditions import IfCondition
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.parameter_descriptions import ParameterValue
+
+def generate_launch_description():
+    robot_base = os.getenv('ROBOT_BASE', '2wd')
+    
+    urdf_path = PathJoinSubstitution(
+        [FindPackageShare('robot_description'), 'urdf/robots', f"{robot_base}.urdf.xacro"]
+    )
+    
+    rviz_config_path = PathJoinSubstitution(
+        [FindPackageShare('robot_description'), 'rviz', 'description.rviz']
+    )
+    
+    return LaunchDescription([
+        # Arguments
+        DeclareLaunchArgument(
+            name='urdf',
+            default_value=urdf_path,
+            description='URDF path',
+        ),
+        
+        DeclareLaunchArgument(
+            name='publish_joints',
+            default_value='true',
+            description='Launch joint_state_publisher',
+        ),
+        
+        DeclareLaunchArgument(
+            name='rviz',
+            default_value='false',
+            description='Launch rviz',
+        ),
+        
+        DeclareLaunchArgument(
+            name="use_sim_time",
+            default_value="false",
+            description="Use simulation (Gazebo) clock if true",
+        ),
+        
+        # Nodes
+        Node(
+            package='joint_state_publisher',
+            executable='joint_state_publisher',
+            name='joint_state_publisher',
+            condition=IfCondition(LaunchConfiguration('publish_joints')),
+            parameters=[{
+                'use_sim_time': LaunchConfiguration('use_sim_time')
+            }],
+        ),
+        
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[{
+                'use_sim_time': LaunchConfiguration('use_sim_time'),
+                'robot_description': ParameterValue(Command(['xacro ', LaunchConfiguration('urdf')]))
+            }],
+        ),
+        
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            condition=IfCondition(LaunchConfiguration('rviz')),
+            arguments=['-d', rviz_config_path],
+            parameters=[{
+                'use_sim_time': LaunchConfiguration('use_sim_time')
+            }],
+        )
+    ])
diff --git a/src/robot_description/rviz/description.rviz b/src/robot_description/rviz/description.rviz
new file mode 100644
index 0000000..a1ff5ae
--- /dev/null
+++ b/src/robot_description/rviz/description.rviz
@@ -0,0 +1,142 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 793
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.8999999761581421
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+      Marker Scale: 0.10000000149011612
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base_footprint:
+          base_link:
+            {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz_default_plugins/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.0010000000474974513
+      Reference Frame: <Fixed Frame>
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 238; 238; 236
+    Fixed Frame: base_footprint
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 0.5964717268943787
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5003980398178101
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 6.013586521148682
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000010000000000000156000003a2fc0200000001fb000000100044006900730070006c006100790073010000003b000003a2000000c700ffffff00000624000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Width: 1920
+  X: 0
+  Y: 27

From 54bb32a16095537c39bee0237c020c4b764b1181 Mon Sep 17 00:00:00 2001
From: Ronan <ronan.bonnet42@wanadoo.fr>
Date: Sun, 29 Sep 2024 19:30:37 +0000
Subject: [PATCH 5/8] simu: Add wheels descriptions

There are 3 types of wheels: caster, mecanum, normal. The difference
between mecanum and normal is mainly the friction coefficient.
---
 .../parts/wheels/caster_wheel.gazebo.xacro    |  9 ++++
 .../urdf/parts/wheels/caster_wheel.urdf.xacro | 47 +++++++++++++++++++
 .../parts/wheels/mecanum_wheel.gazebo.xacro   | 17 +++++++
 .../parts/wheels/mecanum_wheel.urdf.xacro     | 40 ++++++++++++++++
 .../urdf/parts/wheels/wheel.gazebo.xacro      | 15 ++++++
 .../urdf/parts/wheels/wheel.urdf.xacro        | 40 ++++++++++++++++
 6 files changed, 168 insertions(+)
 create mode 100644 src/robot_description/urdf/parts/wheels/caster_wheel.gazebo.xacro
 create mode 100644 src/robot_description/urdf/parts/wheels/caster_wheel.urdf.xacro
 create mode 100644 src/robot_description/urdf/parts/wheels/mecanum_wheel.gazebo.xacro
 create mode 100644 src/robot_description/urdf/parts/wheels/mecanum_wheel.urdf.xacro
 create mode 100644 src/robot_description/urdf/parts/wheels/wheel.gazebo.xacro
 create mode 100644 src/robot_description/urdf/parts/wheels/wheel.urdf.xacro

diff --git a/src/robot_description/urdf/parts/wheels/caster_wheel.gazebo.xacro b/src/robot_description/urdf/parts/wheels/caster_wheel.gazebo.xacro
new file mode 100644
index 0000000..7b1f644
--- /dev/null
+++ b/src/robot_description/urdf/parts/wheels/caster_wheel.gazebo.xacro
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:macro name="caster_wheel_link" params="side">
+        <gazebo reference="${side}_caster_wheel_link">
+            <mu1>0.01</mu1>
+            <mu2>0.01</mu2>
+        </gazebo>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/parts/wheels/caster_wheel.urdf.xacro b/src/robot_description/urdf/parts/wheels/caster_wheel.urdf.xacro
new file mode 100644
index 0000000..bfb4f0a
--- /dev/null
+++ b/src/robot_description/urdf/parts/wheels/caster_wheel.urdf.xacro
@@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:include filename="$(find robot_description)/urdf/inertial_macros.xacro" />
+    <xacro:include filename="$(find robot_description)/urdf/parts/wheels/caster_wheel.gazebo.xacro" />
+
+    <xacro:macro name="caster_wheel" params="side mass base_length wheel_radius wheel_pos_z">
+        <xacro:property name="caster_wheel_radius" value="${(wheel_radius - wheel_pos_z) / 2}" />
+        <xacro:if value="${side == 'front'}">
+            <xacro:property name="caster_wheel_x" value="${(base_length / 2) - caster_wheel_radius}" />
+        </xacro:if>
+
+        <xacro:if value="${side == 'rear'}">
+            <xacro:property name="caster_wheel_x"
+                value="${-(base_length / 2) + caster_wheel_radius}" />
+        </xacro:if>
+
+        <link name="${side}_caster_wheel_link">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <sphere radius="${caster_wheel_radius}" />
+                </geometry>
+                <material name="black">
+                    <color rgba="0.0 0.0 0.0 1.0" />
+                </material>
+            </visual>
+
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <sphere radius="${caster_wheel_radius}" />
+                </geometry>
+            </collision>
+
+            <xacro:inertial_sphere mass="${mass}" radius="${caster_wheel_radius}">
+                <origin xyz="0 0 0" rpy="0 0 0" />
+            </xacro:inertial_sphere>
+        </link>
+
+        <joint name="${side}_caster_wheel_joint" type="fixed">
+            <parent link="base_link" />
+            <child link="${side}_caster_wheel_link" />
+            <origin xyz="${caster_wheel_x} 0 -${caster_wheel_radius}" rpy="0 0 0" />
+        </joint>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/parts/wheels/mecanum_wheel.gazebo.xacro b/src/robot_description/urdf/parts/wheels/mecanum_wheel.gazebo.xacro
new file mode 100644
index 0000000..a0f9d2a
--- /dev/null
+++ b/src/robot_description/urdf/parts/wheels/mecanum_wheel.gazebo.xacro
@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:macro name="mecanum_wheel_link" params="side">
+        <gazebo reference="${side}_wheel_link">
+            <visual>
+                <material>
+                    <ambient>0.1764 0.4588 0.8509 1.0</ambient>
+                    <diffuse>0.1764 0.4588 0.8509 1.0</diffuse>
+                    <specular>0.1764 0.4588 0.8509 1.0</specular>
+                    <emissive>0.0 0.0 0.0 0.0</emissive>
+                </material>
+            </visual>
+            <mu1>0.01</mu1>
+            <mu2>0.01</mu2>
+        </gazebo>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/parts/wheels/mecanum_wheel.urdf.xacro b/src/robot_description/urdf/parts/wheels/mecanum_wheel.urdf.xacro
new file mode 100644
index 0000000..e2cf417
--- /dev/null
+++ b/src/robot_description/urdf/parts/wheels/mecanum_wheel.urdf.xacro
@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:include filename="$(find robot_description)/urdf/inertial_macros.xacro" />
+    <xacro:include filename="$(find robot_description)/urdf/parts/wheels/mecanum_wheel.gazebo.xacro" />
+
+    <xacro:macro name="mecanum_wheel" params="side radius width pos_x pos_y pos_z mass">
+        <link name="${side}_wheel_link">
+            <visual>
+                <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+                <geometry>
+                    <cylinder radius="${radius}" length="${width}" />
+                </geometry>
+                <material name="blue">
+                    <color rgba="0.1764 0.4588 0.8509 1.0" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+                <geometry>
+                    <cylinder radius="${radius}" length="${width}" />
+                </geometry>
+            </collision>
+
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <mass value="${mass}" />
+            <xacro:inertial_sphere mass="${mass}" radius="${radius}">
+                <origin xyz="0 0 0" rpy="0 0 0" />
+            </xacro:inertial_sphere>
+        </link>
+
+        <xacro:mecanum_wheel_link side="${side}" />
+
+        <joint name="${side}_wheel_joint" type="continuous">
+            <axis xyz="0 1 0" />
+            <parent link="base_link" />
+            <child link="${side}_wheel_link" />
+            <origin xyz="${pos_x} ${pos_y} ${pos_z}" rpy="0 0 0" />
+        </joint>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/parts/wheels/wheel.gazebo.xacro b/src/robot_description/urdf/parts/wheels/wheel.gazebo.xacro
new file mode 100644
index 0000000..7cb34df
--- /dev/null
+++ b/src/robot_description/urdf/parts/wheels/wheel.gazebo.xacro
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:macro name="wheel_link" params="side">
+        <gazebo reference="${side}_wheel_link">
+            <visual>
+                <material>
+                    <ambient>0.1764 0.4588 0.8509 1.0</ambient>
+                    <diffuse>0.1764 0.4588 0.8509 1.0</diffuse>
+                    <specular>0.1764 0.4588 0.8509 1.0</specular>
+                    <emissive>0.0 0.0 0.0 0.0</emissive>
+                </material>
+            </visual>
+        </gazebo>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/parts/wheels/wheel.urdf.xacro b/src/robot_description/urdf/parts/wheels/wheel.urdf.xacro
new file mode 100644
index 0000000..4d01274
--- /dev/null
+++ b/src/robot_description/urdf/parts/wheels/wheel.urdf.xacro
@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:include filename="$(find robot_description)/urdf/inertial_macros.xacro" />
+    <xacro:include filename="$(find robot_description)/urdf/parts/wheels/wheel.gazebo.xacro" />
+
+    <xacro:macro name="wheel" params="side radius width pos_x pos_y pos_z mass">
+        <link name="${side}_wheel_link">
+            <visual>
+                <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+                <geometry>
+                    <cylinder radius="${radius}" length="${width}" />
+                </geometry>
+                <material name="blue">
+                    <color rgba="0.1764 0.4588 0.8509 1.0" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+                <geometry>
+                    <cylinder radius="${radius}" length="${width}" />
+                </geometry>
+            </collision>
+
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <mass value="${mass}" />
+            <xacro:inertial_sphere mass="${mass}" radius="${radius}">
+                <origin xyz="0 0 0" rpy="0 0 0" />
+            </xacro:inertial_sphere>
+        </link>
+
+        <xacro:wheel_link side="${side}" />
+
+        <joint name="${side}_wheel_joint" type="continuous">
+            <axis xyz="0 1 0" />
+            <parent link="base_link" />
+            <child link="${side}_wheel_link" />
+            <origin xyz="${pos_x} ${pos_y} ${pos_z}" rpy="0 0 0" />
+        </joint>
+    </xacro:macro>
+</robot>
\ No newline at end of file

From 0cc09f4342d33789812b3ee91f32e1ee6a7d369e Mon Sep 17 00:00:00 2001
From: Ronan <ronan.bonnet42@wanadoo.fr>
Date: Sun, 29 Sep 2024 19:39:10 +0000
Subject: [PATCH 6/8] simu: Add wheels to 2WD robot

---
 .../urdf/robots/2wd.urdf.xacro                | 64 +++++++++++++++----
 1 file changed, 53 insertions(+), 11 deletions(-)

diff --git a/src/robot_description/urdf/robots/2wd.urdf.xacro b/src/robot_description/urdf/robots/2wd.urdf.xacro
index b75b74b..c20ad70 100644
--- a/src/robot_description/urdf/robots/2wd.urdf.xacro
+++ b/src/robot_description/urdf/robots/2wd.urdf.xacro
@@ -1,15 +1,57 @@
 <?xml version="1.0"?>
 <robot name="robot_2wd" xmlns:xacro="http://ros.org/wiki/xacro">
 
-  <xacro:include filename="$(find robot_description)/urdf/2wd.properties.xacro" />
-  <xacro:include filename="$(find robot_description)/urdf/parts/base/base.urdf.xacro" />
-
-  <xacro:base 
-    length="${base_length}" 
-    width="${base_width}" 
-    height="${base_height}" 
-    mass="${base_mass}"
-    wheel_radius="${wheel_radius}"
-    wheel_pos_z="${wheel_pos_z}"
-/>
+    <xacro:include filename="$(find robot_description)/urdf/2wd.properties.xacro" />
+    <xacro:include filename="$(find robot_description)/urdf/parts/base/base.urdf.xacro" />
+    <xacro:include filename="$(find robot_description)/urdf/parts/wheels/wheel.urdf.xacro" />
+    <xacro:include filename="$(find robot_description)/urdf/parts/wheels/caster_wheel.urdf.xacro" />
+
+    <xacro:base
+        length="${base_length}"
+        width="${base_width}"
+        height="${base_height}"
+        mass="${base_mass}"
+        wheel_radius="${wheel_radius}"
+        wheel_pos_z="${wheel_pos_z}"
+    />
+
+    <xacro:wheel
+        side="left"
+        radius="${wheel_radius}"
+        width="${wheel_width}"
+        pos_x="${wheel_pos_x}"
+        pos_y="${wheel_pos_y}"
+        pos_z="${wheel_pos_z}"
+        mass="${wheel_mass}"
+    />
+
+    <xacro:wheel
+        side="right"
+        radius="${wheel_radius}"
+        width="${wheel_width}"
+        pos_x="${wheel_pos_x}"
+        pos_y="${-wheel_pos_y}"
+        pos_z="${wheel_pos_z}"
+        mass="${wheel_mass}"
+    />
+
+    <xacro:if value="${front_caster_wheel}">
+        <xacro:caster_wheel
+            side="front"
+            mass="0.01"
+            base_length="${base_length}"
+            wheel_radius="${wheel_radius}"
+            wheel_pos_z="${wheel_pos_z}"
+        />
+    </xacro:if>
+
+    <xacro:if value="${rear_caster_wheel}">
+        <xacro:caster_wheel
+            side="rear"
+            mass="0.01"
+            base_length="${base_length}"
+            wheel_radius="${wheel_radius}"
+            wheel_pos_z="${wheel_pos_z}"
+        />
+    </xacro:if>
 </robot>
\ No newline at end of file

From 4255827948bf2b4bae937bc77203a322901e21dc Mon Sep 17 00:00:00 2001
From: Ronan <ronan.bonnet42@wanadoo.fr>
Date: Sun, 29 Sep 2024 19:53:41 +0000
Subject: [PATCH 7/8] simu: Add sensors IMU, Depth Sensor, Lidar

Have not tried yet if these work correctly, these files mostly come from linorobot2.
---
 .../urdf/sensors/depth_sensor.urdf.xacro      | 102 ++++++++++++++++++
 .../urdf/sensors/generic_laser.urdf.xacro     |  35 ++++++
 .../urdf/sensors/imu.gazebo.xacro             |  28 +++++
 .../urdf/sensors/imu.urdf.xacro               |  14 +++
 .../urdf/sensors/laser.urdf.xacro             |  82 ++++++++++++++
 5 files changed, 261 insertions(+)
 create mode 100644 src/robot_description/urdf/sensors/depth_sensor.urdf.xacro
 create mode 100644 src/robot_description/urdf/sensors/generic_laser.urdf.xacro
 create mode 100644 src/robot_description/urdf/sensors/imu.gazebo.xacro
 create mode 100644 src/robot_description/urdf/sensors/imu.urdf.xacro
 create mode 100644 src/robot_description/urdf/sensors/laser.urdf.xacro

diff --git a/src/robot_description/urdf/sensors/depth_sensor.urdf.xacro b/src/robot_description/urdf/sensors/depth_sensor.urdf.xacro
new file mode 100644
index 0000000..19619fd
--- /dev/null
+++ b/src/robot_description/urdf/sensors/depth_sensor.urdf.xacro
@@ -0,0 +1,102 @@
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+    <xacro:macro name="depth_sensor" params="*origin">
+        <link name="camera_link">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <box size="0.030 0.124 0.030" />
+                    <!-- <mesh filename="" scale="0.001 0.001 0.001"/> -->
+                </geometry>
+                <material name="red">
+                    <color rgba="0.416 0.051 0.047 1.0" />
+                </material>
+            </visual>
+
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <box size="0.030 0.124 0.030" />
+                </geometry>
+            </collision>
+
+            <inertial>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <mass value="0.135" />
+                <inertia ixx="${(1/12) * 0.135 * ((0.124  * 0.124)  + (0.030 * 0.030))}" ixy="0.0"
+                    ixz="0.0"
+                    iyy="${(1/12) * 0.135 * ((0.030 * 0.030) + (0.030 * 0.030))}" iyz="0.0"
+                    izz="${(1/12) * 0.135 * ((0.030 * 0.030) + (0.124  * 0.124))}" />
+            </inertial>
+        </link>
+      
+      <gazebo
+            reference="camera_link">
+            <visual>
+                <material>
+                    <ambient>0.416 0.051 0.047 1.0</ambient>
+                    <diffuse>0.416 0.051 0.047 1.0</diffuse>
+                    <specular>0.416 0.051 0.047 1.0</specular>
+                    <emissive>0.416 0.051 0.047 1.0</emissive>
+                </material>
+            </visual>
+        </gazebo>
+        rgb(213,28,40)
+  
+      <joint name="camera_to_base_link" type="fixed">
+            <parent link="base_link" />
+            <child link="camera_link" />
+            <xacro:insert_block name="origin" />
+        </joint>
+  
+      <link name="camera_depth_link" />
+      <joint
+            name="camera_depth_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="-1.57 0 -1.57" />
+            <parent link="camera_link" />
+            <child link="camera_depth_link" />
+        </joint>
+
+        <!--
+        https://github.com/ros-simulation/gazebo_ros_pkgs/wiki/ROS-2-Migration:-Camera#gazebo_ros_camera -->
+      <gazebo reference="camera_link">
+            <sensor name="camera" type="depth">
+                <always_on>true</always_on>
+                <update_rate>30.0</update_rate>
+                <camera name="camera">
+                    <horizontal_fov>1.50098</horizontal_fov>
+                    <image>
+                        <width>640</width>
+                        <height>480</height>
+                        <format>R8G8B8</format>
+                    </image>
+                </camera>
+                <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+                    <ros>
+                        <remapping>/camera/camera_info:=/camera/color/camera_info</remapping>
+                        <remapping>/camera/image_raw:=/camera/color/image_raw</remapping>
+                        <remapping>/camera/points:=/camera/depth/color/points</remapping>
+                        <remapping>/camera/depth/image_raw:=/camera/depth/image_rect_raw</remapping>
+                        <qos>
+                            <topic name="/camera/depth/color/points">
+                                <publisher>
+                                    <reliability>best_effort</reliability>
+                                </publisher>
+                            </topic>
+                        </qos>
+                    </ros>
+                    <min_depth>0.015</min_depth>
+                    <max_depth>3.0</max_depth>
+                    <hack_baseline>0.07</hack_baseline>
+                    <frame_name>camera_depth_link</frame_name>
+                    <distortion_k1>0.00000001</distortion_k1>
+                    <distortion_k2>0.00000001</distortion_k2>
+                    <distortion_k3>0.00000001</distortion_k3>
+                    <distortion_t1>0.00000001</distortion_t1>
+                    <distortion_t2>0.00000001</distortion_t2>
+                </plugin>
+            </sensor>
+
+        </gazebo>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/sensors/generic_laser.urdf.xacro b/src/robot_description/urdf/sensors/generic_laser.urdf.xacro
new file mode 100644
index 0000000..f691c9b
--- /dev/null
+++ b/src/robot_description/urdf/sensors/generic_laser.urdf.xacro
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:include filename="$(find robot_description)/urdf/sensors/laser.urdf.xacro" />
+
+    <xacro:property name="M_PI" value="3.1416" />
+    <xacro:macro name="generic_laser" params="*origin">
+        <xacro:laser
+            update_rate="10"
+            ray_count="360"
+            min_angle="-${M_PI}"
+            max_angle="${M_PI}"
+            min_range="0.08"
+            max_range="12.0"
+            frame_id="laser"
+            topic_name="scan"
+        >
+            <xacro:insert_block name="origin" />
+        </xacro:laser>
+    </xacro:macro>
+
+    <xacro:macro name="generic_base_laser" params="*origin">
+        <xacro:laser
+            update_rate="10"
+            ray_count="360"
+            min_angle="-${M_PI}"
+            max_angle="${M_PI}"
+            min_range="0.21"
+            max_range="5.5"
+            frame_id="base_laser"
+            topic_name="base/scan"
+        >
+            <xacro:insert_block name="origin" />
+        </xacro:laser>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/sensors/imu.gazebo.xacro b/src/robot_description/urdf/sensors/imu.gazebo.xacro
new file mode 100644
index 0000000..db15d5f
--- /dev/null
+++ b/src/robot_description/urdf/sensors/imu.gazebo.xacro
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:macro name="imu_gazebo">
+        <gazebo reference="imu_link">
+            <gravity>true</gravity>
+            <sensor name="imu_sensor" type="imu">
+                <always_on>true</always_on>
+                <update_rate>20</update_rate>
+                <visualize>true</visualize>
+                <topic>__default_topic__</topic>
+                <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+                    <ros>
+                        <remapping>~/out:=imu/data</remapping>
+                    </ros>
+                    <topicName>imu/data</topicName>
+                    <bodyName>imu_link</bodyName>
+                    <updateRateHZ>10.0</updateRateHZ>
+                    <gaussianNoise>0.0</gaussianNoise>
+                    <xyzOffset>0 0 0</xyzOffset>
+                    <rpyOffset>0 0 0</rpyOffset>
+                    <frameName>imu_link</frameName>
+                    <initialOrientationAsReference>false</initialOrientationAsReference>
+                </plugin>
+                <pose>0 0 0 0 0 0</pose>
+            </sensor>
+        </gazebo>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/sensors/imu.urdf.xacro b/src/robot_description/urdf/sensors/imu.urdf.xacro
new file mode 100644
index 0000000..3efb3ed
--- /dev/null
+++ b/src/robot_description/urdf/sensors/imu.urdf.xacro
@@ -0,0 +1,14 @@
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:include filename="$(find robot_description)/urdf/sensors/imu.gazebo.xacro" />
+    <xacro:macro name="imu">
+        <link name="imu_link" />
+
+        <joint name="imu_to_base_link" type="fixed">
+            <parent link="base_link" />
+            <child link="imu_link" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </joint>
+
+        <xacro:imu_gazebo />
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/robot_description/urdf/sensors/laser.urdf.xacro b/src/robot_description/urdf/sensors/laser.urdf.xacro
new file mode 100644
index 0000000..ad12f98
--- /dev/null
+++ b/src/robot_description/urdf/sensors/laser.urdf.xacro
@@ -0,0 +1,82 @@
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:include filename="$(find robot_description)/urdf/inertial_macros.xacro" />
+    <xacro:property name="mass" value="0.1" />
+    <xacro:property name="laser_radius" value="0.0375" />
+    <xacro:property name="laser_length" value="0.05" />
+    <xacro:macro name="laser"
+        params="*origin update_rate ray_count min_angle max_angle min_range max_range frame_id topic_name">
+        <link name="${frame_id}">
+            <visual>
+                <origin xyz="0 0 -0.025" rpy="0 0 0" />
+                <geometry>
+                    <cylinder radius="${laser_radius}" length="${laser_length}" />
+                </geometry>
+                <material name="green">
+                    <color rgba="0.003 0.639 0.223 1.0" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 -0.025" rpy="0 0 0" />
+                <geometry>
+                    <cylinder radius="${laser_radius}" length="${laser_length}" />
+                </geometry>
+            </collision>
+
+            <xacro:inertial_sphere mass="${mass}" radius="${laser_radius}">
+                <origin xyz="0 0 0" rpy="0 0 0" />
+            </xacro:inertial_sphere>
+        </link>
+
+        <gazebo reference="${frame_id}">
+            <visual>
+                <material>
+                    <ambient>0.003 0.639 0.223 1.0</ambient>
+                    <diffuse>0.003 0.639 0.223 1.0</diffuse>
+                    <specular>0.003 0.639 0.223 1.0</specular>
+                    <emissive>0.0 0.0 0.0 0.0</emissive>
+                </material>
+            </visual>
+        </gazebo>
+
+        <joint name="${frame_id}_to_base_link" type="fixed">
+            <parent link="base_link" />
+            <child link="${frame_id}" />
+            <xacro:insert_block name="origin" />
+        </joint>
+
+        <gazebo reference="${frame_id}">
+            <sensor type="gpu_ray" name="${frame_id}_lidar">
+                <always_on>true</always_on>
+                <update_rate>${update_rate}</update_rate>
+                <pose>0 0 0 0 0 0</pose>
+                <visualize>false</visualize>
+                <ray>
+                    <scan>
+                        <horizontal>
+                            <samples>${ray_count}</samples>
+                            <resolution>1</resolution>
+                            <min_angle>${min_angle}</min_angle>
+                            <max_angle>${max_angle}</max_angle>
+                        </horizontal>
+                    </scan>
+                    <range>
+                        <min>${min_range}</min>
+                        <max>${max_range}</max>
+                        <resolution>0.01</resolution>
+                    </range>
+                </ray>
+                <plugin name="${frame_id}_lidar_controller" filename="libgazebo_ros_ray_sensor.so">
+                    <ros>
+                        <remapping>~/out:=${topic_name}</remapping>
+                    </ros>
+                    <output_type>sensor_msgs/LaserScan</output_type>
+                    <alwaysOn>true</alwaysOn>
+                    <updateRate>${update_rate}</updateRate>
+                    <topicName>${topic_name}</topicName>
+                    <frame_name>${frame_id}</frame_name>
+                    <output_type>sensor_msgs/LaserScan</output_type>
+                </plugin>
+            </sensor>
+        </gazebo>
+    </xacro:macro>
+</robot>
\ No newline at end of file

From da03aabc8e6f5819f0cf0dc897f7193109e64233 Mon Sep 17 00:00:00 2001
From: Ronan <ronan.bonnet42@wanadoo.fr>
Date: Sun, 29 Sep 2024 19:55:45 +0000
Subject: [PATCH 8/8] simu: Add sensors to 2WD robot

---
 src/robot_description/urdf/robots/2wd.urdf.xacro | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/src/robot_description/urdf/robots/2wd.urdf.xacro b/src/robot_description/urdf/robots/2wd.urdf.xacro
index c20ad70..c4786dd 100644
--- a/src/robot_description/urdf/robots/2wd.urdf.xacro
+++ b/src/robot_description/urdf/robots/2wd.urdf.xacro
@@ -6,6 +6,10 @@
     <xacro:include filename="$(find robot_description)/urdf/parts/wheels/wheel.urdf.xacro" />
     <xacro:include filename="$(find robot_description)/urdf/parts/wheels/caster_wheel.urdf.xacro" />
 
+    <xacro:include filename="$(find robot_description)/urdf/sensors/imu.urdf.xacro" />
+    <xacro:include filename="$(find robot_description)/urdf/sensors/generic_laser.urdf.xacro" />
+    <xacro:include filename="$(find robot_description)/urdf/sensors/depth_sensor.urdf.xacro" />
+
     <xacro:base
         length="${base_length}"
         width="${base_width}"
@@ -54,4 +58,14 @@
             wheel_pos_z="${wheel_pos_z}"
         />
     </xacro:if>
+
+    <xacro:imu />
+
+    <xacro:generic_laser>
+        <xacro:insert_block name="laser_pose" />
+    </xacro:generic_laser>
+
+    <xacro:depth_sensor>
+        <xacro:insert_block name="depth_sensor_pose" />
+    </xacro:depth_sensor>
 </robot>
\ No newline at end of file