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