From 5e6b4f4569a6b07177e0b80891c7846a1a397a4e Mon Sep 17 00:00:00 2001
From: Matthew Hansen <12618239+mkhansenbot@users.noreply.github.com>
Date: Sat, 30 Dec 2023 01:06:51 +0000
Subject: [PATCH 1/3] Add odometry publisher to mars_rover model (#11).

Fix indentation for mars_rover control plugin

Set odom publish frequency to 10

Fix plugin filename to be consistent

re-add background_color to plugin parameters
---
 .../urdf/curiosity_mars_rover.gazebo            | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/models/curiosity_path/urdf/curiosity_mars_rover.gazebo b/models/curiosity_path/urdf/curiosity_mars_rover.gazebo
index 63421cb..c209b6b 100644
--- a/models/curiosity_path/urdf/curiosity_mars_rover.gazebo
+++ b/models/curiosity_path/urdf/curiosity_mars_rover.gazebo
@@ -116,9 +116,20 @@
 
   <gazebo>
       <plugin filename="libign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
-      <robot_param>robot_description</robot_param>
-      <robot_param_node>robot_state_publisher</robot_param_node>
-      <parameters>$(find mars_rover)/config/mars_rover_control.yaml</parameters>
+          <robot_param>robot_description</robot_param>
+          <robot_param_node>robot_state_publisher</robot_param_node>
+          <parameters>$(find mars_rover)/config/mars_rover_control.yaml</parameters>
+      </plugin>
+
+      <plugin filename="ignition-gazebo-odometry-publisher-system" name="ignition::gazebo::systems::OdometryPublisher">
+          <odom_frame>/odom</odom_frame>
+          <robot_base_frame>/base_footprint</robot_base_frame>
+          <odom_publish_frequency>10</odom_publish_frequency>
+      </plugin>
+
+      <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
+          <render_engine>ogre2</render_engine>
+          <background_color>0.9 0.753 0.66 1</background_color>
       </plugin>
   </gazebo>
 

From 504df16b1d52da3b3ba916d93cdf9ab4511f7c7b Mon Sep 17 00:00:00 2001
From: Matthew Hansen <12618239+mkhansenbot@users.noreply.github.com>
Date: Sat, 20 Jan 2024 22:03:03 +0000
Subject: [PATCH 2/3] Add base_link to mars_rover model (#11).

---
 models/curiosity_path/urdf/chassis.xacro | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/models/curiosity_path/urdf/chassis.xacro b/models/curiosity_path/urdf/chassis.xacro
index 90ba576..8a9ebf4 100644
--- a/models/curiosity_path/urdf/chassis.xacro
+++ b/models/curiosity_path/urdf/chassis.xacro
@@ -5,10 +5,17 @@
 
     <!-- Used for fixing robot to Gazebo 'chassis' -->
 	<link name="base_footprint" />
+	<link name="base_link" />
 
-	<joint name="base_joint" type="fixed">
+	<joint name="base_footprint_to_link" type="fixed">
 		<parent link="base_footprint"/>
+		<child link="base_link"/>
+	</joint>
+
+	<joint name="base_to_chassis" type="fixed">
+		<parent link="base_link"/>
 		<child link="chassis"/>
+		<origin rpy="0 0 1.57" xyz="0 0 0"/>
 	</joint>
 
 

From 650552619e0e3d43c4160f82010b586012b8e5a7 Mon Sep 17 00:00:00 2001
From: Matthew Hansen <12618239+mkhansenbot@users.noreply.github.com>
Date: Sat, 20 Jan 2024 22:07:57 +0000
Subject: [PATCH 3/3] Add lidar to mars_rover model (#11).

---
 .../urdf/curiosity_mars_rover.gazebo          | 37 ++++++++++++++++---
 models/curiosity_path/urdf/sensor_mast.xacro  | 26 +++++++++++++
 2 files changed, 57 insertions(+), 6 deletions(-)

diff --git a/models/curiosity_path/urdf/curiosity_mars_rover.gazebo b/models/curiosity_path/urdf/curiosity_mars_rover.gazebo
index c209b6b..49720b4 100644
--- a/models/curiosity_path/urdf/curiosity_mars_rover.gazebo
+++ b/models/curiosity_path/urdf/curiosity_mars_rover.gazebo
@@ -133,6 +133,37 @@
       </plugin>
   </gazebo>
 
+  <gazebo reference="lidar_link">
+      <sensor name='lidar' type='gpu_lidar'>"
+          <ignition_frame_id>lidar_link</ignition_frame_id>
+          <pose relative_to='lidar_link'>0 0 0 0 0 0</pose>
+          <topic>scan</topic>
+          <update_rate>10</update_rate>
+          <ray>
+              <scan>
+                  <horizontal>
+                      <samples>640</samples>
+                      <resolution>1</resolution>
+                      <min_angle>-3.1416</min_angle>
+                      <max_angle>3.1416</max_angle>
+                  </horizontal>
+                  <vertical>
+                      <samples>1</samples>
+                      <resolution>0.01</resolution>
+                      <min_angle>0</min_angle>
+                      <max_angle>0</max_angle>
+                  </vertical>
+              </scan>
+              <range>
+                  <min>1.0</min>
+                  <max>20.0</max>
+                  <resolution>0.05</resolution>
+              </range>
+          </ray>
+          <always_on>1</always_on>
+          <visualize>true</visualize>
+      </sensor>
+  </gazebo>
 
   <gazebo reference="camera_link">
       <sensor type="camera" name="camera_sensor">
@@ -157,12 +188,6 @@
           </camera>
           <always_on>1</always_on>
           <topic>image_raw</topic>
-          <plugin
-              filename="ignition-gazebo-sensors-system"
-              name="ignition::gazebo::systems::Sensors">
-              <render_engine>ogre2</render_engine>
-              <background_color>0.9 0.753 0.66 1</background_color>
-          </plugin>
       </sensor>
   </gazebo>
 
diff --git a/models/curiosity_path/urdf/sensor_mast.xacro b/models/curiosity_path/urdf/sensor_mast.xacro
index 2c084eb..5737d1a 100644
--- a/models/curiosity_path/urdf/sensor_mast.xacro
+++ b/models/curiosity_path/urdf/sensor_mast.xacro
@@ -268,6 +268,32 @@
         <origin xyz="0.13642 -0.218683 0.222314" rpy="0 0 ${-PI/2}"/>
     </joint>
 
+  	<link name="lidar_link">
+        <!-- Inertial for gazebo to take it into account, Put 1gr weight -->
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <mass value="0.001" />
+            <inertia ixx="9.1875e-09" ixy="0.0" ixz="0.0" iyy="9.1875e-09" iyz="0.0" izz="1.378125e-08"/>
+        </inertial>
+		<visual>
+            <origin rpy="0.0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <cylinder radius="0.05" length="0.02"/>
+            </geometry>
+            <material name="black"/>
+        </visual>
+	</link>
+
+    <gazebo reference="lidar_link">
+        <material>Gazebo/Black</material>
+    </gazebo>
+
+    <joint name="lidar_joint" type="fixed">
+        <parent link="mast_cameras"/>
+        <child link="lidar_link"/>
+        <origin xyz="0 0 0.325" rpy="0 0 ${-PI/2}"/>
+    </joint>
+
   </xacro:macro>
 
 </robot>