update

src/TurtleBot-RRT-Star/launch/bringup_localization_with_initial_pose.launch.py

@@ -1,11 +1,10 @@
 #!/usr/bin/env python3
 """
-Launch localization with automatic AMCL initial pose setting.
+Launch localization with AMCL configured by the provided Nav2 params file.
-No need to manually click 2D Pose Estimate in RViz.
 """
 
 from launch import LaunchDescription
-from launch.actions import ExecuteProcess, IncludeLaunchDescription, TimerAction
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 import os
@@ -15,15 +14,20 @@ from ament_index_python.packages import get_package_share_directory
 
 def generate_launch_description():
     bringup_dir = get_package_share_directory('nav2_bringup')
-    script_path = os.path.join(
+    default_params_file = os.path.join(
-        get_package_share_directory('nav2_rrtstar_planner'),
+        bringup_dir, 'params', 'nav2_params.yaml'
-        'scripts',
-        'publish_initial_pose.py'
     )
 
     # Launch arguments
     use_sim_time = LaunchConfiguration('use_sim_time', default='true')
     map_yaml_file = LaunchConfiguration('map')
+    params_file = LaunchConfiguration('params_file')
+
+    declare_params_file = DeclareLaunchArgument(
+        'params_file',
+        default_value=default_params_file,
+        description='Full path to the ROS2 parameters file to use for localization nodes',
+    )
 
     # Include original localization_launch.py
     localization_launch = IncludeLaunchDescription(
@@ -32,23 +36,13 @@ def generate_launch_description():
         ),
         launch_arguments={
             'use_sim_time': use_sim_time,
-            'map': map_yaml_file
+            'map': map_yaml_file,
+            'params_file': params_file,
         }.items()
     )
 
-    # Auto-publish initial pose (0, 0) after 3 seconds delay
-    auto_initial_pose = TimerAction(
-        period=3.0,
-        actions=[
-            ExecuteProcess(
-                cmd=['python3', script_path],
-                output='screen'
-            )
-        ]
-    )
-
     ld = LaunchDescription()
+    ld.add_action(declare_params_file)
     ld.add_action(localization_launch)
-    ld.add_action(auto_initial_pose)
 
     return ld

src/TurtleBot-RRT-Star/scripts/publish_initial_pose.py

@@ -1,66 +0,0 @@
-#!/usr/bin/env python3
-"""Publish initial pose for AMCL localization."""
-
-import rclpy
-from rclpy.node import Node
-from geometry_msgs.msg import PoseWithCovarianceStamped
-
-
-class InitialPosePublisher(Node):
-    def __init__(self):
-        super().__init__('initial_pose_publisher')
-        self.publisher = self.create_publisher(
-            PoseWithCovarianceStamped,
-            '/initialpose',
-            10
-        )
-        self.timer = self.create_timer(1.0, self.publish_pose)
-        self.get_logger().info('Initial pose publisher started')
-        self.published = False
-
-    def publish_pose(self):
-        if self.published:
-            return
-
-        msg = PoseWithCovarianceStamped()
-        msg.header.stamp.sec = 0
-        msg.header.stamp.nanosec = 0
-        msg.header.frame_id = 'map'
-        msg.pose.pose.position.x = 0.0
-        msg.pose.pose.position.y = 0.0
-        msg.pose.pose.position.z = 0.0
-        msg.pose.pose.orientation.x = 0.0
-        msg.pose.pose.orientation.y = 0.0
-        msg.pose.pose.orientation.z = 0.0
-        msg.pose.pose.orientation.w = 1.0
-        msg.pose.covariance = [
-            0.25, 0.0, 0.0, 0.0, 0.0, 0.0,
-            0.0, 0.25, 0.0, 0.0, 0.0, 0.0,
-            0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-            0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-            0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-            0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942
-        ]
-
-        self.publisher.publish(msg)
-        self.get_logger().info('Published initial pose (0, 0)')
-        self.published = True
-        # Shutdown after publishing
-        self.get_logger().info('Shutting down initial pose publisher')
-        raise rclpy.shutdown()
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    try:
-        node = InitialPosePublisher()
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        if rclpy.ok():
-            rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

src/turtlebot3_simulations/turtlebot3_gazebo/launch/robot_state_publisher.launch.py

@@ -19,6 +19,7 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
+from launch.conditions import IfCondition, UnlessCondition
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
@@ -48,11 +49,27 @@ def generate_launch_description():
             'use_sim_time',
             default_value='false',
             description='Use simulation (Gazebo) clock if true'),
+        DeclareLaunchArgument(
+            'frame_prefix',
+            default_value='',
+            description='Optional TF frame prefix for multi-robot use'),
         Node(
             package='robot_state_publisher',
             executable='robot_state_publisher',
             name='robot_state_publisher',
             output='screen',
+            condition=IfCondition(PythonExpression(["'", frame_prefix, "' == ''"])),
+            parameters=[{
+                'use_sim_time': use_sim_time,
+                'robot_description': robot_desc
+            }],
+        ),
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            condition=UnlessCondition(PythonExpression(["'", frame_prefix, "' == ''"])),
             parameters=[{
                 'use_sim_time': use_sim_time,
                 'robot_description': robot_desc,

src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_burger/model_gz.sdf

@@ -315,6 +315,7 @@
                  Gz Sim uses raycasting rendering so it is NOT needed here.
            Note: min_depth is replaced by <clip><near>=0.02 for minimum sensing distance. -->
       <sensor name="intel_realsense_r200_depth" type="depth">
+        <gz_frame_id>realsense_depth_frame</gz_frame_id>
         <always_on>1</always_on>
         <update_rate>30</update_rate>
         <pose>0.076 0.0 0.093 0 0 0</pose>
@@ -341,6 +342,7 @@
            Original plugin had: hack_baseline=0.07 (stereo baseline)
            Note: Not applicable in Gz Sim native sensor (same reason as depth camera above). -->
       <sensor name="intel_realsense_r200_rgb" type="camera">
+        <gz_frame_id>camera_rgb_frame</gz_frame_id>
         <always_on>true</always_on>
         <visualize>true</visualize>
         <update_rate>30</update_rate>