Autoware.Auto
Record/Replay Planner

Table of Contents

Record/Replay a Trajectory in LGSVL

Autoware.Auto is capable of recording a path of waypoints to disk and then loading and attempting to follow that path. This demonstration combines several subsystems including NDT Localization and the 3D perception stack. To test this functionality, do the following:

Prerequisites

The following instructions assume you are running the demo inside of ADE. For instructions on setting up an ADE environment, see Installation with ADE. For instructions on setting up Autoware.Auto without ADE, see Installation w/o ADE. If running outside of ADE, replace source /opt/AutowareAuto/setup.bash with source /<path_to_your_autoware_folder>/install/setup.bash.

Instructions

  • Launch the LGSVL simulator with the Lexus RX 450h and the AutonomouStuff Parking Lot map. Do not start the simulation at this point.
  • In a new terminal:
    ade$ source /opt/AutowareAuto/setup.bash
    ade$ ros2 launch autoware_auto_avp_demo recordreplay_planner_demo.launch.py

In rviz, locate the spawn point of the vehicle in the AutonomouStuff parking lot map using the point cloud as a reference.

recordreplay_spawn_point.png
LGSVL Spawn Point

Use the "2D Pose Estimate" tool in rviz to provide an initial pose estimate for localization.

recordreplay_pose_estimate.jpeg
Initial Pose Estimate

Using the LGSVL web interface, start the simulation. Once localization has begun estimating the vehicle's position, the view in rviz will jump away from the vehicle. Re-center the view on the vehicle. In a new terminal:

ade$ source /opt/AutowareAuto/setup.bash
ade$ ros2 action send_goal /planning/recordtrajectory recordreplay_planner_actions/action/RecordTrajectory "{record_path: "/tmp/path"}" --feedback

In LGSVL, drive the vehicle around to record the path. When finished recording, go to the terminal in which you ran the ros2 action send_goal command and hit CTRL+C to stop recording. In LGSVL, hit F12 to re-center the vehicle at the default spawn point. You will then need to re-initialize localization at the new location with the 2D Pose Estimate tool in rviz. To replay the recorded path:

ade$ source /opt/AutowareAuto/setup.bash
ade$ ros2 action send_goal /planning/replaytrajectory recordreplay_planner_actions/action/ReplayTrajectory "{replay_path: "/tmp/path"}" --feedback

When the vehicle reaches to the goal of the replayed trajectory, the planner stops automatically and outputs status::SUCCEED to the terminal in which you ran the ros2 action send_goal command. You can modify end conditions by tuning parameters in recordreplay_planner.param.yaml. The planner terminates planning when both of the following conditions are satisfied:

  • goal_distance_threshold_m: threshold for the distance between nav_base frame and the last point in the replayed trajectory
  • goal_angle_threshold_rad: threshold for the heading angle between nav_base frame and the last point in the replayed trajectory