Method 1: using Rosbag, PCD Map and NDT Matching
- Load all the nodes required by NDT (
map, nmea*, tf, voxel, ndt_matching
) and verify ndt is working - Launch
vel_pose_connect
current_pose: /ndt_pose
current_velocity/estimate_twist

- Launch
waypoint_saver
[X] Check Save /current_velocity
Interval 1 (store a waypoint every meter)

- In Rviz add the topic
waypoint_saver_marker
- Play the rosbag, the path followed by NDT will be stored as a series of waypoints in the specified CSV file, in the same coordinates as the PCD map’s frame
Method 2: Using Vector Map, TF, Lane and RViz’s Publish Point
- Launch
waypoint_clicker
velocity
output file
- In Rviz add the topic
/waypoint_guide

Small points belonging to the lanes will be displayed.
- Use the Published Point in Rviz to click the desired points in the lane to create a path. (Points can only be added following the lane direction)
Load the path to confirm the file
The file can be loaded using a text editor.
- Select the csv file and launch
waypoint_loader

- In Rviz add the
/global_waypoints_marker
