acml初始化

README.md

@@ -43,12 +43,12 @@ source ~/.bashrc
 
 ros2 launch nav2_rrtstar_planner bringup_localization_with_initial_pose.launch.py \
     use_sim_time:=true \
-    map:=$NAV2_MAP_PATH
+    map:=$NAV2_MAP_PATH \
+    params_file:=$HOME/ros_ws/src/vlm-semantic-nav2/TurtleBot-RRT-Star/nav2_params.yaml
 
 ros2 launch nav2_bringup navigation_launch.py \
     use_sim_time:=True \
-    params_file:=/workspace/src/TurtleBot-RRT-Star/nav2_params.yaml \
+    params_file:=$HOME/ros_ws/src/vlm-semantic-nav2/TurtleBot-RRT-Star/nav2_params.yaml
-    map:=$NAV2_MAP_PATH
  
 ros2 run rviz2 rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz --ros-args -p use_sim_time:=true
 ```

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

@@ -16,14 +16,15 @@
 #
 # Authors: Darby Lim
 
-import os
+import os
-
+
-from ament_index_python.packages import get_package_share_directory
+from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
+from launch.conditions import IfCondition, UnlessCondition
-from launch.actions import DeclareLaunchArgument
+from launch import LaunchDescription
-from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
-from launch.substitutions import PythonExpression
+from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
+from launch.substitutions import PythonExpression
+from launch_ros.actions import Node
 
 
 def generate_launch_description():
@@ -43,20 +44,36 @@ def generate_launch_description():
     with open(urdf_path, 'r') as infp:
         robot_desc = infp.read()
 
-    return LaunchDescription([
+    return LaunchDescription([
-        DeclareLaunchArgument(
+        DeclareLaunchArgument(
-            'use_sim_time',
+            'use_sim_time',
-            default_value='false',
+            default_value='false',
-            description='Use simulation (Gazebo) clock if true'),
+            description='Use simulation (Gazebo) clock if true'),
-        Node(
+        DeclareLaunchArgument(
-            package='robot_state_publisher',
+            'frame_prefix',
-            executable='robot_state_publisher',
+            default_value='',
-            name='robot_state_publisher',
+            description='Optional TF frame prefix for multi-robot use'),
-            output='screen',
+        Node(
-            parameters=[{
+            package='robot_state_publisher',
-                'use_sim_time': use_sim_time,
+            executable='robot_state_publisher',
-                'robot_description': robot_desc,
+            name='robot_state_publisher',
-                'frame_prefix': PythonExpression(["'", frame_prefix, "/'"])
+            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,
+                'frame_prefix': PythonExpression(["'", frame_prefix, "/'"])
+            }],
+        ),
+    ])

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

@@ -288,7 +288,7 @@
 
     <link name="realsense_link">
       <inertial>
-        <pose>0.076 0.0 0.093 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <inertia>
           <ixx>0.001</ixx>
           <ixy>0.000</ixy>
@@ -300,14 +300,14 @@
         <mass>0.035</mass>
       </inertial>
       <collision name="collision">
-        <pose>0 0.047 0 0 0 0</pose>
+        <pose>0.003 0.065 0.007 0 0 0</pose>
         <geometry>
           <box>
-            <size>0.008 0.130 0.022</size>
+            <size>0.012 0.132 0.020</size>
           </box>
         </geometry>
       </collision>
-      <pose>0.076 0 0.093 0 0 0</pose>
+      <pose>0.040 0 0.110 0 0 0</pose>
     </link>
 
     <joint name="base_joint" type="fixed">
@@ -368,7 +368,7 @@
     <joint name="camera_joint" type="fixed">
       <parent>base_link</parent>
       <child>realsense_link</child>
-      <pose>0.076 0.0 0.093 0 0 0</pose>
+      <pose>0.040 0.0 0.110 0 0 0</pose>
     </joint>
 
     <!-- Diff Drive plugin: migrated from libgazebo_ros_diff_drive.so to gz-sim system -->