fengyujia
/
office_RRT_star
forked from hq/office_RRT_star
1
0
Fork 0
Code Pull Requests Activity

acml初始化

This commit is contained in:
Xu Shiyuan 2026-05-08 20:51:19 +08:00
parent a4e7060ce3
commit 6e9342014f
5 changed files with 64 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
README.md
View File

@ -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
```

34
src/TurtleBot-RRT-Star/launch/bringup_localization_with_initial_pose.launch.py
View File

@ -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

66
src/TurtleBot-RRT-Star/scripts/publish_initial_pose.py
View File

@ -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()

67
src/turtlebot3_simulations/turtlebot3_gazebo/launch/robot_state_publisher.launch.py
View File

@ -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, "/'"])
}],
),
])

10
src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_burger/model_gz.sdf
View File

@ -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 -->