sl-perception
5dcaa7bd49
Implements Phase 3 ride-once-replay-forever route system. saltybot_routes package: - route_recorder_node: samples GPS+odom+heading at 1Hz during follow-me rides; 2m waypoint spacing; JSON-Lines .jsonl on NVMe /data/routes/; services start_recording/stop_recording/save/discard - route_replayer_node: loads .jsonl, GPS->ENU flat-earth conversion, heading->quaternion, 3m subsampling for Nav2 navigate_through_poses; 2m GPS tolerance (SIM7600X +-2.5m); pause/resume/stop services - route_manager_node: list/info/delete services for saved routes - route_system.launch.py: all three nodes with shared params - route_params.yaml: waypoint_spacing_m=2.0, replay_spacing_m=3.0 GPS: /gps/fix from SIM7600X (PR #65) UWB: /uwb/target from follow-me (PR #66) Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
100 lines
3.1 KiB
Python
100 lines
3.1 KiB
Python
"""
|
|
route_system.launch.py — Route recording and replay system for SaltyBot
|
|
|
|
Starts:
|
|
route_recorder_node — records GPS + odom path during follow-me rides
|
|
route_replayer_node — replays recorded routes autonomously via Nav2
|
|
route_manager_node — list/delete/info for saved routes
|
|
|
|
Depends on:
|
|
saltybot-nav2 container (Nav2 action server /navigate_through_poses)
|
|
saltybot_cellular (/gps/fix from SIM7600X GPS — PR #65)
|
|
saltybot_uwb (/uwb/target — PR #66, used for context during recording)
|
|
STM32 bridge (/odom from wheel encoders)
|
|
D435i (/imu/data for heading)
|
|
|
|
Usage — record a route:
|
|
# Set name, start recording, ride with Tee, stop and save:
|
|
ros2 param set /route_recorder route_name "friday_loop"
|
|
ros2 service call /route/start_recording std_srvs/srv/Trigger '{}'
|
|
# ... ride the route ...
|
|
ros2 service call /route/stop_recording std_srvs/srv/Trigger '{}'
|
|
ros2 service call /route/save std_srvs/srv/Trigger '{}'
|
|
|
|
Usage — replay a route:
|
|
ros2 param set /route_replayer route_name "friday_loop"
|
|
ros2 service call /route/load std_srvs/srv/Trigger '{}'
|
|
ros2 service call /route/start_replay std_srvs/srv/Trigger '{}'
|
|
|
|
Arguments:
|
|
save_dir (default /data/routes) — NVMe route storage
|
|
route_name (default route) — name for record/replay
|
|
"""
|
|
|
|
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_ros.actions import Node
|
|
|
|
|
|
def generate_launch_description():
|
|
pkg_dir = get_package_share_directory('saltybot_routes')
|
|
params_file = os.path.join(pkg_dir, 'config', 'route_params.yaml')
|
|
|
|
args = [
|
|
DeclareLaunchArgument('save_dir', default_value='/data/routes'),
|
|
DeclareLaunchArgument('route_name', default_value='route'),
|
|
]
|
|
|
|
recorder = Node(
|
|
package='saltybot_routes',
|
|
executable='route_recorder_node',
|
|
name='route_recorder',
|
|
output='screen',
|
|
parameters=[
|
|
params_file,
|
|
{
|
|
'save_dir': LaunchConfiguration('save_dir'),
|
|
'route_name': LaunchConfiguration('route_name'),
|
|
},
|
|
],
|
|
)
|
|
|
|
replayer = Node(
|
|
package='saltybot_routes',
|
|
executable='route_replayer_node',
|
|
name='route_replayer',
|
|
output='screen',
|
|
parameters=[
|
|
params_file,
|
|
{
|
|
'save_dir': LaunchConfiguration('save_dir'),
|
|
'route_name': LaunchConfiguration('route_name'),
|
|
},
|
|
],
|
|
)
|
|
|
|
manager = Node(
|
|
package='saltybot_routes',
|
|
executable='route_manager_node',
|
|
name='route_manager',
|
|
output='screen',
|
|
parameters=[
|
|
params_file,
|
|
{
|
|
'save_dir': LaunchConfiguration('save_dir'),
|
|
'route_name': LaunchConfiguration('route_name'),
|
|
},
|
|
],
|
|
)
|
|
|
|
return LaunchDescription([
|
|
*args,
|
|
recorder,
|
|
replayer,
|
|
manager,
|
|
])
|