From ec450bee89ac0b374f58c0cb19ed90b7f1aba616 Mon Sep 17 00:00:00 2001
From: Chazyman <c.demontaigu@gmail.com>
Date: Sun, 21 Sep 2025 18:30:53 +0200
Subject: [PATCH 1/3] remove unsupported jazzy gz_frame_id for imu & depth

---
 linorobot2_description/urdf/sensors/depth_sensor.urdf.xacro | 1 -
 linorobot2_description/urdf/sensors/imu.urdf.xacro          | 1 -
 2 files changed, 2 deletions(-)

diff --git a/linorobot2_description/urdf/sensors/depth_sensor.urdf.xacro b/linorobot2_description/urdf/sensors/depth_sensor.urdf.xacro
index 96c8d2ad..c8e5ccf9 100644
--- a/linorobot2_description/urdf/sensors/depth_sensor.urdf.xacro
+++ b/linorobot2_description/urdf/sensors/depth_sensor.urdf.xacro
@@ -56,7 +56,6 @@
     <gazebo reference="camera_link">
       <sensor name="camera" type="rgbd_camera">
         <topic>camera</topic>
-        <gz_frame_id>camera_link</gz_frame_id>
         <update_rate>30.0</update_rate>
         <always_on>true</always_on>
         <visualize>true</visualize>
diff --git a/linorobot2_description/urdf/sensors/imu.urdf.xacro b/linorobot2_description/urdf/sensors/imu.urdf.xacro
index 881c47e7..37c1c295 100644
--- a/linorobot2_description/urdf/sensors/imu.urdf.xacro
+++ b/linorobot2_description/urdf/sensors/imu.urdf.xacro
@@ -16,7 +16,6 @@
     <gazebo reference="imu_link">
       <sensor name="imu_sensor" type="imu">
         <topic>imu/data</topic>
-        <gz_frame_id>imu_link</gz_frame_id>
         <update_rate>50</update_rate>
         <always_on>true</always_on>
         <visualize>true</visualize>

From e341dde7e3282b7a8e7a112a79cb35199a8509aa Mon Sep 17 00:00:00 2001
From: Chazyman <c.demontaigu@gmail.com>
Date: Sun, 21 Sep 2025 18:34:15 +0200
Subject: [PATCH 2/3] initialize pose config parameters in navigation & slam
 launch files

---
 .../launch/navigation.launch.py               | 23 +++++++++++-
 linorobot2_navigation/launch/slam.launch.py   | 35 +++++++++++++++----
 2 files changed, 50 insertions(+), 8 deletions(-)

diff --git a/linorobot2_navigation/launch/navigation.launch.py b/linorobot2_navigation/launch/navigation.launch.py
index 41315cca..752afb1f 100644
--- a/linorobot2_navigation/launch/navigation.launch.py
+++ b/linorobot2_navigation/launch/navigation.launch.py
@@ -61,12 +61,33 @@ def generate_launch_description():
             description='Navigation map path'
         ),
 
+        DeclareLaunchArgument(
+            name='initial_pose_x',
+            default_value='0.5',
+            description='Initial robot X position'
+        ),
+
+        DeclareLaunchArgument(
+            name='initial_pose_y',
+            default_value='0.0',
+            description='Initial robot Y position'
+        ),
+
+        DeclareLaunchArgument(
+            name='initial_pose_yaw',
+            default_value='0.0',
+            description='Initial robot yaw'
+        ),
+
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(nav2_launch_path),
             launch_arguments={
                 'map': LaunchConfiguration("map"),
                 'use_sim_time': LaunchConfiguration("sim"),
-                'params_file': nav2_config_path
+                'params_file': nav2_config_path,
+                'initial_pose_x': LaunchConfiguration('initial_pose_x'),
+                'initial_pose_y': LaunchConfiguration('initial_pose_y'),
+                'initial_pose_yaw': LaunchConfiguration('initial_pose_yaw')
             }.items()
         ),
 
diff --git a/linorobot2_navigation/launch/slam.launch.py b/linorobot2_navigation/launch/slam.launch.py
index 5d4dc3fe..098c8c1e 100644
--- a/linorobot2_navigation/launch/slam.launch.py
+++ b/linorobot2_navigation/launch/slam.launch.py
@@ -63,20 +63,41 @@ def generate_launch_description():
             default_value='false',
             description='Run rviz'
         ),
+        DeclareLaunchArgument(
+            name='initial_pose_x',
+            default_value='0.5',
+            description='Initial robot X position'
+        ),
 
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(navigation_launch_path),
-            launch_arguments={
-                'use_sim_time': LaunchConfiguration("sim"),
-                'params_file': nav2_config_path
-            }.items()
+        DeclareLaunchArgument(
+            name='initial_pose_y',
+            default_value='0.0',
+            description='Initial robot Y position'
         ),
 
+        DeclareLaunchArgument(
+            name='initial_pose_yaw',
+            default_value='0.0',
+            description='Initial robot yaw'
+        ),
+
+        # IncludeLaunchDescription(
+        #     PythonLaunchDescriptionSource(navigation_launch_path),
+        #     launch_arguments={
+        #         'use_sim_time': LaunchConfiguration("sim"),
+        #         'params_file': nav2_config_path
+        #     }.items()
+        # ),
+
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(slam_launch_path),
             launch_arguments={
                 'use_sim_time': LaunchConfiguration("sim"),
-                slam_param_name: slam_config_path
+                slam_param_name: slam_config_path,
+                # Pass the initial pose to slam_toolbox
+                'initial_pose_x': LaunchConfiguration('initial_pose_x'),
+                'initial_pose_y': LaunchConfiguration('initial_pose_y'),
+                'initial_pose_yaw': LaunchConfiguration('initial_pose_yaw')
             }.items()
         ),
 

From f5b0006014ea46296bafe49d2d235ecf774db7f9 Mon Sep 17 00:00:00 2001
From: Chazyman <c.demontaigu@gmail.com>
Date: Sun, 21 Sep 2025 18:35:46 +0200
Subject: [PATCH 3/3] set the linorobot pose identical to gazebo and navigation
 rviz config

---
 linorobot2_navigation/rviz/linorobot2_slam.rviz | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/linorobot2_navigation/rviz/linorobot2_slam.rviz b/linorobot2_navigation/rviz/linorobot2_slam.rviz
index e21dfe8a..df4396dd 100644
--- a/linorobot2_navigation/rviz/linorobot2_slam.rviz
+++ b/linorobot2_navigation/rviz/linorobot2_slam.rviz
@@ -256,7 +256,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: 0
+      Angle: -1.5599987506866455
       Class: rviz_default_plugins/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549