From 6e9342014f7e939f5f16dba75b8dabe87f4bf839 Mon Sep 17 00:00:00 2001 From: Xu Shiyuan Date: Fri, 8 May 2026 20:51:19 +0800 Subject: [PATCH] =?UTF-8?q?acml=E5=88=9D=E5=A7=8B=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 6 +- ...p_localization_with_initial_pose.launch.py | 34 ++++------ .../scripts/publish_initial_pose.py | 66 ------------------ .../launch/robot_state_publisher.launch.py | 67 ++++++++++++------- .../models/turtlebot3_burger/model_gz.sdf | 10 +-- 5 files changed, 64 insertions(+), 119 deletions(-) delete mode 100755 src/TurtleBot-RRT-Star/scripts/publish_initial_pose.py diff --git a/README.md b/README.md index 6710477..07244b6 100644 --- a/README.md +++ b/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 \ - map:=$NAV2_MAP_PATH + params_file:=$HOME/ros_ws/src/vlm-semantic-nav2/TurtleBot-RRT-Star/nav2_params.yaml ros2 run rviz2 rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz --ros-args -p use_sim_time:=true ``` diff --git a/src/TurtleBot-RRT-Star/launch/bringup_localization_with_initial_pose.launch.py b/src/TurtleBot-RRT-Star/launch/bringup_localization_with_initial_pose.launch.py index a684cb0..d493aa5 100644 --- a/src/TurtleBot-RRT-Star/launch/bringup_localization_with_initial_pose.launch.py +++ b/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. -No need to manually click 2D Pose Estimate in RViz. +Launch localization with AMCL configured by the provided Nav2 params file. """ 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( - get_package_share_directory('nav2_rrtstar_planner'), - 'scripts', - 'publish_initial_pose.py' + default_params_file = os.path.join( + bringup_dir, 'params', 'nav2_params.yaml' ) # 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 diff --git a/src/TurtleBot-RRT-Star/scripts/publish_initial_pose.py b/src/TurtleBot-RRT-Star/scripts/publish_initial_pose.py deleted file mode 100755 index 4bf9f98..0000000 --- a/src/TurtleBot-RRT-Star/scripts/publish_initial_pose.py +++ /dev/null @@ -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() diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/robot_state_publisher.launch.py b/src/turtlebot3_simulations/turtlebot3_gazebo/launch/robot_state_publisher.launch.py index a4827da..c9003e3 100755 --- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/robot_state_publisher.launch.py +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/launch/robot_state_publisher.launch.py @@ -16,14 +16,15 @@ # # Authors: Darby Lim -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression -from launch_ros.actions import Node +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 +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([ - DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use simulation (Gazebo) clock if true'), - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time, - 'robot_description': robot_desc, - 'frame_prefix': PythonExpression(["'", frame_prefix, "/'"]) - }], - ), - ]) + return LaunchDescription([ + DeclareLaunchArgument( + '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, + 'frame_prefix': PythonExpression(["'", frame_prefix, "/'"]) + }], + ), + ]) diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_burger/model_gz.sdf b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_burger/model_gz.sdf index 0112d30..267d3d9 100755 --- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_burger/model_gz.sdf +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_burger/model_gz.sdf @@ -288,7 +288,7 @@ - 0.076 0.0 0.093 0 0 0 + 0 0 0 0 0 0 0.001 0.000 @@ -300,14 +300,14 @@ 0.035 - 0 0.047 0 0 0 0 + 0.003 0.065 0.007 0 0 0 - 0.008 0.130 0.022 + 0.012 0.132 0.020 - 0.076 0 0.093 0 0 0 + 0.040 0 0.110 0 0 0 @@ -368,7 +368,7 @@ base_link realsense_link - 0.076 0.0 0.093 0 0 0 + 0.040 0.0 0.110 0 0 0