add waypoint_maker from ros. add change autoware code ,skip lat/lon to x/y, for test route.

src/ros/catkin/src/lane_planner/nodes/lane_navi/lane_navi.cpp

@@ -24,6 +24,12 @@
 
 #include "lane_planner/lane_planner_vmap.hpp"
 
+
+bool bpointok = false;
+bool bnodeok = false;
+bool blaneok = false;
+
+
 namespace {
 
 int waypoint_max;
@@ -32,6 +38,8 @@ double velocity; // km/h
 std::string frame_id;
 std::string output_file;
 
+
+
 ros::Publisher waypoint_pub;
 
 lane_planner::vmap::VectorMap all_vmap;
@@ -85,19 +93,28 @@ void create_waypoint(const tablet_socket_msgs::route_cmd& msg)
   if (all_vmap.points.empty() || all_vmap.lanes.empty() || all_vmap.nodes.empty()) {
     cached_route.header = header;
     cached_route.point = msg.point;
+    std::cout<<" map not ok"<<std::endl;
     return;
   }
 
   lane_planner::vmap::VectorMap coarse_vmap = lane_planner::vmap::create_coarse_vmap_from_route(msg);
   if (coarse_vmap.points.size() < 2)
+  {
+    std::cout<<" coarse vmap not ok. "<<std::endl;
     return;
+  }
+
+
 
   std::vector<lane_planner::vmap::VectorMap> fine_vmaps;
   lane_planner::vmap::VectorMap fine_mostleft_vmap =
     lane_planner::vmap::create_fine_vmap(lane_vmap, lane_planner::vmap::LNO_MOSTLEFT, coarse_vmap,
                  search_radius, waypoint_max);
   if (fine_mostleft_vmap.points.size() < 2)
+  {
+    std::cout<<" mostleft < 2  return "<<std::endl;
     return;
+  }
   fine_vmaps.push_back(fine_mostleft_vmap);
 
   int lcnt = count_lane(fine_mostleft_vmap);
@@ -107,6 +124,7 @@ void create_waypoint(const tablet_socket_msgs::route_cmd& msg)
     if (v.points.size() < 2)
       continue;
     fine_vmaps.push_back(v);
+
   }
 
   autoware_msgs::LaneArray lane_waypoint;
@@ -143,6 +161,8 @@ void create_waypoint(const tablet_socket_msgs::route_cmd& msg)
     }
     lane_waypoint.lanes.push_back(l);
   }
+
+    std::cout<<" size "<<lane_waypoint.lanes.size() <<std::endl;
   waypoint_pub.publish(lane_waypoint);
 
   for (size_t i = 0; i < fine_vmaps.size(); ++i) {
@@ -177,22 +197,48 @@ void cache_point(const vector_map::PointArray& msg)
 {
   all_vmap.points = msg.data;
   update_values();
+  bpointok = true;
 }
 
 void cache_lane(const vector_map::LaneArray& msg)
 {
   all_vmap.lanes = msg.data;
   update_values();
+  blaneok = true;
 }
 
 void cache_node(const vector_map::NodeArray& msg)
 {
   all_vmap.nodes = msg.data;
   update_values();
+  bnodeok = true;
 }
 
 } // namespace
 
+#include <thread>
+void ThreadTest()
+{
+    while((bpointok == false)||(bnodeok == false) ||(blaneok == false))
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    }
+
+    std::cout<<" All map data ok"<<std::endl;
+
+      tablet_socket_msgs::route_cmd xc;
+ tablet_socket_msgs::Waypoint p1;
+  p1.lat = 35;
+  p1.lon = -9;
+ xc.point.push_back(p1);
+  tablet_socket_msgs::Waypoint p2;
+  p2.lat = -155;//-68;
+  p2.lon = -68;//-155;
+  xc.point.push_back(p2);
+
+  create_waypoint(xc);
+}
+
 int main(int argc, char **argv)
 {
   ros::init(argc, argv, "lane_navi");
@@ -231,6 +277,10 @@ int main(int argc, char **argv)
   ros::Subscriber lane_sub = n.subscribe("/vector_map_info/lane", sub_vmap_queue_size, cache_lane);
   ros::Subscriber node_sub = n.subscribe("/vector_map_info/node", sub_vmap_queue_size, cache_node);
 
+
+void * pthead = new std::thread(ThreadTest);
+   
+
   ros::spin();
 
   return EXIT_SUCCESS;

src/ros/catkin/src/waypoint_maker/CHANGELOG.rst

@@ -0,0 +1,480 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package waypoint_maker
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* [Feature] Improve Hybrid A* planner (`#1594 <https://github.com/CPFL/Autoware/issues/1594>`_)
+  * Delete obstacle_sim from astar_planner package, replaced to lidar_fake_perception
+  * Modify package name, astar_planner -> waypoint_planner, and create astar_planner library package
+  * Delete obstacle_avoid/astar* and modify its dependency to astar_planner library
+  * Fix astar_navi with astar_planner library
+  * Refactor astar_navi by separating HAstar library and fixing coodinate system
+  * Rename obstacle_avoid -> astar_avoid and under refactoring
+  * Fix cost function and configures
+  * Fix backward search and refactor configurations
+  * Apply clang-format
+  * Refactor include
+  * Fix typo and so on
+  * Improve astar_avoid by incremental goal search
+  * Apply clang-format
+  * Revert package names
+  * Fix package/code names
+  * Update runtime_manager
+  * Improve astar_avoid to execute avoidance behavior by state transition (by rebuild decision maker)
+  * Fix PascalCase message names by `#1408 <https://github.com/CPFL/Autoware/issues/1408>`_
+  * Remove obstacle_avoid directory
+  * Fix default parameter for costmap topic
+  * Fix warning and initialize condition
+  * Remove use_avoidance_state mode (TODO: after merging rebuild decision maker)
+  * Improve astar_avoid behavior by simple state transition and multi-threading
+  * Apply clang-format
+  * Fix replan_interval in astar_avoid
+  * Add descriptions for paramters
+  * Rename pkg name, astar_planner -> waypoint_planner
+  * Fix param name
+  * Fix avoid waypoints height
+  * Fix parameter and formalize code
+  * Add README for freespace/waypoint_planner
+  * Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Fix CHANGELOG
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  * Fix astar_navi/README.md
+  * Add License terms
+  * Fix const pointer
+  * Added unit test base
+  * Restructured folders
+  * Fix bug by adding AstarSearch reset
+  * Fix WaveFrontNode initialization
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  * Fix variable name
+  * Refactor threading
+  * Re-adding lidar_fake_perception
+  * Fix the condition to judge reaching goal
+  * Add 'use_decision state' mode to transit avoidance state by decision_maker
+  * Fix calcDiffOfRadian (if diff > 2pi)
+  * Feature/test astar planner (`#1753 <https://github.com/CPFL/Autoware/issues/1753>`_)
+  * Restructured folders
+  * Added unit test base
+  * Removed remaining folder
+  * Test WIP
+  * Added astar_util tests and base file for astar_search tests
+  * Updated to ROS Cpp Style guidelines
+  * Added test for SimpleNode constructor
+  * Updated Copyright date
+  * Added tests for astar algorithm
+  * Added default constructor to WaveFront struct
+  * Revert use_state_decision mode (94af7b6)
+  * Fix costmap topic names by merging costmap_generator
+* [fix] Install commands for all the packages (`#1861 <https://github.com/CPFL/Autoware/issues/1861>`_)
+  * Initial fixes to detection, sensing, semantics and utils
+  * fixing wrong filename on install command
+  * Fixes to install commands
+  * Hokuyo fix name
+  * Fix obj db
+  * Obj db include fixes
+  * End of final cleaning sweep
+  * Incorrect command order in runtime manager
+  * Param tempfile not required by runtime_manager
+  * * Fixes to runtime manager install commands
+  * Remove devel directory from catkin, if any
+  * Updated launch files for robosense
+  * Updated robosense
+  * Fix/add missing install (`#1977 <https://github.com/CPFL/Autoware/issues/1977>`_)
+  * Added launch install to lidar_kf_contour_track
+  * Added install to op_global_planner
+  * Added install to way_planner
+  * Added install to op_local_planner
+  * Added install to op_simulation_package
+  * Added install to op_utilities
+  * Added install to sync
+  * * Improved installation script for pointgrey packages
+  * Fixed nodelet error for gmsl cameras
+  * USe install space in catkin as well
+  * add install to catkin
+  * Fix install directives (`#1990 <https://github.com/CPFL/Autoware/issues/1990>`_)
+  * Fixed installation path
+  * Fixed params installation path
+  * Fixed cfg installation path
+  * Delete cache on colcon_release
+* [Feature] waypoint replanner and extractor (`#1951 <https://github.com/CPFL/Autoware/issues/1951>`_)
+* Fix license notice in corresponding package.xml
+* Cleanup WaypointState.msg and add sttering_state=STR_BACK (`#1822 <https://github.com/CPFL/Autoware/issues/1822>`_)
+* Contributors: Abraham Monrroy Cano, Akihito Ohsato, Kenji Funaoka, Yuma Nihei, amc-nu
+
+1.10.0 (2019-01-17)
+-------------------
+* Fixes for catkin_make
+* Switch to Apache 2 license (develop branch) (`#1741 <https://github.com/CPFL/Autoware/issues/1741>`_)
+  * Switch to Apache 2
+  * Replace BSD-3 license header with Apache 2 and reassign copyright to the
+  Autoware Foundation.
+  * Update license on Python files
+  * Update copyright years
+  * Add #ifndef/define _POINTS_IMAGE_H\_
+  * Updated license comment
+* Use colcon as the build tool (`#1704 <https://github.com/CPFL/Autoware/issues/1704>`_)
+  * Switch to colcon as the build tool instead of catkin
+  * Added cmake-target
+  * Added note about the second colcon call
+  * Added warning about catkin* scripts being deprecated
+  * Fix COLCON_OPTS
+  * Added install targets
+  * Update Docker image tags
+  * Message packages fixes
+  * Fix missing dependency
+* Contributors: Esteve Fernandez, amc-nu
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+* Moved configuration messages to autoware_config_msgs
+* [fix] PascalCase messages (`#1408 <https://github.com/CPFL/Autoware/issues/1408>`_)
+  * Switch message files to pascal case
+  * Switch message names to pascal case in Runtime Manager
+  * Switch message names to pascal case in *.yaml
+  * Rename brake_cmd and steer_cmd to BrakeCmd and SteerCmd in main.yaml
+* Contributors: Esteve Fernandez
+
+1.8.0 (2018-08-31)
+------------------
+* [Fix] Moved C++11 flag to autoware_build_flags (`#1395 <https://github.com/CPFL/Autoware/pull/1395>`_)
+* [Feature] Makes sure that all binaries have their dependencies linked (`#1385 <https://github.com/CPFL/Autoware/pull/1385>`_)
+* [Fix] Extend and Update interface.yaml (`#1291 <https://github.com/CPFL/Autoware/pull/1291>`_)
+* Contributors: Esteve Fernandez, Kenji Funaoka
+
+1.7.0 (2018-05-18)
+------------------
+* Fix flashing marker & set lifetime
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* Update README
+* Fix date of lisence & add {}
+* Return disable_decision_maker to rosparam
+* Clang-format
+* Split resample method
+* Split the velocity limit method.
+* Move calculate method of velocity param to init_config
+* Optimize accel_decel_limitation_method
+* Optimize radius-calculate-method
+* Rename waypoint_replan to velocity_replanner
+* Fix curve_radius_method on resampling_mode
+* Rename cpp and header_files filter->replan
+* Rename class and functions filter->replan
+* Fix claculate radius function
+* Fix CalcVelParam func & add comment
+* Modify visualized topic, /base_waypoints -> /lane_waypoints_array, mainly for editing waypoints
+* Fix to deleting marker before re-publishing
+* Fix bug, increasing path size for each loop
+* [fix] Fixes for all packages and dependencies (`#1240 <https://github.com/CPFL/Autoware/pull/1240>`_)
+  * Initial Cleanup
+  * fixed also for indigo
+  * kf cjeck
+  * Fix road wizard
+  * Added travis ci
+  * Trigger CI
+  * Fixes to cv_tracker and lidar_tracker cmake
+  * Fix kitti player dependencies
+  * Removed unnecessary dependencies
+  * messages fixing for can
+  * Update build script travis
+  * Travis Path
+  * Travis Paths fix
+  * Travis test
+  * Eigen checks
+  * removed unnecessary dependencies
+  * Eigen Detection
+  * Job number reduced
+  * Eigen3 more fixes
+  * More Eigen3
+  * Even more Eigen
+  * find package cmake modules included
+  * More fixes to cmake modules
+  * Removed non ros dependency
+  * Enable industrial_ci for indidog and kinetic
+  * Wrong install command
+  * fix rviz_plugin install
+  * FastVirtualScan fix
+  * Fix Qt5 Fastvirtualscan
+  * Fixed qt5 system dependencies for rosdep
+  * NDT TKU Fix catkin not pacakged
+  * More in detail dependencies fixes for more packages
+  * GLEW library for ORB
+  * Ignore OrbLocalizer
+  * Ignore Version checker
+  * Fix for driveworks interface
+  * driveworks not catkinpackagedd
+  * Missing catkin for driveworks
+  * libdpm opencv not catkin packaged
+  * catkin lib gnss  not included in obj_db
+  * Points2Polygon fix
+  * More missing dependencies
+  * image viewer not packaged
+  * Fixed SSH2 detection, added viewers for all distros
+  * Fix gnss localizer incorrect dependency config
+  * Fixes to multiple packages dependencies
+  * gnss plib and package
+  * More fixes to gnss
+  * gnss dependencies for gnss_loclaizer
+  * Missing gnss dependency for gnss on localizer
+  * More fixes for dependencies
+  Replaced gnss for autoware_gnss_library
+  * gnss more fixes
+  * fixes to more dependencies
+  * header dependency
+  * Debug message
+  * more debug messages changed back to gnss
+  * debud messages
+  * gnss test
+  * gnss install command
+  * Several fixes for OpenPlanner and its lbiraries
+  * Fixes to ROSInterface
+  * More fixes to robotsdk and rosinterface
+  * robotsdk calibration fix
+  * Fixes to rosinterface robotsdk libraries and its nodes
+  * Fixes to Qt5 missing dependencies in robotsdk
+  * glviewer missing dependencies
+  * Missing qt specific config cmake for robotsdk
+  * disable cv_tracker
+  * Fix to open planner un needed dependendecies
+  * Fixes for libraries indecision maker
+  * Fixes to libraries decision_maker installation
+  * Gazebo on Kinetic
+  * Added Missing library
+  * * Removed Gazebo and synchonization packages
+  * Renames vmap in lane_planner
+  * Added installation commands for missing pakcages
+  * Fixes to lane_planner
+  * Added NDT TKU Glut extra dependencies
+  * ndt localizer/lib fast pcl fixes
+  re enable cv_tracker
+  * Fix kf_lib
+  * Keep industrial_ci
+  * Fixes for dpm library
+  * Fusion lib fixed
+  * dpm and fusion header should match exported project name
+  * Fixes to dpm_ocv  ndt_localizer and pcl_omp
+  * no fast_pcl anymore
+  * fixes to libdpm and its package
+  * CI test
+  * test with native travis ci
+  * missing update for apt
+  * Fixes to pcl_omp installation and headers
+  * Final fixes for tests, modified README
+  * * Fixes to README
+  * Enable industrial_ci
+  * re enable native travis tests
+* Add end point offset option
+* retry Clang-format
+* Revert "Clang-format"
+  This reverts commit fb284523027c0df1ec513b09c6426c9d235c9f12.
+* Ignore space character of waypoints.csv
+* Reflect waypoints height updown
+* Fix/cmake cleanup (`#1156 <https://github.com/CPFL/Autoware/pull/1156>`_)
+  * Initial Cleanup
+  * fixed also for indigo
+  * kf cjeck
+  * Fix road wizard
+  * Added travis ci
+  * Trigger CI
+  * Fixes to cv_tracker and lidar_tracker cmake
+  * Fix kitti player dependencies
+  * Removed unnecessary dependencies
+  * messages fixing for can
+  * Update build script travis
+  * Travis Path
+  * Travis Paths fix
+  * Travis test
+  * Eigen checks
+  * removed unnecessary dependencies
+  * Eigen Detection
+  * Job number reduced
+  * Eigen3 more fixes
+  * More Eigen3
+  * Even more Eigen
+  * find package cmake modules included
+  * More fixes to cmake modules
+  * Removed non ros dependency
+  * Enable industrial_ci for indidog and kinetic
+  * Wrong install command
+  * fix rviz_plugin install
+  * FastVirtualScan fix
+  * Fix Qt5 Fastvirtualscan
+  * Fixed qt5 system dependencies for rosdep
+  * NDT TKU Fix catkin not pacakged
+  * Fixes from industrial_ci
+* Clang-format
+* Correspond to new version of waypoint_csv(for decision_maker)
+* fix runtime_manager layout and description
+* Add config_callback for online waypoint tuning
+* Add velocity plan offset for system delay
+* fix launch files for ros parameter
+* Add waypoint_filter functions
+* Fixing `#1064 <https://github.com/CPFL/Autoware/pull/1064>`_ and `#1065 <https://github.com/CPFL/Autoware/pull/1065>`_
+* Add deleting marker/buffers for replaying rosbag, and some fix
+* Apply clang-format
+* Fix typo
+* Modify plotting of controller response by metric interval
+* Add some rosparam and fix launch file
+* Add velocity visualize as facing text
+* Fix circular buffer initialization
+* Refactor using boost::circular_buffer, ApproximateTimeSyncPolicy, and so on
+* Separate speed vizualizer -> waypoint_marker/waypoint_velocity_vizualizer
+* Add current_velocity and twist_command line graph
+* Remove unused comments
+* Add velocity visualizer with 3D graph.
+* Contributors: AMC, Abraham Monrroy, Akihito Ohsato, Kosuke Murakami, Yuma, Yuma Nihei
+
+1.6.3 (2018-03-06)
+------------------
+
+1.6.2 (2018-02-27)
+------------------
+* Update CHANGELOG
+* Contributors: Yusuke FUJII
+
+1.6.1 (2018-01-20)
+------------------
+* update CHANGELOG
+* Contributors: Yusuke FUJII
+
+1.6.0 (2017-12-11)
+------------------
+* Prepare release for 1.6.0
+* add smoothing on waypoint loader
+* add waypoint relay
+* Contributors: Yamato ANDO, Yusuke FUJII
+
+1.5.1 (2017-09-25)
+------------------
+* Release/1.5.1 (`#816 <https://github.com/cpfl/autoware/issues/816>`_)
+  * fix a build error by gcc version
+  * fix build error for older indigo version
+  * update changelog for v1.5.1
+  * 1.5.1
+* Contributors: Yusuke FUJII
+
+1.5.0 (2017-09-21)
+------------------
+* Update changelog
+* Contributors: Yusuke FUJII
+
+1.4.0 (2017-08-04)
+------------------
+* version number must equal current release number so we can start releasing in the future
+* added changelogs
+* Contributors: Dejan Pangercic
+
+1.3.1 (2017-07-16)
+------------------
+
+1.3.0 (2017-07-14)
+------------------
+* Add obstacle avoid feature in waypoint_planner
+* convert to autoware_msgs
+* Contributors: TomohitoAndo, YamatoAndo
+
+1.2.0 (2017-06-07)
+------------------
+* fix circular-dependency
+* Contributors: Shohei Fujii
+
+1.1.2 (2017-02-27 23:10)
+------------------------
+
+1.1.1 (2017-02-27 22:25)
+------------------------
+
+1.1.0 (2017-02-24)
+------------------
+* Publish local waypoint velocity
+* Update interface.yaml for each packages
+* Update README.md for waypoint_maker
+* Add the function, velocity plannning, for format ver2 and 3
+* initial commit for README.md for each packages
+* Fix not using reserved word in C++
+* Comment out conflict part in visualization, Add Local Point Marker
+* Apply clang-format
+* extract processing as function
+* Rename function
+* Add enum class "ChangeFlag"
+* Rewrite waypoint_loader
+* Add visualization for change flag
+* Adjust for new fileformat
+* Add checkFileFormat() function
+* Add g\_ prefix to global variables
+* Add support for multi lane files
+* Add no name namespame instead of using static modifier
+* Contributors: TomohitoAndo, h_ohta
+
+1.0.1 (2017-01-14)
+------------------
+
+1.0.0 (2016-12-22)
+------------------
+* Run visualization node when astar_navi is launched
+* Publish marker when the traffic light detection is unknown
+* Fix codes to use map_file messages and old vector_map_info topics
+* Change message type for current velocity , Vector3stamepd -> TwistStamped
+* Use clang-format
+* Accomodate to vel_pose_mux
+* Add module graph tool
+* Remove needless compiling flags
+* Divide waypoints marker into global and local
+* Fix code style
+* Delete static modifier,Add noname namespace
+* Switch signal detection source by Runtime Manager configuration
+* Avoid segmentation fault when parsing waypoint file
+* Create verifyFileConsistency function
+* Fix some place
+* Fix Node name
+* Parse old CSV format
+* Compute yaw in lane_navi and waypoint_clicker
+* Add debug code ,checking the orientation of waypoint
+* Delete needless code
+* Fix style
+* Add Markers which show traffic_waypoints_array
+* Rewrite waypoint_clicker by new API
+* Change to show LaneArray
+* Some Changes
+* Load two lanes from csv files
+* Change Marker style
+* Bug fix
+* changed to use yaw in a waypoint
+* added yaw in waypoint data
+* Make junction more visible
+* Show guides for the waypoint_clicker
+  The waypoint_clicker have clicked a waypoint freehand so far.
+  This commit show guides of waypoint, junction, clicked point and found route.
+* Add dependent packages
+* modified somethings in computing tab
+* Use c++11 option instead of c++0x
+  We can use newer compilers which support 'c++11' option
+* bug fix
+* some fix
+* published local path marker ,and some fix in order to be easy to see
+* published local path marker ,and some fix in order to be easy to see
+* changed topic name
+* Change subscribing topic from 'safety_waypoint' to 'temporal_waypoints'
+* first commit major update for waypoint_saver
+* modified velocity_set
+* Fix subscribing topic
+* Add waypoint_clicker
+* Fixed typo
+* Add the state lattice motion planning features
+* Initial commit for public release
+* Contributors: Hiroki Ohta, Manato Hirabayashi, Shinpei Kato, Syohei YOSHIDA, TomohitoAndo, USUDA Hisashi, h_ohta, pdsljp, syouji

src/ros/catkin/src/waypoint_maker/CMakeLists.txt

@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(waypoint_maker)
+
+find_package(autoware_build_flags REQUIRED)
+
+find_package(
+  catkin REQUIRED COMPONENTS
+    amathutils_lib
+    autoware_msgs
+    autoware_config_msgs
+    geometry_msgs
+    gnss
+    lane_planner
+    libwaypoint_follower
+    nav_msgs
+    roscpp
+    std_msgs
+    tablet_socket_msgs
+    tf
+    vector_map
+)
+
+find_package(Boost REQUIRED)
+
+catkin_package()
+
+SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(
+  waypoint_loader
+  nodes/waypoint_loader/waypoint_loader_core.cpp
+  nodes/waypoint_loader/waypoint_loader_node.cpp
+)
+target_link_libraries(waypoint_loader ${catkin_LIBRARIES})
+add_dependencies(waypoint_loader ${catkin_EXPORTED_TARGETS})
+
+add_executable(
+  waypoint_replanner
+  nodes/waypoint_replanner/waypoint_replanner.cpp
+  nodes/waypoint_replanner/waypoint_replanner_node.cpp
+)
+
+target_link_libraries(waypoint_replanner ${catkin_LIBRARIES})
+add_dependencies(waypoint_replanner ${catkin_EXPORTED_TARGETS})
+
+add_executable(waypoint_saver nodes/waypoint_saver/waypoint_saver.cpp)
+target_link_libraries(waypoint_saver ${catkin_LIBRARIES})
+add_dependencies(waypoint_saver ${catkin_EXPORTED_TARGETS})
+
+add_executable(waypoint_extractor nodes/waypoint_extractor/waypoint_extractor.cpp)
+target_link_libraries(waypoint_extractor ${catkin_LIBRARIES})
+add_dependencies(waypoint_extractor ${catkin_EXPORTED_TARGETS})
+
+add_executable(waypoint_clicker nodes/waypoint_clicker/waypoint_clicker.cpp)
+target_link_libraries(waypoint_clicker ${catkin_LIBRARIES})
+
+add_executable(
+  waypoint_marker_publisher
+  nodes/waypoint_marker_publisher/waypoint_marker_publisher.cpp
+)
+
+target_link_libraries(waypoint_marker_publisher ${catkin_LIBRARIES})
+add_dependencies(waypoint_marker_publisher ${catkin_EXPORTED_TARGETS})
+
+add_executable(
+  waypoint_velocity_visualizer
+  nodes/waypoint_velocity_visualizer/waypoint_velocity_visualizer.cpp
+)
+
+target_link_libraries(waypoint_velocity_visualizer ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+add_dependencies(waypoint_velocity_visualizer ${catkin_EXPORTED_TARGETS})
+
+add_executable(
+  waypoint_creator
+  nodes/waypoint_creator/waypoint_creator_node.cpp
+  nodes/waypoint_creator/waypoint_creator_core.cpp
+  nodes/waypoint_creator/interpolate.cpp
+)
+
+target_link_libraries(waypoint_creator ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+add_dependencies(waypoint_creator ${catkin_EXPORTED_TARGETS})
+
+install(
+  TARGETS
+    waypoint_loader
+    waypoint_replanner
+    waypoint_saver
+    waypoint_clicker
+    waypoint_marker_publisher
+    waypoint_velocity_visualizer
+    waypoint_extractor
+    waypoint_creator
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(
+  DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)

src/ros/catkin/src/waypoint_maker/README.md

@@ -0,0 +1,188 @@
+# waypoint_maker
+
+## Overview
+
+- This package has following nodes.
+
+    - waypoint_clicker
+    - waypoint_saver
+    - waypoint_extractor
+    - waypoint_marker_publisher
+    - waypoint_loader
+    - waypoint_replanner
+    - waypoint_velocity_visualizer
+
+- 3-formats of waypoints.csv handled by waypoint_maker
+
+    - ver1： consist of x, y, z, velocity（no velocity in the first line）
+
+    ex)
+
+    > 3699.6206,-99426.6719,85.8506 <br>
+    > 3700.6453,-99426.6562,85.8224,3.1646 <br>
+    > 3701.7373,-99426.6250,85.8017,4.4036 <br>
+    > 3702.7729,-99426.6094,85.7969,4.7972 <br>
+    > 3703.9048,-99426.6094,85.7766,4.7954 <br>
+    > 3704.9192,-99426.5938,85.7504,4.5168 <br>
+    > 3705.9497,-99426.6094,85.7181,3.6313 <br>
+    > 3706.9897,-99426.5859,85.6877,4.0757 <br>
+    > 3708.0266,-99426.4453,85.6608,4.9097 <br>
+    > ... <br>
+
+    - ver2： consist of x, y, z, yaw, velocity（no velocity in the first line）
+
+    ex)
+
+    > 3804.5657,-99443.0156,85.6206,3.1251 <br>
+    > 3803.5195,-99443.0078,85.6004,3.1258,4.8800 <br>
+    > 3802.3425,-99442.9766,85.5950,3.1279,7.2200 <br>
+    > 3801.2092,-99442.9844,85.5920,3.1293,8.8600 <br>
+    > 3800.1633,-99442.9688,85.5619,3.1308,10.6000 <br>
+    > 3798.9702,-99442.9609,85.5814,3.1326,10.5200 <br>
+    > 3796.5706,-99442.9375,85.6056,3.1359,10.2200 <br>
+    > 3795.3232,-99442.9453,85.6082,3.1357,11.0900 <br>
+    > 3794.0771,-99442.9375,85.6148,3.1367,11.2300 <br>
+    > ... <br>
+
+    - ver3： category names are on the first line
+
+    ex） consist of x,y,z,yaw,velocity,change_flag
+
+    > x,y,z,yaw,velocity,change_flag <br>
+    > 3742.216,-99411.311,85.728,3.141593,0,0 <br>
+    > 3741.725,-99411.311,85.728,3.141593,10,0 <br>
+    > 3740.725,-99411.311,85.733,3.141593,10,0 <br>
+    > 3739.725,-99411.311,85.723,3.141593,10,0 <br>
+    > 3738.725,-99411.311,85.719,3.141593,10,0 <br>
+    > 3737.725,-99411.311,85.695,3.141593,10,0 <br>
+    > 3736.725,-99411.311,85.667,3.141593,10,0 <br>
+    > 3735.725,-99411.311,85.654,3.141593,10,0 <br>
+    > ... <br>
+
+## Nodes
+
+### waypoint_loader
+
+1. Overview
+
+  * waypoint_loader
+    - Convert waypoints.csv to ROS message type.
+    - Correspond to the above 3 types of csv.
+  * waypoint_replanner
+    - Adjust waypoints offline (resample and replan velocity)
+
+1. How to use
+
+  * How to start
+    - At `Computing`->`waypoint_loader`:
+      - Check app->`load csv`.
+        If you want to load csv_files, switch to true.
+        Otherwise switch to false.
+      - Check `waypoint_loader` and start.
+  * Idea of velocity replanning
+    - The velocity plan is based on the following idea.
+      - On a straight line, it accelerates to the maximum speed.
+      - At one curve:
+        * Finish decelerating before entering the curve.
+          If the curve is sharper, the more deceleration is required.
+        * Maintain a constant speed in the curve.
+        * Start to accelerate after getting out of the curve.
+  * Detail of app tab
+    - On `multi_lane`, please select multiple input files. If you want lane change with `lane_select`, prepare ver3 type.
+    - Check `replanning_mode` if you want to replan velocity.
+      - On replanning mode:
+        * Check `realtime_tuning_mode` if you want to tune waypoint.
+        * Check `resample_mode` if you want to resample waypoints.
+          On resample mode, please set `resample_interval`.
+        * Velocity replanning parameter
+          - Check `replan curve mode` if you want to decelerate on curve.
+          - Check `overwrite vmax mode` if you want to overwrite velocity of all waypoint.
+          - Check `replan endpoint mode` if you want to decelerate on endpoint.
+          - `Vmax` is max velocity.
+          - `Rth`  is radius threshold for extracting curve in waypoints.
+            Increasing this, you can extract curves more sensitively.
+          - `Rmin` and `Vmin` are pairs of values used for velocity replanning.
+            Designed velocity plan that minimizes velocity in the assumed sharpest curve.
+            In the _i_-th curve, the minimum radius _r<sub>i</sub>_ and the velocity _v<sub>i</sub>_ are expressed by the following expressions.
+            _v<sub>i</sub>_ = _Vmax_ - _(Vmax - Vmin)/(Rth - Rmin)_ * _(Rth - r<sub>i</sub>)_
+          - `Accel limit` is acceleration value for limitting velocity.
+          - `Decel limit` is deceleration value for limitting velocity.
+          - `Velocity Offset` is offset amount preceding the velocity plan.
+          - `Braking Distance` is the number of minimum velocity before end point offset.
+          - `End Point Offset` is the number of 0 velocity points at the end of waypoints.
+
+1. Subscribed Topics
+
+  * waypoint_replanner
+    - /based/lane_waypoints_raw (autoware_msgs/LaneArray)
+    - /config/waypoint_replanner (autoware_config_msgs/ConfigWaypointReplanner)
+
+1. Published Topics
+
+  * waypoint_loader
+    - /based/lane_waypoints_raw (autoware_msgs/LaneArray)
+
+  * waypoint_replanner
+    - /based/lane_waypoints_array (autoware_msgs/LaneArray)
+    - /lane_waypoints_array (autoware_msgs/LaneArray)
+
+1. Parameters
+
+  * waypoint_loader
+    - ~multi_lane_csv
+
+
+### waypoint_saver
+
+1. Overview
+
+  * waypoint_saver
+    - When activated, subscribe `/current_pose`, `/current_velocity`(option) and save waypoint in the file at specified intervals.
+  * waypoint_extractor
+    - When activated, subscribe autoware_msgs/LaneArray and save waypoint in the file. Input topic name is selectable.
+  * common
+    - `change_flag` is basically stored as 0 (straight ahead),
+      so if you want to change the lane, edit by yourself. (1 turn right, 2 turn left)
+    - This node corresponds to preservation of ver3 format.
+
+1. How to use
+
+    On app:
+  * common
+    - Ref on the `Save File` and specify the save file name.
+    - Select the function you want to use from the `Input Type`.
+  * if `Input Type` == VehicleFootprint (`waypoint_saver`)   
+    - Check `Save/current_velocity` if you want to save velocity.
+      In otherwise, saved as 0 velocity.
+    - Using `Interval`, it is set how many meters to store waypoint.
+  * if `Input Type` == LaneArrayTopic (`waypoint_extractor`)
+    - Set lane_array topic name you want to save.
+    - Cache starts at the same time as the node starts up.
+    - The cache is always overwritten while the node is running.
+    - The cache is flushed when the node is closed.
+
+1. Subscribed Topics
+
+  * waypoint_saver
+    - /current_pose (geometry_msgs/PoseStamped) : default
+    - /current_velocity (geometry_msgs/TwistStamped) : default
+
+  * waypoint_extractor
+    - /lane_waypoints_array (autoware_msgs/LaneArray) : default
+
+1. Published Topics
+
+    - nothing
+
+1. Parameters
+
+  * waypoints_saver
+    - ~save_filename
+    - ~interval
+    - ~velocity_topic
+    - ~pose_topic
+    - ~save_velocity
+
+  * waypoints_extractor
+    - ~lane_csv
+    - ~lane_topic

src/ros/catkin/src/waypoint_maker/include/waypoint_creator/interpolate.h

@@ -0,0 +1,50 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef WAYPOINT_CREATOR_INTERPOLATE_H_
+#define WAYPOINT_CREATOR_INTERPOLATE_H_
+
+#include <iostream>
+#include <cmath>
+#include <vector>
+
+class LinearInterpolate
+{
+public:
+  LinearInterpolate(){};
+  static bool interpolate(const std::vector<double>& base_index, const std::vector<double>& base_value,
+                          const std::vector<double>& return_index, std::vector<double>& return_value);
+};
+
+class SplineInterpolate
+{
+  bool initialized_;
+  std::vector<double> a_;  //!< @brief temporal vector for calculation
+  std::vector<double> b_;  //!< @brief temporal vector for calculation
+  std::vector<double> c_;  //!< @brief temporal vector for calculation
+  std::vector<double> d_;  //!< @brief temporal vector for calculation
+
+public:
+  SplineInterpolate();
+  SplineInterpolate(const std::vector<double>& x);
+  void generateSpline(const std::vector<double>& x);
+  double getValue(const double& s);
+  bool interpolate(const std::vector<double>& base_index, const std::vector<double>& base_value,
+                   const std::vector<double>& return_index, std::vector<double>& return_value);
+  void getValueVector(const std::vector<double>& s_v, std::vector<double>& value_v);
+};
+
+#endif

src/ros/catkin/src/waypoint_maker/include/waypoint_creator/waypoint_creator_core.hpp

@@ -0,0 +1,131 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @file waypoint_creator_core.hpp
+ * @brief create smooth waypoints with interpolation from source poses
+ * @author Takamasa Horibe
+ * @date 2019.08.28
+ */
+
+#ifndef WAYPOINT_MARKER_WAYPOINT_CREATOR_CORE_H_
+#define WAYPOINT_MARKER_WAYPOINT_CREATOR_CORE_H_
+
+#include <ros/ros.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PointStamped.h>
+
+#include <tf2_ros/transform_listener.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <autoware_msgs/LaneArray.h>
+
+class WaypointCreator
+{
+public:
+  /**
+   * @brief constructor
+   */
+  WaypointCreator();
+
+private:
+  /* ros system */
+  ros::NodeHandle nh_;                       //!< @brief ros node handle
+  ros::NodeHandle pnh_;                      //!< @brief private ros node handle
+  tf2_ros::Buffer tf2_buffer_;               //!< @brief tf2 buffer
+  tf2_ros::TransformListener tf2_listener_;  //!< @brief tf2 listener
+  ros::Subscriber sub_source_point_;         //!< @brief subscriber for source poses
+  ros::Subscriber sub_delete_point_;         //!< @brief subscriber for delete poses
+  ros::Publisher pub_waypoints_;             //!< @brief publihser for generated waypoints
+  ros::Publisher pub_visualize_;             //!< @brief publisher for waypoints visualization
+
+  /* pose vector */
+  std::vector<geometry_msgs::Pose> source_pose_v_;        //!< @brief source poses vector
+  std::vector<geometry_msgs::Pose> interpolated_pose_v_;  //!< @brief interpolated poses vector
+
+  /* parameters */
+  double waypoint_velocity_;       //!< @brief waypoint velocity
+  double remove_pose_thr_;         //!< @brief when received delete pose, remove a closest source point which is in this
+                                   //!< threshold range
+  double interpolation_interval_;  //!< @brief length between waypoints used for interpolation
+  std::string interpolation_method_;  //!< @brief interpolation method (option: linear or spline)
+  std::string waypoint_frame_id_;     //!< @brief output waypoints frame_id
+
+  /**
+   * @brief callback function for source poses
+   * @param [in] in_point received point stocked in source_pose_v_
+   */
+  void inputPointCallback(const geometry_msgs::PointStamped& in_point);
+
+  /**
+   * @brief callback function for delete poses
+   * @param [in] in_pose received pose to remove source pose in source_pose_v_
+   */
+  void deletePoseCallback(const geometry_msgs::PoseStamped& in_pose);
+
+  /**
+   * @brief interpolate poses with constant interval length
+   * @param [in] in_poses source pose vector for interpolation
+   * @param [in] interpolation_interval interpolated output pose vector distance
+   * @param [in] method interpolation method (option: "linear" or "spline")
+   * @param [out] out_poses interpolated pose vector
+   */
+  bool interpolatePosesWithConstantDistance(const std::vector<geometry_msgs::Pose>& in_poses,
+                                            const double& interpolation_interval, const std::string& method,
+                                            std::vector<geometry_msgs::Pose>& out_poses) const;
+
+  /**
+   * @brief interpolate poses
+   * @param [in] in_poses source pose vector for interpolation
+   * @param [in] interpolation_interval sampling span for interpolation
+   * @param [in] method interpolation method (option: "linear" or "spline")
+   * @param [out] out_poses interpolated pose vector
+   */
+  bool interpolatePoses(const std::vector<geometry_msgs::Pose>& in_poses, const double& interpolation_interval,
+                        const std::string& method, std::vector<geometry_msgs::Pose>& out_poses) const;
+
+  /**
+   * @brief publish generated waypoints
+   */
+  void publishWaypoints() const;
+
+  /**
+   * @brief publish visualization marker for waypoints
+   */
+  void visualizeWaypoints() const;
+
+  /**
+   * @brief create pose marker for visualization
+   * @param [in] pose source pose vector for interpolation
+   * @param [in] color output marker
+   * @param [in] scale output marker scake
+   * @param [in] ns output marker namespace
+   * @param [in] id output marker id
+   * @return created marker
+   */
+  visualization_msgs::Marker createPoseMarker(const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& color,
+                                              const geometry_msgs::Vector3& scale, const std::string& ns,
+                                              const int32_t id) const;
+
+  /**
+   * @brief publish visualize waypoints with delete action
+   */
+  void deleteVisualizeWaypoints();
+};
+
+#endif

src/ros/catkin/src/waypoint_maker/interface.yaml

@@ -0,0 +1,22 @@
+- name: /waypoint_clicker
+  publish: [/waypoint_guide]
+  subscribe: [/clicked_point, /vector_map_info/point, /vector_map_info/lane,
+    /vector_map_info/node]
+- name: /waypoint_loader
+  publish: [/based/lane_waypoint_raw]
+  subscribe: []
+- name: /waypoint_replanner
+  publish: [/based/lane_waypoint_array, /lane_waypoints_array]
+  subscribe: [/based/lane_waypoint_raw]
+- name: /waypoints_marker_publisher
+  publish: [/local_waypoints_mark, /global_waypoints_mark]
+  subscribe: [/light_color, /light_color_managed, /lane_waypoints_array,
+    /traffic_waypoints_array, /final_waypoints, /closest_waypoint,
+    /config/lane_stop]
+- name: /waypoint_saver
+  publish: [/waypoint_saver_marker]
+  subscribe: [/current_pose, /current_velocity]
+- name: /waypoint_velocity_visualizer
+  publish: [/waypoints_velocity]
+  subscribe: [/base_waypoints, /final_waypoints, /current_pose, /current_velocity,
+    /twist_cmd]

src/ros/catkin/src/waypoint_maker/launch/waypoint_creator.launch

@@ -0,0 +1,29 @@
+<launch>
+  <arg name="lane_topic" default="/waypoint_creator/lane_array" />
+  <arg name="remove_pose_thr" default="2.0" />
+  <arg name="interpolation_interval" default="1.0" />
+  <arg name="interpolation_method" default="spline"/>
+  <arg name="waypoint_velocity" default="5.0" />
+  <arg name="waypoint_frame_id" default="map" />
+  <arg name="source_point_topic" default="/clicked_point" />
+  <arg name="delete_pose_topic" default="/move_base_simple/goal" />
+
+  <!-- waypoint creator to generate waypoints -->
+  <node pkg="waypoint_maker" type="waypoint_creator" name="waypoint_creator" output="screen">
+    <param name="out_lane_array_topic" value="$(arg lane_topic)" />
+    <param name="remove_pose_thr" value="$(arg remove_pose_thr)" />
+    <param name="interpolation_interval" value="$(arg interpolation_interval)" />
+    <param name="interpolation_method" value="$(arg interpolation_method)" />
+    <param name="waypoint_velocity" value="$(arg waypoint_velocity)" />
+    <param name="waypoint_frame_id" value="$(arg waypoint_frame_id)" />
+    <param name="source_point_topic" value="$(arg source_point_topic)" />
+    <param name="delete_pose_topic" value="$(arg delete_pose_topic)" />
+  </node>
+
+  <!-- waypoint extractor to save waypoints -->
+  <node pkg="waypoint_maker" type="waypoint_extractor" name="waypoint_extractor" output="screen">
+    <param name="lane_csv" command="bash -c &quot;echo -n $HOME`date '+/waypoint_creator_%Y%m%d-%H%M%S.csv'`&quot;" />
+    <param name="lane_topic" value="$(arg lane_topic)" />
+  </node>
+
+</launch>

src/ros/catkin/src/waypoint_maker/launch/waypoint_loader.launch

@@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="load_csv" default="false" />
+  <arg name="multi_lane_csv" default="/tmp/driving_lane.csv" />
+  <arg name="replanning_mode" default="False" />
+  <arg name="realtime_tuning_mode" default="False" />
+  <arg name="resample_mode" default="True" />
+  <arg name="resample_interval" default="1.0" />
+  <arg name="replan_curve_mode" default="False" />
+  <arg name="overwrite_vmax_mode" default="False" />
+  <arg name="replan_endpoint_mode" default="True" />
+  <arg name="velocity_max" default="20" />
+  <arg name="radius_thresh" default="20" />
+  <arg name="radius_min" default="6" />
+  <arg name="velocity_min" default="4" />
+  <arg name="accel_limit" default="0.5" />
+  <arg name="decel_limit" default="0.3" />
+  <arg name="velocity_offset" default="4" />
+  <arg name="braking_distance" default="5" />
+  <arg name="end_point_offset" default="1" />
+  <arg name="use_decision_maker" default="false" />
+
+  <!-- rosrun waypoint_maker waypoint_loader _multi_lane_csv:="path file" -->
+  <node pkg="waypoint_maker" type="waypoint_loader" name="waypoint_loader" output="screen" if="$(arg load_csv)">
+    <param name="multi_lane_csv" value="$(arg multi_lane_csv)" />
+  </node>
+  <node pkg="waypoint_maker" type="waypoint_replanner" name="waypoint_replanner" output="screen">
+    <param name="replanning_mode" value="$(arg replanning_mode)" />
+    <param name="realtime_tuning_mode" value="$(arg realtime_tuning_mode)" />
+    <param name="resample_mode" value="$(arg resample_mode)" />
+    <param name="resample_interval" value="$(arg resample_interval)" />
+    <param name="replan_curve_mode" value="$(arg replan_curve_mode)" />
+    <param name="overwrite_vmax_mode" value="$(arg overwrite_vmax_mode)" />
+    <param name="replan_endpoint_mode" value="$(arg replan_endpoint_mode)" />
+    <param name="velocity_max" value="$(arg velocity_max)" />
+    <param name="radius_thresh" value="$(arg radius_thresh)" />
+    <param name="radius_min" value="$(arg radius_min)" />
+    <param name="velocity_min" value="$(arg velocity_min)" />
+    <param name="accel_limit" value="$(arg accel_limit)" />
+    <param name="decel_limit" value="$(arg decel_limit)" />
+    <param name="velocity_offset" value="$(arg velocity_offset)" />
+    <param name="braking_distance" value="$(arg braking_distance)" />
+    <param name="end_point_offset" value="$(arg end_point_offset)" />
+    <param name="use_decision_maker" value="$(arg use_decision_maker)" />
+  </node>
+  <node pkg="waypoint_maker" type="waypoint_marker_publisher" name="waypoint_marker_publisher" />
+</launch>

src/ros/catkin/src/waypoint_maker/launch/waypoint_saver.launch

@@ -0,0 +1,24 @@
+<!-- -->
+<launch>
+
+  <arg name="input_type" default="LaneArrayTopic" />
+  <arg name="save_finename" default="/tmp/saved_waypoints.csv" />
+  <arg name="interval" default="1" />
+  <arg name="pose_topic" default="current_pose" />
+  <arg name="velocity_topic" default="current_velocity" />
+  <arg name="save_velocity" default="true" />
+  <arg name="lane_topic" default="/lane_waypoints_array" />
+
+  <node pkg="waypoint_maker" type="waypoint_saver" name="waypoint_saver" output="screen" unless="$(arg input_type)">
+    <param name="save_filename" value="$(arg save_finename)" />
+    <param name="interval" value="$(arg interval)" />
+    <param name="velocity_topic" value="$(arg velocity_topic)" />
+    <param name="pose_topic" value="$(arg pose_topic)" />
+    <param name="save_velocity" value="$(arg save_velocity)" />
+  </node>
+  <node pkg="waypoint_maker" type="waypoint_extractor" name="waypoint_extractor" output="screen" if="$(arg input_type)">
+    <param name="lane_csv" value="$(arg save_finename)" />
+    <param name="lane_topic" value="$(arg lane_topic)" />
+  </node>
+
+</launch>

src/ros/catkin/src/waypoint_maker/launch/waypoint_velocity_visualizer.launch

@@ -0,0 +1,41 @@
+<!-- -->
+<launch>
+  <arg name="base_waypoints" default="base_waypoints" />
+  <arg name="final_waypoints" default="final_waypoints" />
+  <arg name="current_pose" default="current_pose" />
+  <arg name="current_velocity" default="current_velocity" />
+  <arg name="twist_cmd" default="twist_cmd" />
+
+  <arg name="use_bar_plot" default="false" />
+  <arg name="use_line_plot" default="true" />
+  <arg name="use_text_plot" default="true" />
+
+  <arg name="control_buffer_size" default="100" />
+  <arg name="plot_height_ratio" default="1.0" />
+  <arg name="plot_height_shift" default="0.2" />
+  <arg name="plot_metric_interval" default="1.0" />
+
+  <arg name="base_waypoints_rgba" default="[1.0, 1.0, 1.0, 0.5]" />
+  <arg name="final_waypoints_rgba" default="[0.0, 1.0, 0.0, 0.5]" />
+  <arg name="current_twist_rgba" default="[0.0, 0.0, 1.0, 0.5]" />
+  <arg name="command_twist_rgba" default="[1.0, 0.0, 0.0, 0.5]" />
+
+  <node pkg="waypoint_maker" type="waypoint_velocity_visualizer" name="waypoint_velocity_visualizer" output="screen">
+    <remap from="base_waypoints" to="$(arg base_waypoints)" />
+    <remap from="final_waypoints" to="$(arg final_waypoints)" />
+    <remap from="current_pose" to="$(arg current_pose)" />
+    <remap from="current_velocity" to="$(arg current_velocity)" />
+    <remap from="twist_cmd" to="$(arg twist_cmd)" />
+    <param name="use_bar_plot" value="$(arg use_bar_plot)" />
+    <param name="use_line_plot" value="$(arg use_line_plot)" />
+    <param name="use_text_plot" value="$(arg use_text_plot)" />
+    <param name="control_buffer_size" value="$(arg control_buffer_size)" />
+    <param name="plot_height_ratio" value="$(arg plot_height_ratio)" />
+    <param name="plot_height_shift" value="$(arg plot_height_shift)" />
+    <param name="plot_metric_interval" value="$(arg plot_metric_interval)" />
+    <rosparam param="base_waypoints_rgba" subst_value="True">$(arg base_waypoints_rgba)</rosparam>
+    <rosparam param="final_waypoints_rgba" subst_value="True">$(arg final_waypoints_rgba)</rosparam>
+    <rosparam param="current_twist_rgba" subst_value="True">$(arg current_twist_rgba)</rosparam>
+    <rosparam param="command_twist_rgba" subst_value="True">$(arg command_twist_rgba)</rosparam>
+  </node>
+</launch>

src/ros/catkin/src/waypoint_maker/nodes/waypoint_clicker/waypoint_clicker.cpp

@@ -0,0 +1,223 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <geometry_msgs/PointStamped.h>
+#include <ros/console.h>
+#include <tf/transform_listener.h>
+
+#include <vector_map/vector_map.h>
+
+#include <lane_planner/lane_planner_vmap.hpp>
+
+namespace {
+
+int waypoint_max;
+double search_radius; // meter
+double velocity; // km/h
+std::string frame_id;
+std::string output_file;
+
+visualization_msgs::Marker waypoint_marker;
+visualization_msgs::Marker branching_marker;
+visualization_msgs::Marker merging_marker;
+visualization_msgs::Marker selection_marker;
+visualization_msgs::Marker route_marker;
+ros::Publisher marker_pub;
+tf::StampedTransform transform;
+
+lane_planner::vmap::VectorMap all_vmap;
+lane_planner::vmap::VectorMap lane_vmap;
+lane_planner::vmap::VectorMap coarse_vmap;
+
+void create_route(const geometry_msgs::PointStamped& msg)
+{
+  if (all_vmap.points.empty() || all_vmap.lanes.empty() || all_vmap.nodes.empty())
+    return;
+
+  geometry_msgs::Point point;
+  point.x = msg.point.x + transform.getOrigin().x();
+  point.y = msg.point.y + transform.getOrigin().y();
+  point.z = msg.point.z + transform.getOrigin().z();
+  coarse_vmap.points.push_back(lane_planner::vmap::create_vector_map_point(point));
+  lane_planner::vmap::publish_add_marker(marker_pub, selection_marker, coarse_vmap.points);
+
+  if (coarse_vmap.points.size() < 2)
+    return;
+
+  lane_planner::vmap::VectorMap fine_vmap =
+    lane_planner::vmap::create_fine_vmap(lane_vmap, lane_planner::vmap::LNO_ALL, coarse_vmap,
+                 search_radius, waypoint_max);
+  if (fine_vmap.points.size() < 2)
+    return;
+
+  lane_planner::vmap::publish_add_marker(marker_pub, route_marker, fine_vmap.points);
+
+  lane_planner::vmap::write_waypoints(fine_vmap.points, velocity, output_file);
+}
+
+void update_values()
+{
+  if (all_vmap.points.empty() || all_vmap.lanes.empty() || all_vmap.nodes.empty())
+    return;
+
+  lane_vmap = lane_planner::vmap::create_lane_vmap(all_vmap, lane_planner::vmap::LNO_ALL);
+  coarse_vmap.points.clear();
+  coarse_vmap.points.shrink_to_fit();
+
+  lane_planner::vmap::publish_delete_marker(marker_pub, waypoint_marker);
+  lane_planner::vmap::publish_delete_marker(marker_pub, branching_marker);
+  lane_planner::vmap::publish_delete_marker(marker_pub, merging_marker);
+  lane_planner::vmap::publish_delete_marker(marker_pub, selection_marker);
+  lane_planner::vmap::publish_delete_marker(marker_pub, route_marker);
+
+  lane_planner::vmap::publish_add_marker(marker_pub, waypoint_marker, lane_vmap.points);
+  lane_planner::vmap::publish_add_marker(marker_pub, branching_marker,
+                 lane_planner::vmap::create_branching_points(lane_vmap));
+  lane_planner::vmap::publish_add_marker(marker_pub, merging_marker,
+                 lane_planner::vmap::create_merging_points(lane_vmap));
+}
+
+void cache_point(const vector_map::PointArray& msg)
+{
+  all_vmap.points = msg.data;
+  update_values();
+}
+
+void cache_lane(const vector_map::LaneArray& msg)
+{
+  all_vmap.lanes = msg.data;
+  update_values();
+}
+
+void cache_node(const vector_map::NodeArray& msg)
+{
+  all_vmap.nodes = msg.data;
+  update_values();
+}
+
+} // namespace
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "waypoint_clicker");
+
+  ros::NodeHandle n;
+
+  int sub_vmap_queue_size;
+  n.param<int>("/waypoint_clicker/sub_vmap_queue_size", sub_vmap_queue_size, 1);
+  int sub_pose_queue_size;
+  n.param<int>("/waypoint_clicker/sub_pose_queue_size", sub_pose_queue_size, 1);
+  int pub_marker_queue_size;
+  n.param<int>("/waypoint_clicker/pub_marker_queue_size", pub_marker_queue_size, 10);
+  bool pub_marker_latch;
+  n.param<bool>("/waypoint_clicker/pub_marker_latch", pub_marker_latch, true);
+
+  n.param<int>("/waypoint_clicker/waypoint_max", waypoint_max, 10000);
+  n.param<double>("/waypoint_clicker/search_radius", search_radius, 10);
+  n.param<double>("/waypoint_clicker/velocity", velocity, 40);
+  n.param<std::string>("/waypoint_clicker/frame_id", frame_id, "map");
+  n.param<std::string>("/waypoint_clicker/output_file", output_file, "/tmp/lane_waypoint.csv");
+
+  if (output_file.empty()) {
+    ROS_ERROR_STREAM("output filename is empty");
+    return EXIT_FAILURE;
+  }
+  if (output_file.back() == '/') {
+    ROS_ERROR_STREAM(output_file << " is directory");
+    return EXIT_FAILURE;
+  }
+
+  waypoint_marker.header.frame_id = frame_id;
+  waypoint_marker.ns = "waypoint";
+  waypoint_marker.id = 0;
+  waypoint_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+  waypoint_marker.scale.x = 0.2;
+  waypoint_marker.scale.y = 0.2;
+  waypoint_marker.color.r = 1;
+  waypoint_marker.color.g = 1;
+  waypoint_marker.color.b = 0;
+  waypoint_marker.color.a = 1;
+  waypoint_marker.frame_locked = true;
+
+  branching_marker.header.frame_id = frame_id;
+  branching_marker.ns = "branching";
+  branching_marker.id = 0;
+  branching_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+  branching_marker.scale.x = 0.3;
+  branching_marker.scale.y = 0.3;
+  branching_marker.color.r = 0;
+  branching_marker.color.g = 1;
+  branching_marker.color.b = 0;
+  branching_marker.color.a = 1;
+  branching_marker.frame_locked = true;
+
+  merging_marker.header.frame_id = frame_id;
+  merging_marker.ns = "merging";
+  merging_marker.id = 0;
+  merging_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+  merging_marker.scale.x = 0.3;
+  merging_marker.scale.y = 0.3;
+  merging_marker.color.r = 1;
+  merging_marker.color.g = 0;
+  merging_marker.color.b = 0;
+  merging_marker.color.a = 1;
+  merging_marker.frame_locked = true;
+
+  selection_marker.header.frame_id = frame_id;
+  selection_marker.ns = "selection";
+  selection_marker.id = 0;
+  selection_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+  selection_marker.scale.x = 0.4;
+  selection_marker.scale.y = 0.4;
+  selection_marker.color.r = 1;
+  selection_marker.color.g = 1;
+  selection_marker.color.b = 0;
+  selection_marker.color.a = 1;
+  selection_marker.frame_locked = true;
+
+  route_marker.header.frame_id = frame_id;
+  route_marker.ns = "route";
+  route_marker.id = 0;
+  route_marker.type = visualization_msgs::Marker::LINE_STRIP;
+  route_marker.scale.x = 0.2;
+  route_marker.scale.y = 0.2;
+  route_marker.color.r = 1;
+  route_marker.color.g = 1;
+  route_marker.color.b = 0;
+  route_marker.color.a = 1;
+  route_marker.frame_locked = true;
+
+  marker_pub = n.advertise<visualization_msgs::Marker>("/waypoint_guide", pub_marker_queue_size,
+                   pub_marker_latch);
+
+  tf::TransformListener listener;
+  try {
+    ros::Time zero = ros::Time(0);
+    listener.waitForTransform("map", "world", zero, ros::Duration(10));
+    listener.lookupTransform("map", "world", zero, transform);
+  } catch (tf::TransformException &ex) {
+    ROS_ERROR_STREAM(ex.what());
+  }
+
+  ros::Subscriber pose_sub = n.subscribe("/clicked_point", sub_pose_queue_size, create_route);
+  ros::Subscriber point_sub = n.subscribe("/vector_map_info/point", sub_vmap_queue_size, cache_point);
+  ros::Subscriber lane_sub = n.subscribe("/vector_map_info/lane", sub_vmap_queue_size, cache_lane);
+  ros::Subscriber node_sub = n.subscribe("/vector_map_info/node", sub_vmap_queue_size, cache_node);
+
+  ros::spin();
+
+  return EXIT_SUCCESS;
+}

src/ros/catkin/src/waypoint_maker/nodes/waypoint_creator/interpolate.cpp

@@ -0,0 +1,239 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "waypoint_creator/interpolate.h"
+
+/*
+ * linear interpolation
+ */
+bool LinearInterpolate::interpolate(const std::vector<double>& base_index, const std::vector<double>& base_value,
+                                    const std::vector<double>& return_index, std::vector<double>& return_value)
+{
+  auto isIncrease = [](const std::vector<double>& x) {
+    for (int i = 0; i < (int)x.size() - 1; ++i)
+    {
+      if (x[i] > x[i + 1])
+        return false;
+    }
+    return true;
+  };
+
+  if (base_index.size() == 0 || base_value.size() == 0 || return_index.size() == 0)
+  {
+    printf("[interpolate] some vector size is zero: base_index.size() = %lu, base_value.size() = %lu, "
+           "return_index.size() = %lu\n",
+           base_index.size(), base_value.size(), return_index.size());
+    return false;
+  }
+
+  // check if inputs are valid
+  if (!isIncrease(base_index) || !isIncrease(return_index) || return_index.front() < base_index.front() ||
+      base_index.back() < return_index.back() || base_index.size() != base_value.size())
+  {
+    std::cerr << "[isIncrease] bad index, return false" << std::endl;
+    bool b1 = !isIncrease(base_index);
+    bool b2 = !isIncrease(return_index);
+    bool b3 = return_index.front() < base_index.front();
+    bool b4 = base_index.back() < return_index.back();
+    bool b5 = base_index.size() != base_value.size();
+    printf("%d, %d, %d, %d, %d\n", b1, b2, b3, b4, b5);
+    printf("%f, %f\n", base_index.front(), base_index.back());
+    printf("%f, %f\n", return_index.front(), return_index.back());
+    printf("%lu, %lu\n", base_index.size(), base_value.size());
+    return false;
+  }
+
+  // calculate linear interpolation
+  int i = 0;
+  for (const auto idx : return_index)
+  {
+    if (base_index[i] == idx)
+    {
+      return_value.push_back(base_value[i]);
+      continue;
+    }
+    while (base_index[i] < idx)
+      ++i;
+    if (i <= 0 || (int)base_index.size() - 1 < i)
+    {
+      std::cerr << "? something wrong. skip this idx." << std::endl;
+      continue;
+    }
+
+    const double dist_base_idx = base_index[i] - base_index[i - 1];
+    const double dist_to_forward = base_index[i] - idx;
+    const double dist_to_backward = idx - base_index[i - 1];
+    if (dist_to_forward < 0.0 || dist_to_backward < 0.0)
+    {
+      std::cerr << "?? something wrong. skip this idx." << std::endl;
+      continue;
+    }
+
+    const double value = (dist_to_backward * base_value[i] + dist_to_forward * base_value[i - 1]) / dist_base_idx;
+    return_value.push_back(value);
+  }
+
+  return true;
+}
+
+/*
+ * spline interpolation
+ */
+SplineInterpolate::SplineInterpolate(){};
+SplineInterpolate::SplineInterpolate(const std::vector<double>& x)
+{
+  generateSpline(x);
+};
+void SplineInterpolate::generateSpline(const std::vector<double>& x)
+{
+  int N = x.size();
+
+  a_.clear();
+  b_.clear();
+  c_.clear();
+  d_.clear();
+
+  a_ = x;
+
+  c_.push_back(0.0);
+  for (int i = 1; i < N - 1; i++)
+    c_.push_back(3.0 * (a_[i - 1] - 2.0 * a_[i] + a_[i + 1]));
+  c_.push_back(0.0);
+
+  std::vector<double> e_v;
+  e_v.push_back(0.0);
+
+  for (int i = 1; i < N - 1; i++)
+  {
+    double tmp = 1.0 / (4.0 - e_v[i - 1]);
+    c_[i] = (c_[i] - c_[i - 1]) * tmp;
+    e_v.push_back(tmp);
+  }
+
+  for (int i = N - 2; i > 0; i--)
+    c_[i] = c_[i] - c_[i + 1] * e_v[i];
+
+  for (int i = 0; i < N - 1; i++)
+  {
+    d_.push_back((c_[i + 1] - c_[i]) / 3.0);
+    b_.push_back(a_[i + 1] - a_[i] - c_[i] - d_[i]);
+  }
+  d_.push_back(0.0);
+  b_.push_back(0.0);
+
+  initialized_ = true;
+};
+
+double SplineInterpolate::getValue(const double& s)
+{
+  if (!initialized_)
+    return 0.0;
+
+  int j = std::max(std::min(int(std::floor(s)), (int)a_.size() - 1), 0);
+  const double ds = s - j;
+  return a_[j] + (b_[j] + (c_[j] + d_[j] * ds) * ds) * ds;
+}
+
+void SplineInterpolate::getValueVector(const std::vector<double>& s_v, std::vector<double>& value_v)
+{
+  if (!initialized_)
+    return;
+  value_v.clear();
+  for (int i = 0; i < (int)s_v.size(); ++i)
+  {
+    value_v.push_back(getValue(s_v[i]));
+  }
+}
+
+bool SplineInterpolate::interpolate(const std::vector<double>& base_index, const std::vector<double>& base_value,
+                                    const std::vector<double>& return_index, std::vector<double>& return_value)
+{
+  auto isIncrease = [](const std::vector<double>& x) {
+    for (int i = 0; i < (int)x.size() - 1; ++i)
+    {
+      if (x[i] > x[i + 1])
+        return false;
+    }
+    return true;
+  };
+
+  if (base_index.size() == 0 || base_value.size() == 0 || return_index.size() == 0)
+  {
+    printf("[interpolate] some vector size is zero: base_index.size() = %lu, base_value.size() = %lu, "
+           "return_index.size() = %lu\n",
+           base_index.size(), base_value.size(), return_index.size());
+    return false;
+  }
+
+  // check if inputs are valid
+  if (!isIncrease(base_index) || !isIncrease(return_index) || return_index.front() < base_index.front() ||
+      base_index.back() < return_index.back() || base_index.size() != base_value.size())
+  {
+    std::cerr << "[isIncrease] bad index, return false" << std::endl;
+    bool b1 = !isIncrease(base_index);
+    bool b2 = !isIncrease(return_index);
+    bool b3 = return_index.front() < base_index.front();
+    bool b4 = base_index.back() < return_index.back();
+    bool b5 = base_index.size() != base_value.size();
+    printf("%d, %d, %d, %d, %d\n", b1, b2, b3, b4, b5);
+    printf("%f, %f\n", base_index.front(), base_index.back());
+    printf("%f, %f\n", return_index.front(), return_index.back());
+    printf("%lu, %lu\n", base_index.size(), base_value.size());
+    return false;
+  }
+
+  std::vector<double> normalized_idx;
+
+  // calculate normalized index
+  int i = 0;
+  for (const auto idx : return_index)
+  {
+    if (base_index[i] == idx)
+    {
+      normalized_idx.push_back(i);
+      continue;
+    }
+    while (base_index[i] < idx)
+      ++i;
+    if (i <= 0 || (int)base_index.size() - 1 < i)
+    {
+      std::cerr << "? something wrong. skip this idx." << std::endl;
+      continue;
+    }
+
+    const double dist_base_idx = base_index[i] - base_index[i - 1];
+    const double dist_to_forward = base_index[i] - idx;
+    const double dist_to_backward = idx - base_index[i - 1];
+    if (dist_to_forward < 0.0 || dist_to_backward < 0.0)
+    {
+      std::cerr << "?? something wrong. skip this idx." << std::endl;
+      continue;
+    }
+
+    const double value = (dist_to_backward * i + dist_to_forward * (i - 1)) / dist_base_idx;
+    normalized_idx.push_back(value);
+  }
+
+  // calculate spline coefficients
+  generateSpline(base_value);
+
+  // interpolate by spline  with normalized index
+  for (int i = 0; i < (int)normalized_idx.size(); ++i)
+  {
+    return_value.push_back(getValue(normalized_idx[i]));
+  }
+  return true;
+}

src/ros/catkin/src/waypoint_maker/nodes/waypoint_creator/waypoint_creator_core.cpp

@@ -0,0 +1,322 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <iostream>
+#include <amathutils_lib/amathutils.hpp>
+
+#include "waypoint_creator/interpolate.h"
+#include "waypoint_creator/waypoint_creator_core.hpp"
+
+WaypointCreator::WaypointCreator() : nh_(""), pnh_("~"), tf2_listener_(tf2_buffer_)
+{
+  std::string out_lane_array_topic;
+  pnh_.param("out_lane_array_topic", out_lane_array_topic, std::string("/lane_waypoints_array"));
+  pnh_.param("remove_pose_thr", remove_pose_thr_, double(2.0));
+  pnh_.param("interpolation_interval", interpolation_interval_, double(1.0));
+  pnh_.param("interpolation_method", interpolation_method_, std::string("spline"));
+  pnh_.param("waypoint_velocity", waypoint_velocity_, double(5.0));
+  pnh_.param("waypoint_frame_id", waypoint_frame_id_, std::string("map"));
+
+  std::string source_point, delete_pose;
+  pnh_.param("source_point_topic", source_point, std::string("/clicked_point"));
+  pnh_.param("delete_pose_topic", delete_pose, std::string("/move_base_simple/goal"));
+  sub_source_point_ = nh_.subscribe(source_point, 1, &WaypointCreator::inputPointCallback, this);
+  sub_delete_point_ = nh_.subscribe(delete_pose, 1, &WaypointCreator::deletePoseCallback, this);
+  pub_waypoints_ = pnh_.advertise<autoware_msgs::LaneArray>(out_lane_array_topic, 1);
+  pub_visualize_ = pnh_.advertise<visualization_msgs::MarkerArray>("debug/waypoints", 1);
+
+  source_pose_v_.clear();
+  interpolated_pose_v_.clear();
+}
+
+void WaypointCreator::inputPointCallback(const geometry_msgs::PointStamped& in_point)
+{
+  geometry_msgs::PointStamped in_point_converted;
+  try
+  {
+    tf2_buffer_.transform(in_point, in_point_converted, waypoint_frame_id_, ros::Duration(1.0));
+  }
+  catch (tf2::TransformException& ex)
+  {
+    ROS_WARN("[WaypointCreator] transform failed %s\n", ex.what());  // Print exception which was caught
+    return;
+  }
+  geometry_msgs::Pose p;
+  p.position = in_point_converted.point;
+  source_pose_v_.push_back(p);
+
+  interpolatePosesWithConstantDistance(source_pose_v_, interpolation_interval_, interpolation_method_, interpolated_pose_v_);
+
+  publishWaypoints();
+  visualizeWaypoints();
+}
+
+void WaypointCreator::deletePoseCallback(const geometry_msgs::PoseStamped& in_pose)
+{
+  geometry_msgs::PoseStamped in_pose_converted;
+  try
+  {
+    tf2_buffer_.transform(in_pose, in_pose_converted, waypoint_frame_id_, ros::Duration(1.0));
+  }
+  catch (tf2::TransformException& ex)
+  {
+    ROS_WARN("[WaypointCreator] transform failed %s\n", ex.what());  // Print exception which was caught
+    return;
+  }
+
+  unsigned int closest = 0;
+  double dist_min = 1.0E5;
+  for (unsigned int i = 0; i < source_pose_v_.size(); ++i)
+  {
+    double dist = amathutils::find_distance(in_pose_converted.pose, source_pose_v_.at(i));
+    if (dist < dist_min)
+    {
+      dist_min = dist;
+      closest = i;
+    }
+  }
+  if (dist_min < remove_pose_thr_)
+  {
+    deleteVisualizeWaypoints();
+    ROS_INFO("[WaypointCreator] remove pose");
+    source_pose_v_.erase(source_pose_v_.begin() + closest);
+  }
+  else
+  {
+    ROS_INFO("[WaypointCreator] delete pose called, but found no points around received pose");
+  }
+
+  interpolatePosesWithConstantDistance(source_pose_v_, interpolation_interval_, interpolation_method_, interpolated_pose_v_);
+
+  publishWaypoints();
+  visualizeWaypoints();
+}
+
+bool WaypointCreator::interpolatePosesWithConstantDistance(const std::vector<geometry_msgs::Pose>& in_poses,
+                                                           const double& interpolation_interval,
+                                                           const std::string& method,
+                                                           std::vector<geometry_msgs::Pose>& out_poses) const
+{
+  if (in_poses.size() < 2)
+  {
+    out_poses = in_poses;
+    return true;
+  }
+
+  if (method == "linear")
+  {
+    return interpolatePoses(in_poses, interpolation_interval, method, out_poses);
+  }
+  else if (method == "spline")
+  {
+    // apply twise for interval length
+    std::vector<geometry_msgs::Pose> tmp;
+    bool first_ret = interpolatePoses(in_poses, interpolation_interval, method, tmp);
+    if (!first_ret)
+    {
+      return false;
+    }
+    else
+    {
+      return interpolatePoses(tmp, interpolation_interval, method, out_poses);
+    }
+  }
+  else
+  {
+    ROS_WARN("[WaypointCreator] invalid interpolation method");
+    return false;
+  }
+}
+
+bool WaypointCreator::interpolatePoses(const std::vector<geometry_msgs::Pose>& in_poses,
+                                       const double& interpolation_interval, const std::string& method,
+                                       std::vector<geometry_msgs::Pose>& out_poses) const
+{
+  if (in_poses.size() == 0)
+  {
+    ROS_WARN("[WaypointCreator] input pose vector is empty in interpolation. return empty poses.");
+    return false;
+  }
+
+  if (method != "linear" && method != "spline")
+  {
+    ROS_WARN("[WaypointCreator] unknown method for interpolation. return empty pose.");
+    return false;
+  }
+
+  out_poses.clear();
+
+  // convert vector<pose> to vector<double>
+  std::vector<double> in_pos_x_v, in_pos_y_v, in_pos_z_v, in_yaw_v;
+  for (auto& pose : in_poses)
+  {
+    in_pos_x_v.push_back(pose.position.x);
+    in_pos_y_v.push_back(pose.position.y);
+    in_pos_z_v.push_back(pose.position.z);
+  }
+
+  // calculate input vector distance
+  std::vector<double> in_dist_v;
+  in_dist_v.push_back(0.0);
+  for (unsigned int i = 0; i < in_poses.size() - 1; ++i)
+  {
+    const double dx = in_poses.at(i + 1).position.x - in_poses.at(i).position.x;
+    const double dy = in_poses.at(i + 1).position.y - in_poses.at(i).position.y;
+    const double dz = in_poses.at(i + 1).position.z - in_poses.at(i).position.z;
+    in_dist_v.push_back(in_dist_v.at(i) + std::hypot(std::hypot(dx, dy), dz));
+  }
+
+  // calculate desired distance vector
+  std::vector<double> out_dist_v;
+  for (double dist_sum = 0.0; dist_sum < in_dist_v.back(); dist_sum += interpolation_interval)
+  {
+    out_dist_v.push_back(dist_sum);
+  }
+  out_dist_v.push_back(in_dist_v.back());
+
+  // apply spline interpolation
+  std::vector<double> out_pos_x_v, out_pos_y_v, out_pos_z_v, out_yaw_v;
+  if (method == "spline")
+  {
+    SplineInterpolate spline_interploate;
+    if (!spline_interploate.interpolate(in_dist_v, in_pos_x_v, out_dist_v, out_pos_x_v) ||
+        !spline_interploate.interpolate(in_dist_v, in_pos_y_v, out_dist_v, out_pos_y_v) ||
+        !spline_interploate.interpolate(in_dist_v, in_pos_z_v, out_dist_v, out_pos_z_v))
+    {
+      ROS_ERROR("[WaypointCreator] spline interpolation failed!!!");
+      return false;
+    }
+  }
+  else if (method == "linear")
+  {
+    LinearInterpolate linear_interploate;
+    if (!linear_interploate.interpolate(in_dist_v, in_pos_x_v, out_dist_v, out_pos_x_v) ||
+        !linear_interploate.interpolate(in_dist_v, in_pos_y_v, out_dist_v, out_pos_y_v) ||
+        !linear_interploate.interpolate(in_dist_v, in_pos_z_v, out_dist_v, out_pos_z_v))
+    {
+      ROS_ERROR("[WaypointCreator] linear interpolation failed!!!");
+      return false;
+    }
+  }
+
+  // yaw angle calculation
+  for (int i = 0; i < (int)out_dist_v.size(); ++i)
+  {
+    int i_next = std::min(i + 1, (int)out_dist_v.size() - 1);
+    int i_prev = std::max(i - 1, 0);
+    const double eps = 1e-7;
+    const double dx = out_pos_x_v.at(i_next) - out_pos_x_v.at(i_prev);
+    const double dy = out_pos_y_v.at(i_next) - out_pos_y_v.at(i_prev);
+    const double den = std::fabs(dx) > eps ? dx : (dx > 0.0 ? eps : -eps);
+    const double yaw_tmp = std::atan2(dy, den);
+    out_yaw_v.push_back(yaw_tmp);
+  }
+
+  // generate pose vector
+  for (unsigned int i = 0; i < out_dist_v.size(); ++i)
+  {
+    geometry_msgs::Pose p;
+    p.position.x = out_pos_x_v.at(i);
+    p.position.y = out_pos_y_v.at(i);
+    p.position.z = out_pos_z_v.at(i);
+    p.orientation = amathutils::getQuaternionFromYaw(out_yaw_v.at(i));
+    out_poses.push_back(p);
+  }
+  return true;
+}
+
+void WaypointCreator::publishWaypoints() const
+{
+  autoware_msgs::LaneArray lane_array;
+  autoware_msgs::Lane lane;
+  autoware_msgs::Waypoint wps;
+
+  lane.header.frame_id = waypoint_frame_id_;
+  lane.header.stamp = ros::Time::now();
+  for (const auto& p : interpolated_pose_v_)
+  {
+    wps.pose.pose = p;
+    wps.twist.twist.linear.x = waypoint_velocity_;
+    lane.waypoints.push_back(wps);
+  }
+  lane_array.lanes.push_back(lane);
+
+  pub_waypoints_.publish(lane_array);
+}
+
+void WaypointCreator::visualizeWaypoints() const
+{
+  geometry_msgs::Vector3 scale;
+  scale.x = 1.0;
+  scale.y = 0.3;
+  scale.z = 0.3;
+  visualization_msgs::MarkerArray marker_array;
+  for (unsigned int i = 0; i < source_pose_v_.size(); ++i)
+  {
+    std_msgs::ColorRGBA color;
+    color.a = 1.0;
+    color.r = 1.0;
+    marker_array.markers.push_back(createPoseMarker(source_pose_v_.at(i), color, scale, "raw", i));
+  }
+  for (unsigned int i = 0; i < interpolated_pose_v_.size(); ++i)
+  {
+    std_msgs::ColorRGBA color;
+    color.a = 1.0;
+    color.g = 1.0;
+    marker_array.markers.push_back(createPoseMarker(interpolated_pose_v_.at(i), color, scale, "interpolated", i));
+  }
+
+  pub_visualize_.publish(marker_array);
+}
+
+visualization_msgs::Marker WaypointCreator::createPoseMarker(const geometry_msgs::Pose& pose,
+                                                             const std_msgs::ColorRGBA& color,
+                                                             const geometry_msgs::Vector3& scale, const std::string& ns,
+                                                             const int32_t id) const
+{
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = waypoint_frame_id_;
+  marker.header.stamp = ros::Time();
+  marker.ns = ns;
+  marker.id = id;
+  marker.type = visualization_msgs::Marker::ARROW;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.pose = pose;
+  marker.scale = scale;
+  marker.color = color;
+  return marker;
+}
+
+void WaypointCreator::deleteVisualizeWaypoints()
+{
+  visualization_msgs::MarkerArray marker_array;
+  visualization_msgs::Marker delete_marker;
+  delete_marker.action = visualization_msgs::Marker::DELETE;
+  for (unsigned int i = 0; i < source_pose_v_.size(); ++i)
+  {
+    delete_marker.ns = "raw";
+    delete_marker.id = i;
+    marker_array.markers.push_back(delete_marker);
+  }
+  for (unsigned int i = 0; i < interpolated_pose_v_.size(); ++i)
+  {
+    delete_marker.ns = "interpolated";
+    delete_marker.id = i;
+    marker_array.markers.push_back(delete_marker);
+  }
+  pub_visualize_.publish(marker_array);
+}
+

src/ros/catkin/src/waypoint_maker/nodes/waypoint_creator/waypoint_creator_node.cpp

@@ -0,0 +1,28 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <ros/ros.h>
+
+#include "waypoint_creator/waypoint_creator_core.hpp"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "waypoint_creator");
+  WaypointCreator obj;
+  ros::spin();
+
+  return 0;
+}

src/ros/catkin/src/waypoint_maker/nodes/waypoint_extractor/waypoint_extractor.cpp

@@ -0,0 +1,139 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+#include <autoware_msgs/LaneArray.h>
+#include <iostream>
+#include <fstream>
+#include <vector>
+
+namespace waypoint_maker
+{
+// Constructor
+class WaypointExtractor
+{
+private:
+  ros::NodeHandle nh_, private_nh_;
+  ros::Subscriber larray_sub_;
+  std::string lane_csv_, lane_topic_;
+  autoware_msgs::LaneArray lane_;
+public:
+  WaypointExtractor() : private_nh_("~")
+  {
+    init();
+  }
+  // Destructor
+  ~WaypointExtractor()
+  {
+    deinit();
+  }
+
+  double mps2kmph(double velocity_mps)
+  {
+    return (velocity_mps * 60 * 60) / 1000;
+  }
+
+  const std::string addFileSuffix(std::string file_path, std::string suffix)
+  {
+    std::string output_file_path, tmp;
+    std::string directory_path, filename, extension;
+
+    tmp = file_path;
+    const std::string::size_type idx_slash(tmp.find_last_of("/"));
+    if (idx_slash != std::string::npos)
+    {
+      tmp.erase(0, idx_slash);
+    }
+    const std::string::size_type idx_dot(tmp.find_last_of("."));
+    const std::string::size_type idx_dot_allpath(file_path.find_last_of("."));
+    if (idx_dot != std::string::npos && idx_dot != tmp.size() - 1)
+    {
+      file_path.erase(idx_dot_allpath, file_path.size() - 1);
+    }
+    file_path += suffix + ".csv";
+    return file_path;
+  }
+
+  void init()
+  {
+    private_nh_.param<std::string>("lane_csv", lane_csv_, "/tmp/driving_lane.csv");
+    private_nh_.param<std::string>("lane_topic", lane_topic_, "/lane_waypoints_array");
+    // setup publisher
+    larray_sub_ = nh_.subscribe(lane_topic_, 1, &WaypointExtractor::LaneArrayCallback, this);
+  }
+
+  void deinit()
+  {
+    if (lane_.lanes.empty())
+    {
+      return;
+    }
+    std::vector<std::string> dst_multi_file_path(lane_.lanes.size(), lane_csv_);
+    if (lane_.lanes.size() > 1)
+    {
+      for (auto& el : dst_multi_file_path)
+      {
+        el = addFileSuffix(el, std::to_string(&el - &dst_multi_file_path[0]));
+      }
+    }
+    saveLaneArray(dst_multi_file_path, lane_);
+  }
+
+  void LaneArrayCallback(const autoware_msgs::LaneArray::ConstPtr& larray)
+  {
+    if (larray->lanes.empty())
+    {
+      return;
+    }
+    lane_ = *larray;
+  }
+
+  void saveLaneArray(const std::vector<std::string>& paths,
+                                         const autoware_msgs::LaneArray& lane_array)
+  {
+    for (const auto& file_path : paths)
+    {
+      const unsigned long idx = &file_path - &paths[0];
+      std::ofstream ofs(file_path.c_str());
+      ofs << "x,y,z,yaw,velocity,change_flag,steering_flag,accel_flag,stop_flag,event_flag" << std::endl;
+      for (const auto& el : lane_array.lanes[idx].waypoints)
+      {
+        const geometry_msgs::Point p = el.pose.pose.position;
+        const double yaw = tf::getYaw(el.pose.pose.orientation);
+        const double vel = mps2kmph(el.twist.twist.linear.x);
+        const int states[] =
+        {
+          el.change_flag, el.wpstate.steering_state, el.wpstate.accel_state,
+          el.wpstate.stop_state, el.wpstate.event_state
+        };
+        ofs << std::fixed << std::setprecision(4);
+        ofs << p.x << "," << p.y << "," << p.z << "," << yaw << "," << vel;
+        for (int i = 0; i < 5; ofs << "," << states[i++]){}
+        ofs << std::endl;
+      }
+    }
+  }
+};
+
+}  // waypoint_maker
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "waypoint_extractor");
+  waypoint_maker::WaypointExtractor we;
+  ros::spin();
+  return 0;
+}

src/ros/catkin/src/waypoint_maker/nodes/waypoint_loader/waypoint_loader_core.cpp

@@ -0,0 +1,319 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "waypoint_loader_core.h"
+
+namespace waypoint_maker
+{
+// Constructor
+WaypointLoaderNode::WaypointLoaderNode() : private_nh_("~")
+{
+  initPubSub();
+}
+
+// Destructor
+WaypointLoaderNode::~WaypointLoaderNode()
+{
+}
+
+void WaypointLoaderNode::initPubSub()
+{
+  private_nh_.param<std::string>("multi_lane_csv", multi_lane_csv_, "/tmp/driving_lane.csv");
+  // setup publisher
+  lane_pub_ = nh_.advertise<autoware_msgs::LaneArray>("/based/lane_waypoints_raw", 10, true);
+}
+
+void WaypointLoaderNode::run()
+{
+  multi_file_path_.clear();
+  parseColumns(multi_lane_csv_, &multi_file_path_);
+  autoware_msgs::LaneArray lane_array;
+  createLaneArray(multi_file_path_, &lane_array);
+  lane_pub_.publish(lane_array);
+  output_lane_array_ = lane_array;
+  ros::spin();
+}
+
+void WaypointLoaderNode::createLaneArray(const std::vector<std::string>& paths, autoware_msgs::LaneArray* lane_array)
+{
+  for (const auto& el : paths)
+  {
+    autoware_msgs::Lane lane;
+    createLaneWaypoint(el, &lane);
+    lane_array->lanes.emplace_back(lane);
+  }
+}
+
+void WaypointLoaderNode::createLaneWaypoint(const std::string& file_path, autoware_msgs::Lane* lane)
+{
+  if (!verifyFileConsistency(file_path.c_str()))
+  {
+    ROS_ERROR("lane data is something wrong...");
+    return;
+  }
+
+  ROS_INFO("lane data is valid. publishing...");
+  FileFormat format = checkFileFormat(file_path.c_str());
+  std::vector<autoware_msgs::Waypoint> wps;
+  if (format == FileFormat::ver1)
+  {
+    loadWaypointsForVer1(file_path.c_str(), &wps);
+  }
+  else if (format == FileFormat::ver2)
+  {
+    loadWaypointsForVer2(file_path.c_str(), &wps);
+  }
+  else
+  {
+    loadWaypointsForVer3(file_path.c_str(), &wps);
+  }
+  lane->header.frame_id = "/map";
+  lane->header.stamp = ros::Time(0);
+  lane->waypoints = wps;
+}
+
+void WaypointLoaderNode::loadWaypointsForVer1(const char* filename, std::vector<autoware_msgs::Waypoint>* wps)
+{
+  std::ifstream ifs(filename);
+
+  if (!ifs)
+  {
+    return;
+  }
+
+  std::string line;
+  std::getline(ifs, line);  // Remove first line
+
+  while (std::getline(ifs, line))
+  {
+    autoware_msgs::Waypoint wp;
+    parseWaypointForVer1(line, &wp);
+    wps->emplace_back(wp);
+  }
+
+  size_t last = wps->size() - 1;
+  for (size_t i = 0; i < wps->size(); ++i)
+  {
+    if (i != last)
+    {
+      double yaw = atan2(wps->at(i + 1).pose.pose.position.y - wps->at(i).pose.pose.position.y,
+                         wps->at(i + 1).pose.pose.position.x - wps->at(i).pose.pose.position.x);
+      wps->at(i).pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
+    }
+    else
+    {
+      wps->at(i).pose.pose.orientation = wps->at(i - 1).pose.pose.orientation;
+    }
+  }
+}
+
+void WaypointLoaderNode::parseWaypointForVer1(const std::string& line, autoware_msgs::Waypoint* wp)
+{
+  std::vector<std::string> columns;
+  parseColumns(line, &columns);
+
+  wp->pose.pose.position.x = std::stod(columns[0]);
+  wp->pose.pose.position.y = std::stod(columns[1]);
+  wp->pose.pose.position.z = std::stod(columns[2]);
+  wp->twist.twist.linear.x = kmph2mps(std::stod(columns[3]));
+}
+
+void WaypointLoaderNode::loadWaypointsForVer2(const char* filename, std::vector<autoware_msgs::Waypoint>* wps)
+{
+  std::ifstream ifs(filename);
+
+  if (!ifs)
+  {
+    return;
+  }
+
+  std::string line;
+  std::getline(ifs, line);  // Remove first line
+
+  while (std::getline(ifs, line))
+  {
+    autoware_msgs::Waypoint wp;
+    parseWaypointForVer2(line, &wp);
+    wps->emplace_back(wp);
+  }
+}
+
+void WaypointLoaderNode::parseWaypointForVer2(const std::string& line, autoware_msgs::Waypoint* wp)
+{
+  std::vector<std::string> columns;
+  parseColumns(line, &columns);
+
+  wp->pose.pose.position.x = std::stod(columns[0]);
+  wp->pose.pose.position.y = std::stod(columns[1]);
+  wp->pose.pose.position.z = std::stod(columns[2]);
+  wp->pose.pose.orientation = tf::createQuaternionMsgFromYaw(std::stod(columns[3]));
+  wp->twist.twist.linear.x = kmph2mps(std::stod(columns[4]));
+}
+
+void WaypointLoaderNode::loadWaypointsForVer3(const char* filename, std::vector<autoware_msgs::Waypoint>* wps)
+{
+  std::ifstream ifs(filename);
+
+  if (!ifs)
+  {
+    return;
+  }
+
+  std::string line;
+  std::getline(ifs, line);  // get first line
+  std::vector<std::string> contents;
+  parseColumns(line, &contents);
+
+  // std::getline(ifs, line);  // remove second line
+  while (std::getline(ifs, line))
+  {
+    autoware_msgs::Waypoint wp;
+    parseWaypointForVer3(line, contents, &wp);
+    wps->emplace_back(wp);
+  }
+}
+
+void WaypointLoaderNode::parseWaypointForVer3(const std::string& line, const std::vector<std::string>& contents,
+                                              autoware_msgs::Waypoint* wp)
+{
+  std::vector<std::string> columns;
+  parseColumns(line, &columns);
+  std::unordered_map<std::string, std::string> map;
+  for (size_t i = 0; i < contents.size(); i++)
+  {
+    map[contents.at(i)] = columns.at(i);
+  }
+
+  wp->pose.pose.position.x = std::stod(map["x"]);
+  wp->pose.pose.position.y = std::stod(map["y"]);
+  wp->pose.pose.position.z = std::stod(map["z"]);
+  wp->pose.pose.orientation = tf::createQuaternionMsgFromYaw(std::stod(map["yaw"]));
+  wp->twist.twist.linear.x = kmph2mps(std::stod(map["velocity"]));
+  wp->change_flag = std::stoi(map["change_flag"]);
+  wp->wpstate.steering_state = (map.find("steering_flag") != map.end()) ? std::stoi(map["steering_flag"]) : 0;
+  wp->wpstate.accel_state = (map.find("accel_flag") != map.end()) ? std::stoi(map["accel_flag"]) : 0;
+  wp->wpstate.stop_state = (map.find("stop_flag") != map.end()) ? std::stoi(map["stop_flag"]) : 0;
+  wp->wpstate.event_state = (map.find("event_flag") != map.end()) ? std::stoi(map["event_flag"]) : 0;
+}
+
+FileFormat WaypointLoaderNode::checkFileFormat(const char* filename)
+{
+  std::ifstream ifs(filename);
+
+  if (!ifs)
+  {
+    return FileFormat::unknown;
+  }
+
+  // get first line
+  std::string line;
+  std::getline(ifs, line);
+
+  // parse first line
+  std::vector<std::string> parsed_columns;
+  parseColumns(line, &parsed_columns);
+
+  // check if first element in the first column does not include digit
+  if (!std::any_of(parsed_columns.at(0).cbegin(), parsed_columns.at(0).cend(), isdigit))
+  {
+    return FileFormat::ver3;
+  }
+
+  // if element consists only digit
+  int num_of_columns = countColumns(line);
+  ROS_INFO("columns size: %d", num_of_columns);
+
+  return (num_of_columns == 3 ? FileFormat::ver1  // if data consists "x y z (velocity)"
+                                :
+                                num_of_columns == 4 ? FileFormat::ver2  // if data consists "x y z yaw (velocity)
+                                                      :
+                                                      FileFormat::unknown);
+}
+
+bool WaypointLoaderNode::verifyFileConsistency(const char* filename)
+{
+  ROS_INFO("verify...");
+  std::ifstream ifs(filename);
+
+  if (!ifs)
+  {
+    return false;
+  }
+
+  FileFormat format = checkFileFormat(filename);
+  ROS_INFO("format: %d", static_cast<int>(format));
+  if (format == FileFormat::unknown)
+  {
+    ROS_ERROR("unknown file format");
+    return false;
+  }
+
+  std::string line;
+  std::getline(ifs, line);  // remove first line
+
+  size_t ncol = format == FileFormat::ver1 ? 4  // x,y,z,velocity
+                                             :
+                                             format == FileFormat::ver2 ? 5  // x,y,z,yaw,velocity
+                                                                          :
+                                                                          countColumns(line);
+
+  while (std::getline(ifs, line))  // search from second line
+  {
+    if (countColumns(line) != ncol)
+    {
+      return false;
+    }
+  }
+  return true;
+}
+
+void parseColumns(const std::string& line, std::vector<std::string>* columns)
+{
+  std::istringstream ss(line);
+  std::string column;
+  while (std::getline(ss, column, ','))
+  {
+    while (1)
+    {
+      auto res = std::find(column.begin(), column.end(), ' ');
+      if (res == column.end())
+      {
+        break;
+      }
+      column.erase(res);
+    }
+    if (!column.empty())
+    {
+      columns->emplace_back(column);
+    }
+  }
+}
+
+size_t countColumns(const std::string& line)
+{
+  std::istringstream ss(line);
+  size_t ncol = 0;
+
+  std::string column;
+  while (std::getline(ss, column, ','))
+  {
+    ++ncol;
+  }
+
+  return ncol;
+}
+
+}  // waypoint_maker

src/ros/catkin/src/waypoint_maker/nodes/waypoint_loader/waypoint_loader_core.h

@@ -0,0 +1,100 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef WAYPOINT_LOADER_CORE_H
+#define WAYPOINT_LOADER_CORE_H
+
+// ROS includes
+#include <ros/ros.h>
+
+// C++ includes
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <std_msgs/Bool.h>
+#include <tf/transform_datatypes.h>
+#include <unordered_map>
+
+#include "autoware_msgs/LaneArray.h"
+
+namespace waypoint_maker
+{
+const std::string MULTI_LANE_CSV = "/tmp/driving_lane.csv";
+
+enum class FileFormat : int32_t
+{
+  ver1,  // x,y,z,(velocity)
+  ver2,  // x,y,z,yaw,(velocity)
+  ver3,  // first line consists on explanation of values
+
+  unknown = -1,
+};
+
+typedef std::underlying_type<FileFormat>::type FileFormatInteger;
+
+inline double kmph2mps(double velocity_kmph)
+{
+  return (velocity_kmph * 1000) / (60 * 60);
+}
+inline double mps2kmph(double velocity_mps)
+{
+  return (velocity_mps * 60 * 60) / 1000;
+}
+
+class WaypointLoaderNode
+{
+public:
+  WaypointLoaderNode();
+  ~WaypointLoaderNode();
+  void run();
+
+private:
+  // handle
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+
+  // publisher & subscriber
+  ros::Publisher lane_pub_;
+  ros::Subscriber config_sub_;
+
+  // variables
+  std::string multi_lane_csv_;
+  std::vector<std::string> multi_file_path_;
+  autoware_msgs::LaneArray output_lane_array_;
+
+  // initializer
+  void initPubSub();
+
+  // functions
+  void createLaneWaypoint(const std::string& file_path, autoware_msgs::Lane* lane);
+  void createLaneArray(const std::vector<std::string>& paths, autoware_msgs::LaneArray* lane_array);
+
+  FileFormat checkFileFormat(const char* filename);
+  bool verifyFileConsistency(const char* filename);
+  void loadWaypointsForVer1(const char* filename, std::vector<autoware_msgs::Waypoint>* wps);
+  void parseWaypointForVer1(const std::string& line, autoware_msgs::Waypoint* wp);
+  void loadWaypointsForVer2(const char* filename, std::vector<autoware_msgs::Waypoint>* wps);
+  void parseWaypointForVer2(const std::string& line, autoware_msgs::Waypoint* wp);
+  void loadWaypointsForVer3(const char* filename, std::vector<autoware_msgs::Waypoint>* wps);
+  void parseWaypointForVer3(const std::string& line, const std::vector<std::string>& contents,
+                            autoware_msgs::Waypoint* wp);
+};
+
+const std::string addFileSuffix(std::string file_path, std::string suffix);
+void parseColumns(const std::string& line, std::vector<std::string>* columns);
+size_t countColumns(const std::string& line);
+}
+#endif  // WAYPOINT_LOADER_CORE_H

src/ros/catkin/src/waypoint_maker/nodes/waypoint_loader/waypoint_loader_node.cpp

@@ -0,0 +1,29 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// ROS Includes
+#include <ros/ros.h>
+
+#include "waypoint_loader_core.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "waypoint_loader");
+  waypoint_maker::WaypointLoaderNode wln;
+  wln.run();
+
+  return 0;
+}

src/ros/catkin/src/waypoint_maker/nodes/waypoint_marker_publisher/waypoint_marker_publisher.cpp

@@ -0,0 +1,445 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <ros/ros.h>
+#include <ros/console.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <std_msgs/Int32.h>
+#include <tf/transform_datatypes.h>
+
+#include <iostream>
+#include <vector>
+#include <string>
+
+#include "libwaypoint_follower/libwaypoint_follower.h"
+#include "autoware_msgs/LaneArray.h"
+#include "autoware_config_msgs/ConfigLaneStop.h"
+#include "autoware_msgs/TrafficLight.h"
+
+namespace
+{
+ros::Publisher g_local_mark_pub;
+ros::Publisher g_global_mark_pub;
+
+constexpr int32_t TRAFFIC_LIGHT_RED = 0;
+constexpr int32_t TRAFFIC_LIGHT_GREEN = 1;
+constexpr int32_t TRAFFIC_LIGHT_UNKNOWN = 2;
+
+std_msgs::ColorRGBA _initial_color;
+std_msgs::ColorRGBA _global_color;
+std_msgs::ColorRGBA g_local_color;
+const double g_global_alpha = 0.2;
+const double g_local_alpha = 1.0;
+int _closest_waypoint = -1;
+
+visualization_msgs::MarkerArray g_global_marker_array;
+visualization_msgs::MarkerArray g_local_waypoints_marker_array;
+
+bool g_config_manual_detection = true;
+
+enum class ChangeFlag : int32_t
+{
+  straight,
+  right,
+  left,
+
+  unknown = -1,
+};
+
+typedef std::underlying_type<ChangeFlag>::type ChangeFlagInteger;
+
+void setLifetime(double sec, visualization_msgs::MarkerArray* marker_array)
+{
+  ros::Duration lifetime(sec);
+  for (auto& marker : marker_array->markers)
+  {
+    marker.lifetime = lifetime;
+  }
+}
+
+void publishMarkerArray(const visualization_msgs::MarkerArray& marker_array, const ros::Publisher& publisher, bool delete_markers=false)
+{
+  visualization_msgs::MarkerArray msg;
+
+  // insert local marker
+  msg.markers.insert(msg.markers.end(), marker_array.markers.begin(), marker_array.markers.end());
+
+  if (delete_markers)
+  {
+    for (auto& marker : msg.markers)
+    {
+      marker.action = visualization_msgs::Marker::DELETE;
+    }
+  }
+
+  publisher.publish(msg);
+}
+
+
+
+void createGlobalLaneArrayVelocityMarker(const autoware_msgs::LaneArray& lane_waypoints_array)
+{
+  visualization_msgs::MarkerArray tmp_marker_array;
+  // display by markers the velocity of each waypoint.
+  visualization_msgs::Marker velocity_marker;
+  velocity_marker.header.frame_id = "map";
+  velocity_marker.header.stamp = ros::Time::now();
+  velocity_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+  velocity_marker.action = visualization_msgs::Marker::ADD;
+  velocity_marker.scale.z = 0.4;
+  velocity_marker.color.r = 1;
+  velocity_marker.color.g = 1;
+  velocity_marker.color.b = 1;
+  velocity_marker.color.a = 1.0;
+  velocity_marker.frame_locked = true;
+
+  int count = 1;
+  for (auto lane : lane_waypoints_array.lanes)
+  {
+    velocity_marker.ns = "global_velocity_lane_" + std::to_string(count);
+    for (int i = 0; i < static_cast<int>(lane.waypoints.size()); i++)
+    {
+      velocity_marker.id = i;
+      geometry_msgs::Point relative_p;
+      relative_p.y = 0.5;
+      velocity_marker.pose.position = calcAbsoluteCoordinate(relative_p, lane.waypoints[i].pose.pose);
+      velocity_marker.pose.position.z += 0.2;
+
+      // double to string
+      std::string vel = std::to_string(mps2kmph(lane.waypoints[i].twist.twist.linear.x));
+      velocity_marker.text = vel.erase(vel.find_first_of(".") + 2);
+
+      tmp_marker_array.markers.push_back(velocity_marker);
+    }
+    count++;
+  }
+
+  g_global_marker_array.markers.insert(g_global_marker_array.markers.end(), tmp_marker_array.markers.begin(),
+                                       tmp_marker_array.markers.end());
+}
+
+void createGlobalLaneArrayChangeFlagMarker(const autoware_msgs::LaneArray& lane_waypoints_array)
+{
+  visualization_msgs::MarkerArray tmp_marker_array;
+  // display by markers the velocity of each waypoint.
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time::now();
+  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.scale.z = 0.4;
+  marker.color.r = 1;
+  marker.color.g = 1;
+  marker.color.b = 1;
+  marker.color.a = 1.0;
+  marker.frame_locked = true;
+
+  int count = 1;
+  for (auto lane : lane_waypoints_array.lanes)
+  {
+    marker.ns = "global_change_flag_lane_" + std::to_string(count);
+    for (int i = 0; i < static_cast<int>(lane.waypoints.size()); i++)
+    {
+      marker.id = i;
+      geometry_msgs::Point relative_p;
+      relative_p.x = -0.1;
+      marker.pose.position = calcAbsoluteCoordinate(relative_p, lane.waypoints[i].pose.pose);
+      marker.pose.position.z += 0.2;
+
+      // double to string
+      std::string str = "";
+      if (lane.waypoints[i].change_flag == static_cast<ChangeFlagInteger>(ChangeFlag::straight))
+      {
+        str = "S";
+      }
+      else if (lane.waypoints[i].change_flag == static_cast<ChangeFlagInteger>(ChangeFlag::right))
+      {
+        str = "R";
+      }
+      else if (lane.waypoints[i].change_flag == static_cast<ChangeFlagInteger>(ChangeFlag::left))
+      {
+        str = "L";
+      }
+      else if (lane.waypoints[i].change_flag == static_cast<ChangeFlagInteger>(ChangeFlag::unknown))
+      {
+        str = "U";
+      }
+
+      marker.text = str;
+
+      tmp_marker_array.markers.push_back(marker);
+    }
+    count++;
+  }
+
+  g_global_marker_array.markers.insert(g_global_marker_array.markers.end(), tmp_marker_array.markers.begin(),
+                                       tmp_marker_array.markers.end());
+}
+
+void createLocalWaypointVelocityMarker(std_msgs::ColorRGBA color, int closest_waypoint,
+                                       const autoware_msgs::Lane& lane_waypoint)
+{
+  // display by markers the velocity of each waypoint.
+  visualization_msgs::Marker velocity;
+  velocity.header.frame_id = "map";
+  velocity.header.stamp = ros::Time::now();
+  velocity.ns = "local_waypoint_velocity";
+  velocity.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+  velocity.action = visualization_msgs::Marker::ADD;
+  velocity.scale.z = 0.4;
+  velocity.color = color;
+  velocity.frame_locked = true;
+
+  for (int i = 0; i < static_cast<int>(lane_waypoint.waypoints.size()); i++)
+  {
+    velocity.id = i;
+    geometry_msgs::Point relative_p;
+    relative_p.y = -0.65;
+    velocity.pose.position = calcAbsoluteCoordinate(relative_p, lane_waypoint.waypoints[i].pose.pose);
+    velocity.pose.position.z += 0.2;
+
+    // double to string
+    std::ostringstream oss;
+    oss << std::fixed << std::setprecision(1) << mps2kmph(lane_waypoint.waypoints[i].twist.twist.linear.x);
+    velocity.text = oss.str();
+
+    g_local_waypoints_marker_array.markers.push_back(velocity);
+  }
+}
+
+void createGlobalLaneArrayMarker(std_msgs::ColorRGBA color, const autoware_msgs::LaneArray& lane_waypoints_array)
+{
+  visualization_msgs::Marker lane_waypoint_marker;
+  lane_waypoint_marker.header.frame_id = "map";
+  lane_waypoint_marker.header.stamp = ros::Time::now();
+  lane_waypoint_marker.ns = "global_lane_array_marker";
+  lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
+  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+  lane_waypoint_marker.scale.x = 1.0;
+  lane_waypoint_marker.color = color;
+  lane_waypoint_marker.frame_locked = true;
+
+  int count = 0;
+  for (auto lane : lane_waypoints_array.lanes)
+  {
+    lane_waypoint_marker.points.clear();
+    lane_waypoint_marker.id = count;
+
+    for (auto el : lane.waypoints)
+    {
+      geometry_msgs::Point point;
+      point = el.pose.pose.position;
+      lane_waypoint_marker.points.push_back(point);
+    }
+    g_global_marker_array.markers.push_back(lane_waypoint_marker);
+    count++;
+  }
+}
+
+void createGlobalLaneArrayOrientationMarker(const autoware_msgs::LaneArray& lane_waypoints_array)
+{
+  visualization_msgs::MarkerArray tmp_marker_array;
+  visualization_msgs::Marker lane_waypoint_marker;
+  lane_waypoint_marker.header.frame_id = "map";
+  lane_waypoint_marker.header.stamp = ros::Time::now();
+  lane_waypoint_marker.type = visualization_msgs::Marker::ARROW;
+  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+  lane_waypoint_marker.scale.x = 0.25;
+  lane_waypoint_marker.scale.y = 0.05;
+  lane_waypoint_marker.scale.z = 0.05;
+  lane_waypoint_marker.color.r = 1.0;
+  lane_waypoint_marker.color.a = 1.0;
+  lane_waypoint_marker.frame_locked = true;
+
+  int count = 1;
+  for (auto lane : lane_waypoints_array.lanes)
+  {
+    lane_waypoint_marker.ns = "global_lane_waypoint_orientation_marker_" + std::to_string(count);
+
+    for (int i = 0; i < static_cast<int>(lane.waypoints.size()); i++)
+    {
+      lane_waypoint_marker.id = i;
+      lane_waypoint_marker.pose = lane.waypoints[i].pose.pose;
+      tmp_marker_array.markers.push_back(lane_waypoint_marker);
+    }
+    count++;
+  }
+
+  g_global_marker_array.markers.insert(g_global_marker_array.markers.end(), tmp_marker_array.markers.begin(),
+                                       tmp_marker_array.markers.end());
+}
+
+void createLocalPathMarker(std_msgs::ColorRGBA color, const autoware_msgs::Lane& lane_waypoint)
+{
+  visualization_msgs::Marker lane_waypoint_marker;
+  lane_waypoint_marker.header.frame_id = "map";
+  lane_waypoint_marker.header.stamp = ros::Time::now();
+  lane_waypoint_marker.ns = "local_path_marker";
+  lane_waypoint_marker.id = 0;
+  lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
+  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+  lane_waypoint_marker.scale.x = 0.2;
+  lane_waypoint_marker.color = color;
+  lane_waypoint_marker.frame_locked = true;
+
+  for (unsigned int i = 0; i < lane_waypoint.waypoints.size(); i++)
+  {
+    geometry_msgs::Point point;
+    point = lane_waypoint.waypoints[i].pose.pose.position;
+    lane_waypoint_marker.points.push_back(point);
+  }
+  g_local_waypoints_marker_array.markers.push_back(lane_waypoint_marker);
+}
+
+void createLocalPointMarker(const autoware_msgs::Lane& lane_waypoint)
+{
+  visualization_msgs::Marker lane_waypoint_marker;
+  lane_waypoint_marker.header.frame_id = "map";
+  lane_waypoint_marker.header.stamp = ros::Time::now();
+  lane_waypoint_marker.ns = "local_point_marker";
+  lane_waypoint_marker.id = 0;
+  lane_waypoint_marker.type = visualization_msgs::Marker::CUBE_LIST;
+  lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
+  lane_waypoint_marker.scale.x = 0.2;
+  lane_waypoint_marker.scale.y = 0.2;
+  lane_waypoint_marker.scale.z = 0.2;
+  lane_waypoint_marker.color.r = 1.0;
+  lane_waypoint_marker.color.a = 1.0;
+  lane_waypoint_marker.frame_locked = true;
+
+  for (unsigned int i = 0; i < lane_waypoint.waypoints.size(); i++)
+  {
+    geometry_msgs::Point point;
+    point = lane_waypoint.waypoints[i].pose.pose.position;
+    lane_waypoint_marker.points.push_back(point);
+  }
+  g_local_waypoints_marker_array.markers.push_back(lane_waypoint_marker);
+}
+
+void lightCallback(const autoware_msgs::TrafficLightConstPtr& msg)
+{
+  std_msgs::ColorRGBA global_color;
+  global_color.a = g_global_alpha;
+
+  std_msgs::ColorRGBA local_color;
+  local_color.a = g_local_alpha;
+
+  switch (msg->traffic_light)
+  {
+    case TRAFFIC_LIGHT_RED:
+      global_color.r = 1.0;
+      _global_color = global_color;
+      local_color.r = 1.0;
+      g_local_color = local_color;
+      break;
+    case TRAFFIC_LIGHT_GREEN:
+      global_color.g = 1.0;
+      _global_color = global_color;
+      local_color.g = 1.0;
+      g_local_color = local_color;
+      break;
+    case TRAFFIC_LIGHT_UNKNOWN:
+      global_color.b = 1.0;
+      global_color.g = 0.7;
+      _global_color = global_color;
+      local_color.b = 1.0;
+      local_color.g = 0.7;
+      g_local_color = local_color;
+      break;
+    default:
+      ROS_ERROR("unknown traffic_light");
+      return;
+  }
+}
+
+void receiveAutoDetection(const autoware_msgs::TrafficLightConstPtr& msg)
+{
+  if (!g_config_manual_detection)
+    lightCallback(msg);
+}
+
+void receiveManualDetection(const autoware_msgs::TrafficLightConstPtr& msg)
+{
+  if (g_config_manual_detection)
+    lightCallback(msg);
+}
+
+void configParameter(const autoware_config_msgs::ConfigLaneStopConstPtr& msg)
+{
+  g_config_manual_detection = msg->manual_detection;
+}
+
+void laneArrayCallback(const autoware_msgs::LaneArrayConstPtr& msg)
+{
+  publishMarkerArray(g_global_marker_array, g_global_mark_pub, true);
+  g_global_marker_array.markers.clear();
+  createGlobalLaneArrayVelocityMarker(*msg);
+  createGlobalLaneArrayOrientationMarker(*msg);
+  createGlobalLaneArrayChangeFlagMarker(*msg);
+  publishMarkerArray(g_global_marker_array, g_global_mark_pub);
+}
+
+void finalCallback(const autoware_msgs::LaneConstPtr& msg)
+{
+  g_local_waypoints_marker_array.markers.clear();
+  if (_closest_waypoint != -1)
+    createLocalWaypointVelocityMarker(g_local_color, _closest_waypoint, *msg);
+  createLocalPathMarker(g_local_color, *msg);
+  createLocalPointMarker(*msg);
+  setLifetime(0.5, &g_local_waypoints_marker_array);
+  publishMarkerArray(g_local_waypoints_marker_array, g_local_mark_pub);
+}
+
+void closestCallback(const std_msgs::Int32ConstPtr& msg)
+{
+  _closest_waypoint = msg->data;
+}
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "waypoints_marker_publisher");
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  // subscribe traffic light
+  ros::Subscriber light_sub = nh.subscribe("light_color", 10, receiveAutoDetection);
+  ros::Subscriber light_managed_sub = nh.subscribe("light_color_managed", 10, receiveManualDetection);
+
+  // subscribe global waypoints
+  ros::Subscriber lane_array_sub = nh.subscribe("lane_waypoints_array", 10, laneArrayCallback);
+  ros::Subscriber traffic_array_sub = nh.subscribe("traffic_waypoints_array", 10, laneArrayCallback);
+
+  // subscribe local waypoints
+  ros::Subscriber final_sub = nh.subscribe("final_waypoints", 10, finalCallback);
+  ros::Subscriber closest_sub = nh.subscribe("closest_waypoint", 10, closestCallback);
+
+  // subscribe config
+  ros::Subscriber config_sub = nh.subscribe("config/lane_stop", 10, configParameter);
+
+  g_local_mark_pub = nh.advertise<visualization_msgs::MarkerArray>("local_waypoints_mark", 10, true);
+  g_global_mark_pub = nh.advertise<visualization_msgs::MarkerArray>("global_waypoints_mark", 10, true);
+
+  // initialize path color
+  _initial_color.g = 0.7;
+  _initial_color.b = 1.0;
+  _global_color = _initial_color;
+  _global_color.a = g_global_alpha;
+  g_local_color = _initial_color;
+  g_local_color.a = g_local_alpha;
+
+  ros::spin();
+}

src/ros/catkin/src/waypoint_maker/nodes/waypoint_replanner/waypoint_replanner.cpp

@@ -0,0 +1,456 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "waypoint_replanner.h"
+
+namespace waypoint_maker
+{
+
+WaypointReplanner::WaypointReplanner()
+{
+}
+
+WaypointReplanner::~WaypointReplanner()
+{
+}
+
+void WaypointReplanner::updateConfig(const WaypointReplannerConfig& config)
+{
+  config_ = config;
+  config_.radius_inf = 10 * config_.radius_thresh;
+  config_.velocity_param = calcVelParam(config_.velocity_max);
+}
+
+void WaypointReplanner::initParameter(const autoware_config_msgs::ConfigWaypointReplanner::ConstPtr& conf)
+{
+  WaypointReplannerConfig temp_config;
+
+  temp_config.velocity_max = kmph2mps(conf->velocity_max);
+  temp_config.velocity_min = kmph2mps(conf->velocity_min);
+  temp_config.accel_limit = conf->accel_limit;
+  temp_config.decel_limit = conf->decel_limit;
+  temp_config.radius_thresh = conf->radius_thresh;
+  temp_config.radius_min = conf->radius_min;
+  temp_config.lookup_crv_width = 5;
+  temp_config.resample_mode = conf->resample_mode;
+  temp_config.resample_interval = conf->resample_interval;
+  temp_config.replan_curve_mode = conf->replan_curve_mode;
+  temp_config.replan_endpoint_mode = conf->replan_endpoint_mode;
+  temp_config.overwrite_vmax_mode = conf->overwrite_vmax_mode;
+  temp_config.velocity_offset = conf->velocity_offset;
+  temp_config.end_point_offset = conf->end_point_offset;
+  temp_config.braking_distance = conf->braking_distance;
+
+  updateConfig(temp_config);
+}
+
+void WaypointReplanner::changeVelSign(autoware_msgs::Lane& lane, bool positive) const
+{
+  const int sgn = positive ? 1 : -1;
+  for (auto& el : lane.waypoints)
+  {
+    el.twist.twist.linear.x = sgn * fabs(el.twist.twist.linear.x);
+  }
+}
+
+void WaypointReplanner::replanLaneWaypointVel(autoware_msgs::Lane& lane)
+{
+  if (lane.waypoints.empty())
+  {
+    return;
+  }
+  const LaneDirection dir = getLaneDirection(lane);
+  unsigned long last = lane.waypoints.size() - 1;
+  changeVelSign(lane, true);
+  limitVelocityByRange(0, last, 0, config_.velocity_max, lane);
+  if (config_.resample_mode)
+  {
+    resampleLaneWaypoint(config_.resample_interval, lane, dir);
+    last = lane.waypoints.size() - 1;
+  }
+  if (config_.replan_curve_mode)
+  {
+    std::vector<double> curve_radius;
+    KeyVal curve_list;
+    createRadiusList(lane, curve_radius);
+    createCurveList(curve_radius, curve_list);
+    if (config_.overwrite_vmax_mode)
+    {// set velocity_max for all_point
+      setVelocityByRange(0, last, 0, config_.velocity_max, lane);
+    }
+    // set velocity by curve
+    for (const auto& el : curve_list)
+    {
+      const double& radius = el.second.second;
+      double vmin = config_.velocity_max - config_.velocity_param * (config_.radius_thresh - radius);
+      vmin = (vmin < config_.velocity_min) ? config_.velocity_min : vmin;
+      limitVelocityByRange(el.first, el.second.first, config_.velocity_offset, vmin, lane);
+    }
+  }
+  // set velocity on start & end of lane
+  if (config_.replan_endpoint_mode)
+  {
+    const unsigned long zerospeed_start = last - config_.end_point_offset;
+    const unsigned long lowspeed_start = zerospeed_start - config_.braking_distance;
+    raiseVelocityByRange(0, last, 0, config_.velocity_min, lane);
+    limitVelocityByRange(0, 0, 0, config_.velocity_min, lane);
+    limitVelocityByRange(lowspeed_start, last, 0, config_.velocity_min, lane);
+    setVelocityByRange(zerospeed_start, last, 0, 0.0, lane);
+  }
+  if (dir == LaneDirection::Backward)
+  {
+    changeVelSign(lane, false);
+  }
+}
+
+void WaypointReplanner::resampleLaneWaypoint(const double resample_interval, autoware_msgs::Lane& lane, LaneDirection dir)
+{
+  if (lane.waypoints.size() < 2)
+  {
+    return;
+  }
+  autoware_msgs::Lane original_lane(lane);
+  lane.waypoints.clear();
+  lane.waypoints.emplace_back(original_lane.waypoints[0]);
+  lane.waypoints.reserve(ceil(1.5 * calcPathLength(original_lane) / config_.resample_interval));
+
+  for (unsigned long i = 1; i < original_lane.waypoints.size(); i++)
+  {
+    CbufGPoint curve_point = getCrvPointsOnResample(lane, original_lane, i);
+    const std::vector<double> curve_param = calcCurveParam(curve_point);
+    lane.waypoints.back().twist.twist = original_lane.waypoints[i - 1].twist.twist;
+    lane.waypoints.back().wpstate = original_lane.waypoints[i - 1].wpstate;
+    lane.waypoints.back().change_flag = original_lane.waypoints[i - 1].change_flag;
+    // if going straight
+    if (curve_param.empty())
+    {
+      resampleOnStraight(curve_point, lane, dir);
+    }
+    // else if turnning curve
+    else
+    {
+      resampleOnCurve(curve_point[1], curve_param, lane, dir);
+    }
+  }
+  lane.waypoints[0].pose.pose.orientation = lane.waypoints[1].pose.pose.orientation;
+  lane.waypoints.back().twist.twist = original_lane.waypoints.back().twist.twist;
+  lane.waypoints.back().wpstate = original_lane.waypoints.back().wpstate;
+  lane.waypoints.back().change_flag = original_lane.waypoints.back().change_flag;
+}
+
+void WaypointReplanner::resampleOnStraight(const CbufGPoint& curve_point, autoware_msgs::Lane& lane, LaneDirection dir)
+{
+  if (curve_point.size() != 3)
+  {
+    return;
+  }
+  autoware_msgs::Waypoint wp(lane.waypoints.back());
+  const geometry_msgs::Point& pt = wp.pose.pose.position;
+  const int sgn = (dir == LaneDirection::Forward) ? 0 : 1;
+  const double yaw = atan2(curve_point[2].y - curve_point[0].y, curve_point[2].x - curve_point[0].x) + sgn * M_PI;
+  wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
+
+  const std::vector<double> nvec = { curve_point[1].x - pt.x, curve_point[1].y - pt.y, curve_point[1].z - pt.z };
+  double dist = sqrt(nvec[0] * nvec[0] + nvec[1] * nvec[1]);
+  std::vector<double> resample_vec = nvec;
+  const double coeff = config_.resample_interval / dist;
+  for (auto& el : resample_vec)
+  {
+    el *= coeff;
+  }
+  for (; dist > config_.resample_interval; dist -= config_.resample_interval)
+  {
+    wp.pose.pose.position.x += resample_vec[0];
+    wp.pose.pose.position.y += resample_vec[1];
+    wp.pose.pose.position.z += resample_vec[2];
+    lane.waypoints.emplace_back(wp);
+  }
+}
+
+void WaypointReplanner::resampleOnCurve(const geometry_msgs::Point& target_point,
+                                        const std::vector<double>& curve_param, autoware_msgs::Lane& lane, LaneDirection dir)
+{
+  if (curve_param.size() != 3)
+  {
+    return;
+  }
+  autoware_msgs::Waypoint wp(lane.waypoints.back());
+  const double& cx = curve_param[0];
+  const double& cy = curve_param[1];
+  const double& radius = curve_param[2];
+  const double reverse_angle = (dir == LaneDirection::Backward) ? M_PI : 0.0;
+
+  const geometry_msgs::Point& p0 = wp.pose.pose.position;
+  const geometry_msgs::Point& p1 = target_point;
+  double theta = fmod(atan2(p1.y - cy, p1.x - cx) - atan2(p0.y - cy, p0.x - cx), 2 * M_PI);
+  int sgn = (theta > 0.0) ? (1) : (-1);
+  if (fabs(theta) > M_PI)
+  {
+    theta -= 2 * sgn * M_PI;
+  }
+  sgn = (theta > 0.0) ? (1) : (-1);
+  // interport
+  double t = atan2(p0.y - cy, p0.x - cx);
+  double dist = radius * fabs(theta);
+  const double resample_dz = config_.resample_interval * (p1.z - p0.z) / dist;
+  for (; dist > config_.resample_interval; dist -= config_.resample_interval)
+  {
+    if (lane.waypoints.size() == lane.waypoints.capacity())
+    {
+      break;
+    }
+    t += sgn * config_.resample_interval / radius;
+    const double yaw = fmod(t + sgn * M_PI / 2.0, 2 * M_PI) + reverse_angle;
+    wp.pose.pose.position.x = cx + radius * cos(t);
+    wp.pose.pose.position.y = cy + radius * sin(t);
+    wp.pose.pose.position.z += resample_dz;
+    wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
+    lane.waypoints.emplace_back(wp);
+  }
+}
+
+// Three points used for curve detection (the target point is the center)
+// [0] = previous point, [1] = target point, [2] = next point
+const CbufGPoint WaypointReplanner::getCrvPointsOnResample(
+    const autoware_msgs::Lane& lane, const autoware_msgs::Lane& original_lane, unsigned long original_index) const
+{
+  unsigned long id = original_index;
+  CbufGPoint curve_point(3);
+  const unsigned int n = (config_.lookup_crv_width - 1) / 2;
+  const autoware_msgs::Waypoint cp[3] = {
+    (lane.waypoints.size() < n) ? lane.waypoints.front() : lane.waypoints[lane.waypoints.size() - n],
+    original_lane.waypoints[id],
+    (id < original_lane.waypoints.size() - n) ? original_lane.waypoints[id + n] : original_lane.waypoints.back()
+  };
+  for (int i = 0; i < 3; i++)
+  {
+    curve_point.push_back(cp[i].pose.pose.position);
+  }
+  return curve_point;
+}
+
+const CbufGPoint WaypointReplanner::getCrvPoints(const autoware_msgs::Lane& lane, unsigned long index) const
+{
+  CbufGPoint curve_point(3);
+  const unsigned int n = (config_.lookup_crv_width - 1) / 2;
+  const unsigned long curve_index[3] = { (index < n) ? 0 : (index - n), index, (index >= lane.waypoints.size() - n) ?
+                                                                                   (lane.waypoints.size() - 1) :
+                                                                                   (index + n) };
+  for (int i = 0; i < 3; i++)
+  {
+    curve_point.push_back(lane.waypoints[curve_index[i]].pose.pose.position);
+  }
+  return curve_point;
+}
+
+void WaypointReplanner::createRadiusList(const autoware_msgs::Lane& lane, std::vector<double>& curve_radius)
+{
+  if (lane.waypoints.empty())
+  {
+    return;
+  }
+  curve_radius.resize(lane.waypoints.size());
+  curve_radius.at(0) = curve_radius.back() = config_.radius_inf;
+
+  for (unsigned long i = 1; i < lane.waypoints.size() - 1; i++)
+  {
+    CbufGPoint curve_point(getCrvPoints(lane, i));
+    const std::vector<double> curve_param(calcCurveParam(curve_point));
+
+    // if going straight
+    if (curve_param.empty())
+    {
+      curve_radius.at(i) = config_.radius_inf;
+    }
+    // else if turnning curve
+    else
+    {
+      curve_radius.at(i) = (curve_param[2] > config_.radius_inf) ? config_.radius_inf : curve_param[2];
+    }
+  }
+}
+
+const double WaypointReplanner::calcVelParam(double vmax) const
+{
+  if (fabs(config_.radius_thresh - config_.radius_min) < 1e-8)
+  {
+    return DBL_MAX;  // error
+  }
+  return (vmax - config_.velocity_min) / (config_.radius_thresh - config_.radius_min);
+}
+
+void WaypointReplanner::createCurveList(const std::vector<double>& curve_radius, KeyVal& curve_list)
+{
+  unsigned long index = 0;
+  bool on_curve = false;
+  double radius_localmin = DBL_MAX;
+  for (unsigned long i = 1; i < curve_radius.size(); i++)
+  {
+    if (!on_curve && curve_radius[i] <= config_.radius_thresh && curve_radius[i - 1] > config_.radius_thresh)
+    {
+      index = i;
+      on_curve = true;
+    }
+    else if (on_curve && curve_radius[i - 1] <= config_.radius_thresh && curve_radius[i] > config_.radius_thresh)
+    {
+      on_curve = false;
+      if (radius_localmin < config_.radius_min)
+      {
+        radius_localmin = config_.radius_min;
+      }
+      curve_list[index] = std::make_pair(i, radius_localmin);
+      radius_localmin = DBL_MAX;
+    }
+    if (!on_curve)
+    {
+      continue;
+    }
+    if (radius_localmin > curve_radius[i])
+    {
+      radius_localmin = curve_radius[i];
+    }
+  }
+}
+
+
+void WaypointReplanner::setVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
+                                             double vel, autoware_msgs::Lane& lane)
+{
+  if (lane.waypoints.empty())
+  {
+    return;
+  }
+  if (offset > 0)
+  {
+    start_idx = (start_idx > offset) ? (start_idx - offset) : 0;
+    end_idx = (end_idx > offset) ? (end_idx - offset) : 0;
+  }
+  end_idx = (end_idx >= lane.waypoints.size()) ? lane.waypoints.size() - 1 : end_idx;
+  for (unsigned long idx = start_idx; idx <= end_idx; idx++)
+  {
+    lane.waypoints[idx].twist.twist.linear.x = vel;
+  }
+}
+
+void WaypointReplanner::raiseVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
+                                           double vmin, autoware_msgs::Lane& lane)
+{
+  if (lane.waypoints.empty())
+  {
+    return;
+  }
+  if (offset > 0)
+  {
+    start_idx = (start_idx > offset) ? (start_idx - offset) : 0;
+    end_idx = (end_idx > offset) ? (end_idx - offset) : 0;
+  }
+  end_idx = (end_idx >= lane.waypoints.size()) ? lane.waypoints.size() - 1 : end_idx;
+  for (unsigned long idx = start_idx; idx <= end_idx; idx++)
+  {
+    if (lane.waypoints[idx].twist.twist.linear.x >= vmin)
+    {
+      continue;
+    }
+    lane.waypoints[idx].twist.twist.linear.x = vmin;
+  }
+}
+
+void WaypointReplanner::limitVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
+                                             double vmin, autoware_msgs::Lane& lane)
+{
+  if (lane.waypoints.empty())
+  {
+    return;
+  }
+  if (offset > 0)
+  {
+    start_idx = (start_idx > offset) ? (start_idx - offset) : 0;
+    end_idx = (end_idx > offset) ? (end_idx - offset) : 0;
+  }
+  end_idx = (end_idx >= lane.waypoints.size()) ? lane.waypoints.size() - 1 : end_idx;
+  for (unsigned long idx = start_idx; idx <= end_idx; idx++)
+  {
+    if (lane.waypoints[idx].twist.twist.linear.x < vmin)
+    {
+      continue;
+    }
+    lane.waypoints[idx].twist.twist.linear.x = vmin;
+  }
+  limitAccelDecel(start_idx, lane);
+  limitAccelDecel(end_idx, lane);
+}
+
+void WaypointReplanner::limitAccelDecel(const unsigned long idx, autoware_msgs::Lane& lane)
+{
+  const double acc[2] = { config_.accel_limit, config_.decel_limit };
+  const unsigned long end_idx[2] = { lane.waypoints.size() - idx, idx + 1 };
+  const int sgn[2] = { 1, -1 };
+  for (int j = 0; j < 2; j++)  // [j=0]: config_.accel_limitprocess, [j=1]: config_.decel_limitprocess
+  {
+    double v = lane.waypoints[idx].twist.twist.linear.x;
+    unsigned long next = idx + sgn[j];
+    for (unsigned long i = 1; i < end_idx[j]; i++, next += sgn[j])
+    {
+      const geometry_msgs::Point& p0 = lane.waypoints[next - sgn[j]].pose.pose.position;
+      const geometry_msgs::Point& p1 = lane.waypoints[next].pose.pose.position;
+      const double dist = std::hypot(p0.x - p1.x, p0.y - p1.y);
+      v = sqrt(2 * acc[j] * dist + v * v);
+      if (v > config_.velocity_max || v > lane.waypoints[next].twist.twist.linear.x)
+      {
+        break;
+      }
+      lane.waypoints[next].twist.twist.linear.x = v;
+    }
+  }
+}
+
+// get curve 3-Parameter [center_x, center_y, radius] with 3 point input. If error occured, return empty vector.
+const std::vector<double> WaypointReplanner::calcCurveParam(CbufGPoint p) const
+{
+  for (int i = 0; i < 3; i++, p.push_back(p.front()))  // if exception occured, change points order
+  {
+    const double d = 2 * ((p[0].y - p[2].y) * (p[0].x - p[1].x) - (p[0].y - p[1].y) * (p[0].x - p[2].x));
+    if (fabs(d) < 1e-8)
+    {
+      continue;
+    }
+    const std::vector<double> x2 = { p[0].x * p[0].x, p[1].x * p[1].x, p[2].x * p[2].x };
+    const std::vector<double> y2 = { p[0].y * p[0].y, p[1].y * p[1].y, p[2].y * p[2].y };
+    const double a = y2[0] - y2[1] + x2[0] - x2[1];
+    const double b = y2[0] - y2[2] + x2[0] - x2[2];
+    std::vector<double> param(3);
+    const double cx = param[0] = ((p[0].y - p[2].y) * a - (p[0].y - p[1].y) * b) / d;
+    const double cy = param[1] = ((p[0].x - p[2].x) * a - (p[0].x - p[1].x) * b) / -d;
+    param[2] = sqrt((cx - p[0].x) * (cx - p[0].x) + (cy - p[0].y) * (cy - p[0].y));
+    return param;
+  }
+  return std::vector<double>();  // error
+}
+
+const double WaypointReplanner::calcPathLength(const autoware_msgs::Lane& lane) const
+{
+  double distance = 0.0;
+  for (unsigned long i = 1; i < lane.waypoints.size(); i++)
+  {
+    const geometry_msgs::Point& p0 = lane.waypoints[i - 1].pose.pose.position;
+    const geometry_msgs::Point& p1 = lane.waypoints[i].pose.pose.position;
+    tf::Vector3 tf0(p0.x, p0.y, 0.0);
+    tf::Vector3 tf1(p1.x, p1.y, 0.0);
+    distance += tf::tfDistance(tf0, tf1);
+  }
+  return distance;
+}
+
+};

src/ros/catkin/src/waypoint_maker/nodes/waypoint_replanner/waypoint_replanner.h

@@ -0,0 +1,98 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef __WAYPOINT_REPLANNER_H__
+#define __WAYPOINT_REPLANNER_H__
+
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+#include <autoware_config_msgs/ConfigWaypointReplanner.h>
+#include <fstream>
+#include <unordered_map>
+#include <algorithm>
+#include <boost/circular_buffer.hpp>
+#include <autoware_msgs/Lane.h>
+#include <libwaypoint_follower/libwaypoint_follower.h>
+
+namespace waypoint_maker
+{
+typedef std::unordered_map<unsigned long, std::pair<unsigned long, double>> KeyVal;
+typedef boost::circular_buffer<geometry_msgs::Point> CbufGPoint;
+
+struct WaypointReplannerConfig
+{
+  double velocity_max = 0.0;
+  double velocity_min = 0.0;
+  double velocity_param = 0.0;
+  double accel_limit = 0.0;
+  double decel_limit = 0.0;
+  double radius_thresh = 0.0;
+  double radius_min = 0.0;
+  double radius_inf = 0.0;
+  bool resample_mode = false;
+  double resample_interval = 0.0;
+  bool replan_curve_mode = false;
+  bool replan_endpoint_mode = false;
+  bool overwrite_vmax_mode = false;
+  double velocity_offset = 0.0;
+  double end_point_offset =  0.0;
+  double braking_distance = 0.0;
+  int lookup_crv_width = 5;
+};
+
+class WaypointReplanner
+{
+private:
+  WaypointReplannerConfig config_;
+
+public:
+  WaypointReplanner();
+  ~WaypointReplanner();
+  void updateConfig(const WaypointReplannerConfig& config);
+  void initParameter(const autoware_config_msgs::ConfigWaypointReplanner::ConstPtr& conf);
+  void replanLaneWaypointVel(autoware_msgs::Lane& lane);
+
+protected:
+  void changeVelSign(autoware_msgs::Lane& lane, bool positive) const;
+  void resampleLaneWaypoint(const double resample_interval, autoware_msgs::Lane& lane, LaneDirection dir);
+  void resampleOnStraight(const CbufGPoint& curve_point, autoware_msgs::Lane& lane, LaneDirection dir);
+  void resampleOnCurve(const geometry_msgs::Point& target_point,
+    const std::vector<double>& param,autoware_msgs::Lane& lane, LaneDirection dir);
+
+  const CbufGPoint getCrvPointsOnResample(const autoware_msgs::Lane& lane,
+    const autoware_msgs::Lane& original_lane, unsigned long original_index) const;
+  const CbufGPoint getCrvPoints(const autoware_msgs::Lane& lane, unsigned long index) const;
+
+  void createRadiusList(const autoware_msgs::Lane& lane, std::vector<double>& curve_radius);
+  const double calcVelParam(double vmax) const;
+  void createCurveList(const std::vector<double>& curve_radius, KeyVal& curve_list);
+  void createVmaxList(const autoware_msgs::Lane& lane, const KeyVal &curve_list,
+    unsigned long offset, KeyVal &vmax_list);
+  double searchVmaxByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
+    const autoware_msgs::Lane &lane) const;
+  void setVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset, double vel,
+    autoware_msgs::Lane& lane);
+  void raiseVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
+    double vmin, autoware_msgs::Lane& lane);
+
+  void limitVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset, double vmin,
+    autoware_msgs::Lane& lane);
+  void limitAccelDecel(const unsigned long idx, autoware_msgs::Lane& lane);
+
+  const std::vector<double> calcCurveParam(CbufGPoint point) const;
+  const double calcPathLength(const autoware_msgs::Lane& lane) const;
+};
+}
+#endif

src/ros/catkin/src/waypoint_maker/nodes/waypoint_replanner/waypoint_replanner_node.cpp

@@ -0,0 +1,133 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include <ros/ros.h>
+#include <autoware_msgs/LaneArray.h>
+#include "waypoint_replanner.h"
+
+namespace waypoint_maker
+{
+class WaypointReplannerNode
+{
+public:
+  WaypointReplannerNode();
+private:
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
+  ros::Publisher lane_pub_;
+  ros::Subscriber lane_sub_, config_sub_;
+  bool replanning_mode_, realtime_tuning_mode_;
+  bool is_first_publish_;
+  WaypointReplanner replanner_;
+  autoware_msgs::LaneArray lane_array_;
+  void replan(autoware_msgs::LaneArray &lane_array);
+  void publishLaneArray();
+  void laneCallback(const autoware_msgs::LaneArray::ConstPtr& lane_array);
+  void configCallback(const autoware_config_msgs::ConfigWaypointReplanner::ConstPtr& conf);
+  autoware_config_msgs::ConfigWaypointReplanner startup_config;
+  bool use_decision_maker_;
+};
+
+WaypointReplannerNode::WaypointReplannerNode() : pnh_("~"), is_first_publish_(true)
+{
+  WaypointReplannerConfig temp_config;
+
+  double velocity_max_kph, velocity_min_kph;
+
+  pnh_.param<bool>("replanning_mode", replanning_mode_, false);
+  pnh_.param<bool>("realtime_tuning_mode", realtime_tuning_mode_, true);
+  pnh_.param<double>("velocity_max", velocity_max_kph, 0.0);
+  pnh_.param<double>("velocity_min", velocity_min_kph, 0.0);
+  pnh_.param<double>("accel_limit", temp_config.accel_limit, 0.0);
+  pnh_.param<double>("decel_limit", temp_config.decel_limit, 0.0);
+  pnh_.param<double>("radius_thresh", temp_config.radius_thresh, 0.0);
+  pnh_.param<double>("radius_min", temp_config.radius_min, 0.0);
+  pnh_.param<bool>("resample_mode", temp_config.resample_mode, false);
+  pnh_.param<double>("resample_interval", temp_config.resample_interval, 0.0);
+  pnh_.param<bool>("replan_curve_mode", temp_config.replan_curve_mode, false);
+  pnh_.param<bool>("replan_endpoint_mode", temp_config.replan_endpoint_mode, false);
+  pnh_.param<bool>("overwrite_vmax_mode", temp_config.overwrite_vmax_mode, false);
+  pnh_.param<double>("velocity_offset", temp_config.velocity_offset, 0.0);
+  pnh_.param<double>("end_point_offset", temp_config.end_point_offset, 0.0);
+  pnh_.param<double>("braking_distance", temp_config.braking_distance, 0.0);
+  pnh_.param<bool>("use_decision_maker", use_decision_maker_, false);
+
+  temp_config.velocity_max = kmph2mps(velocity_max_kph);
+  temp_config.velocity_min = kmph2mps(velocity_min_kph);
+
+  replanner_.updateConfig(temp_config);
+
+  if (use_decision_maker_)
+  {
+    lane_pub_ = nh_.advertise<autoware_msgs::LaneArray>("/based/lane_waypoints_array", 10, true);
+  }
+  else
+  {
+    lane_pub_ = nh_.advertise<autoware_msgs::LaneArray>("/lane_waypoints_array", 10, true);
+  }
+
+  lane_sub_ = nh_.subscribe("/based/lane_waypoints_raw", 1, &WaypointReplannerNode::laneCallback, this);
+  config_sub_ = nh_.subscribe("/config/waypoint_replanner", 1, &WaypointReplannerNode::configCallback, this);
+}
+
+void WaypointReplannerNode::replan(autoware_msgs::LaneArray& lane_array)
+{
+  for (auto &el : lane_array.lanes)
+  {
+    replanner_.replanLaneWaypointVel(el);
+  }
+}
+
+void WaypointReplannerNode::publishLaneArray()
+{
+  autoware_msgs::LaneArray array(lane_array_);
+
+  if (replanning_mode_)
+  {
+    replan(array);
+  }
+
+  lane_pub_.publish(array);
+  is_first_publish_ = false;
+}
+
+void WaypointReplannerNode::laneCallback(const autoware_msgs::LaneArray::ConstPtr& lane_array)
+{
+  lane_array_ = *lane_array;
+  publishLaneArray();
+}
+
+void WaypointReplannerNode::configCallback(const autoware_config_msgs::ConfigWaypointReplanner::ConstPtr& conf)
+{
+  replanning_mode_ = conf->replanning_mode;
+  realtime_tuning_mode_ = conf->realtime_tuning_mode;
+  use_decision_maker_ = conf->use_decision_maker;
+  replanner_.initParameter(conf);
+  if (!lane_array_.lanes.empty() && (is_first_publish_ || realtime_tuning_mode_))
+  {
+    publishLaneArray();
+  }
+}
+
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "waypoint_replanner");
+  waypoint_maker::WaypointReplannerNode wr;
+  ros::spin();
+
+  return 0;
+}

src/ros/catkin/src/waypoint_maker/nodes/waypoint_saver/waypoint_saver.cpp

@@ -0,0 +1,194 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <std_msgs/Float32.h>
+#include <tf/transform_datatypes.h>
+
+#include <fstream>
+
+#include "libwaypoint_follower/libwaypoint_follower.h"
+
+static const int SYNC_FRAMES = 50;
+
+typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::TwistStamped, geometry_msgs::PoseStamped>
+    TwistPoseSync;
+
+class WaypointSaver
+{
+public:
+  WaypointSaver();
+  ~WaypointSaver();
+
+private:
+  // functions
+
+  void TwistPoseCallback(const geometry_msgs::TwistStampedConstPtr &twist_msg,
+                         const geometry_msgs::PoseStampedConstPtr &pose_msg) const;
+  void poseCallback(const geometry_msgs::PoseStampedConstPtr &pose_msg) const;
+  void displayMarker(geometry_msgs::Pose pose, double velocity) const;
+  void outputProcessing(geometry_msgs::Pose current_pose, double velocity) const;
+
+  // handle
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+
+  // publisher
+  ros::Publisher waypoint_saver_pub_;
+
+  // subscriber
+  message_filters::Subscriber<geometry_msgs::TwistStamped> *twist_sub_;
+  message_filters::Subscriber<geometry_msgs::PoseStamped> *pose_sub_;
+  message_filters::Synchronizer<TwistPoseSync> *sync_tp_;
+
+  // variables
+  bool save_velocity_;
+  double interval_;
+  std::string filename_, pose_topic_, velocity_topic_;
+};
+
+WaypointSaver::WaypointSaver() : private_nh_("~")
+{
+  // parameter settings
+  private_nh_.param<std::string>("save_filename", filename_, std::string("data.txt"));
+  private_nh_.param<std::string>("pose_topic", pose_topic_, std::string("current_pose"));
+  private_nh_.param<std::string>("velocity_topic", velocity_topic_, std::string("current_velocity"));
+  private_nh_.param<double>("interval", interval_, 1.0);
+  private_nh_.param<bool>("save_velocity", save_velocity_, false);
+
+  // subscriber
+  pose_sub_ = new message_filters::Subscriber<geometry_msgs::PoseStamped>(nh_, pose_topic_, 50);
+
+  if (save_velocity_)
+  {
+    twist_sub_ = new message_filters::Subscriber<geometry_msgs::TwistStamped>(nh_, velocity_topic_, 50);
+    sync_tp_ = new message_filters::Synchronizer<TwistPoseSync>(TwistPoseSync(SYNC_FRAMES), *twist_sub_, *pose_sub_);
+    sync_tp_->registerCallback(boost::bind(&WaypointSaver::TwistPoseCallback, this, _1, _2));
+  }
+  else
+  {
+    pose_sub_->registerCallback(boost::bind(&WaypointSaver::poseCallback, this, _1));
+  }
+
+  // publisher
+  waypoint_saver_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("waypoint_saver_marker", 10, true);
+}
+
+WaypointSaver::~WaypointSaver()
+{
+  delete twist_sub_;
+  delete pose_sub_;
+  delete sync_tp_;
+}
+
+void WaypointSaver::poseCallback(const geometry_msgs::PoseStampedConstPtr &pose_msg) const
+{
+  outputProcessing(pose_msg->pose, 0);
+}
+
+void WaypointSaver::TwistPoseCallback(const geometry_msgs::TwistStampedConstPtr &twist_msg,
+                                      const geometry_msgs::PoseStampedConstPtr &pose_msg) const
+{
+  outputProcessing(pose_msg->pose, mps2kmph(twist_msg->twist.linear.x));
+}
+
+void WaypointSaver::outputProcessing(geometry_msgs::Pose current_pose, double velocity) const
+{
+  std::ofstream ofs(filename_.c_str(), std::ios::app);
+  static geometry_msgs::Pose previous_pose;
+  static bool receive_once = false;
+  // first subscribe
+  if (!receive_once)
+  {
+    ofs << "x,y,z,yaw,velocity,change_flag" << std::endl;
+    ofs << std::fixed << std::setprecision(4) << current_pose.position.x << "," << current_pose.position.y << ","
+        << current_pose.position.z << "," << tf::getYaw(current_pose.orientation) << ",0,0" << std::endl;
+    receive_once = true;
+    displayMarker(current_pose, 0);
+    previous_pose = current_pose;
+  }
+  else
+  {
+    double distance = sqrt(pow((current_pose.position.x - previous_pose.position.x), 2) +
+                           pow((current_pose.position.y - previous_pose.position.y), 2));
+
+    // if car moves [interval] meter
+    if (distance > interval_)
+    {
+      ofs << std::fixed << std::setprecision(4) << current_pose.position.x << "," << current_pose.position.y << ","
+          << current_pose.position.z << "," << tf::getYaw(current_pose.orientation) << "," << velocity << ",0" << std::endl;
+
+      displayMarker(current_pose, velocity);
+      previous_pose = current_pose;
+    }
+  }
+}
+
+void WaypointSaver::displayMarker(geometry_msgs::Pose pose, double velocity) const
+{
+  static visualization_msgs::MarkerArray marray;
+  static int id = 0;
+
+  // initialize marker
+  visualization_msgs::Marker marker;
+  marker.id = id;
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time();
+  marker.frame_locked = true;
+
+  // create saved waypoint marker
+  marker.scale.x = 0.5;
+  marker.scale.y = 0.1;
+  marker.scale.z = 0.1;
+  marker.color.a = 1.0;
+  marker.color.r = 0.0;
+  marker.color.g = 1.0;
+  marker.color.b = 0.0;
+  marker.ns = "saved_waypoint_arrow";
+  marker.type = visualization_msgs::Marker::ARROW;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.pose = pose;
+  marray.markers.push_back(marker);
+
+  // create saved waypoint velocity text
+  marker.scale.z = 0.4;
+  marker.color.a = 1.0;
+  marker.color.r = 1.0;
+  marker.ns = "saved_waypoint_velocity";
+  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+  marker.action = visualization_msgs::Marker::ADD;
+  std::ostringstream oss;
+  oss << std::fixed << std::setprecision(2) << velocity << " km/h";
+  marker.text = oss.str();
+  marray.markers.push_back(marker);
+
+  waypoint_saver_pub_.publish(marray);
+  id++;
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "waypoint_saver");
+  WaypointSaver ws;
+  ros::spin();
+  return 0;
+}

src/ros/catkin/src/waypoint_maker/nodes/waypoint_velocity_visualizer/waypoint_velocity_visualizer.cpp

@@ -0,0 +1,391 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <ros/ros.h>
+#include <ros/console.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/ColorRGBA.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/transform_datatypes.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <iostream>
+#include <vector>
+#include <string>
+
+#include <boost/circular_buffer.hpp>
+
+#include "libwaypoint_follower/libwaypoint_follower.h"
+#include "autoware_msgs/LaneArray.h"
+#include "autoware_config_msgs/ConfigLaneStop.h"
+#include "autoware_msgs/TrafficLight.h"
+
+class WaypointVelocityVisualizer
+{
+public:
+  WaypointVelocityVisualizer();
+  ~WaypointVelocityVisualizer();
+
+private:
+  typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::PoseStamped, geometry_msgs::TwistStamped,
+                                                          geometry_msgs::TwistStamped>
+      ControlSyncPolicy;
+
+  ros::NodeHandle node_handle_;
+  ros::NodeHandle private_node_handle_;
+
+  ros::Subscriber lane_waypoints_array_sub_;
+  ros::Subscriber final_waypoints_sub_;
+  message_filters::Subscriber<geometry_msgs::PoseStamped>* current_pose_sub_;
+  message_filters::Subscriber<geometry_msgs::TwistStamped>* current_twist_sub_;
+  message_filters::Subscriber<geometry_msgs::TwistStamped>* command_twist_sub_;
+  message_filters::Synchronizer<ControlSyncPolicy>* control_sync_;
+
+  ros::Publisher velocity_marker_pub_;
+
+  visualization_msgs::MarkerArray velocity_marker_array_;
+  visualization_msgs::MarkerArray lane_waypoints_array_marker_array_;
+  visualization_msgs::MarkerArray final_waypoints_marker_array_;
+  visualization_msgs::MarkerArray current_twist_marker_array_;
+  visualization_msgs::MarkerArray command_twist_marker_array_;
+
+  bool use_bar_plot_ = false;
+  bool use_line_plot_ = true;
+  bool use_text_plot_ = true;
+  int control_buffer_size_ = 100;
+  double plot_height_ratio_ = 1.0;
+  double plot_height_shift_ = 0.2;
+  double plot_metric_interval_ = 1.0;
+  std::vector<double> lane_waypoints_array_rgba_ = { 1.0, 1.0, 1.0, 0.5 };
+  std::vector<double> final_waypoints_rgba_ = { 0.0, 1.0, 0.0, 0.5 };
+  std::vector<double> current_twist_rgba_ = { 0.0, 0.0, 1.0, 0.5 };
+  std::vector<double> command_twist_rgba_ = { 1.0, 0.0, 0.0, 0.5 };
+
+  std_msgs::ColorRGBA lane_waypoints_array_color_;
+  std_msgs::ColorRGBA final_waypoints_color_;
+  std_msgs::ColorRGBA current_twist_color_;
+  std_msgs::ColorRGBA command_twist_color_;
+
+  ros::Time previous_time_;
+  boost::circular_buffer<geometry_msgs::PoseStamped> current_pose_buf_;
+  boost::circular_buffer<geometry_msgs::TwistStamped> current_twist_buf_;
+  boost::circular_buffer<geometry_msgs::TwistStamped> command_twist_buf_;
+
+  std_msgs::ColorRGBA vector2color(const std::vector<double>& v);
+  void deleteMarkers();
+  void resetBuffers();
+
+  void laneWaypointsArrayCallback(const autoware_msgs::LaneArray::ConstPtr& msg);
+  void finalWaypointsCallback(const autoware_msgs::Lane::ConstPtr& msg);
+  void controlCallback(const geometry_msgs::PoseStamped::ConstPtr& current_pose_msg,
+                       const geometry_msgs::TwistStamped::ConstPtr& current_twist_msg,
+                       const geometry_msgs::TwistStamped::ConstPtr& command_twist_msg);
+
+  void publishVelocityMarker();
+
+  void createVelocityMarker(const std::vector<nav_msgs::Odometry> waypoints, const std::string& ns,
+                            const std_msgs::ColorRGBA& color, visualization_msgs::MarkerArray& markers);
+  void createVelocityMarker(const autoware_msgs::Lane& lane, const std::string& ns, const std_msgs::ColorRGBA& color,
+                            visualization_msgs::MarkerArray& markers);
+  void createVelocityMarker(const boost::circular_buffer<geometry_msgs::PoseStamped>& poses,
+                            const boost::circular_buffer<geometry_msgs::TwistStamped>& twists, const std::string& ns,
+                            const std_msgs::ColorRGBA& color, visualization_msgs::MarkerArray& markers);
+
+  void createVelocityBarMarker(const std::vector<nav_msgs::Odometry>& waypoints, const std::string& ns,
+                               const std_msgs::ColorRGBA& color, visualization_msgs::MarkerArray& markers);
+  void createVelocityLineMarker(const std::vector<nav_msgs::Odometry>& waypoints, const std::string& ns,
+                                const std_msgs::ColorRGBA& color, visualization_msgs::MarkerArray& markers);
+  void createVelocityTextMarker(const std::vector<nav_msgs::Odometry>& waypoints, const std::string& ns,
+                                const std_msgs::ColorRGBA& color, visualization_msgs::MarkerArray& markers);
+};
+
+WaypointVelocityVisualizer::WaypointVelocityVisualizer() : node_handle_(), private_node_handle_("~")
+{
+  private_node_handle_.param<bool>("use_bar_plot", use_bar_plot_, use_bar_plot_);
+  private_node_handle_.param<bool>("use_line_plot", use_line_plot_, use_line_plot_);
+  private_node_handle_.param<bool>("use_text_plot", use_text_plot_, use_text_plot_);
+
+  private_node_handle_.param<int>("control_buffer_size", control_buffer_size_, control_buffer_size_);
+  private_node_handle_.param<double>("plot_height_ratio", plot_height_ratio_, plot_height_ratio_);
+  private_node_handle_.param<double>("plot_height_shift", plot_height_shift_, plot_height_shift_);
+  private_node_handle_.param<double>("plot_metric_interval", plot_metric_interval_, plot_metric_interval_);
+
+  private_node_handle_.param<std::vector<double> >("lane_waypoints_array_rgba", lane_waypoints_array_rgba_, lane_waypoints_array_rgba_);
+  private_node_handle_.param<std::vector<double> >("final_waypoints_rgba", final_waypoints_rgba_,
+                                                   final_waypoints_rgba_);
+  private_node_handle_.param<std::vector<double> >("current_twist_rgba", current_twist_rgba_, current_twist_rgba_);
+  private_node_handle_.param<std::vector<double> >("command_twist_rgba", command_twist_rgba_, command_twist_rgba_);
+
+  lane_waypoints_array_color_ = vector2color(lane_waypoints_array_rgba_);
+  final_waypoints_color_ = vector2color(final_waypoints_rgba_);
+  current_twist_color_ = vector2color(current_twist_rgba_);
+  command_twist_color_ = vector2color(command_twist_rgba_);
+
+  previous_time_ = ros::Time::now();
+
+  current_pose_buf_.set_capacity(control_buffer_size_);
+  current_twist_buf_.set_capacity(control_buffer_size_);
+  command_twist_buf_.set_capacity(control_buffer_size_);
+
+  lane_waypoints_array_sub_ =
+      node_handle_.subscribe("lane_waypoints_array", 1, &WaypointVelocityVisualizer::laneWaypointsArrayCallback, this);
+  final_waypoints_sub_ =
+      node_handle_.subscribe("final_waypoints", 1, &WaypointVelocityVisualizer::finalWaypointsCallback, this);
+
+  current_pose_sub_ = new message_filters::Subscriber<geometry_msgs::PoseStamped>(node_handle_, "current_pose", 1);
+  current_twist_sub_ =
+      new message_filters::Subscriber<geometry_msgs::TwistStamped>(node_handle_, "current_velocity", 1);
+  command_twist_sub_ = new message_filters::Subscriber<geometry_msgs::TwistStamped>(node_handle_, "twist_cmd", 1);
+  control_sync_ = new message_filters::Synchronizer<ControlSyncPolicy>(ControlSyncPolicy(10), *current_pose_sub_,
+                                                                       *current_twist_sub_, *command_twist_sub_);
+  control_sync_->registerCallback(boost::bind(&WaypointVelocityVisualizer::controlCallback, this, _1, _2, _3));
+
+  velocity_marker_pub_ = node_handle_.advertise<visualization_msgs::MarkerArray>("waypoints_velocity", 10);
+}
+
+WaypointVelocityVisualizer::~WaypointVelocityVisualizer()
+{
+}
+
+std_msgs::ColorRGBA WaypointVelocityVisualizer::vector2color(const std::vector<double>& v)
+{
+  std_msgs::ColorRGBA c;
+  c.r = v[0];
+  c.g = v[1];
+  c.b = v[2];
+  c.a = v[3];
+  return c;
+}
+
+void WaypointVelocityVisualizer::deleteMarkers()
+{
+  velocity_marker_array_.markers.clear();
+  visualization_msgs::Marker marker;
+  marker.action = visualization_msgs::Marker::DELETEALL;
+  velocity_marker_array_.markers.push_back(marker);
+  velocity_marker_pub_.publish(velocity_marker_array_);
+}
+
+void WaypointVelocityVisualizer::resetBuffers()
+{
+  current_pose_buf_.clear();
+  current_twist_buf_.clear();
+  command_twist_buf_.clear();
+}
+
+void WaypointVelocityVisualizer::laneWaypointsArrayCallback(const autoware_msgs::LaneArray::ConstPtr& msg)
+{
+  lane_waypoints_array_marker_array_.markers.clear();
+  for (size_t i = 0; i < msg->lanes.size(); ++i)
+  {
+    std::string ns = "lane_waypoints_" + std::to_string(i);
+    createVelocityMarker(msg->lanes[i], ns, lane_waypoints_array_color_, lane_waypoints_array_marker_array_);
+  }
+  publishVelocityMarker();
+}
+
+void WaypointVelocityVisualizer::finalWaypointsCallback(const autoware_msgs::Lane::ConstPtr& msg)
+{
+  final_waypoints_marker_array_.markers.clear();
+  createVelocityMarker(*msg, "final_waypoints", final_waypoints_color_, final_waypoints_marker_array_);
+  publishVelocityMarker();
+}
+
+void WaypointVelocityVisualizer::controlCallback(const geometry_msgs::PoseStamped::ConstPtr& current_pose_msg,
+                                                 const geometry_msgs::TwistStamped::ConstPtr& current_twist_msg,
+                                                 const geometry_msgs::TwistStamped::ConstPtr& command_twist_msg)
+{
+  // buffers are reset when time goes back, e.g. playback rosbag
+  ros::Time current_time = ros::Time::now();
+  if (previous_time_ > current_time)
+  {
+    ROS_WARN("Detected jump back in time of %.3fs. Clearing markers and buffers.",
+             (previous_time_ - current_time).toSec());
+    deleteMarkers();  // call 'DELETEALL'
+    resetBuffers();   // clear circular buffers
+  }
+  previous_time_ = current_time;
+  // if plot_metric_interval <= 0, velocity is plotted by each callback.
+  if (plot_metric_interval_ > 0 && current_pose_buf_.size() > 0)
+  {
+    tf::Vector3 p1, p2;
+    tf::pointMsgToTF(current_pose_buf_.back().pose.position, p1);
+    tf::pointMsgToTF(current_pose_msg->pose.position, p2);
+    if (!(p1.distance(p2) > plot_metric_interval_))
+      return;  // skipping plot
+  }
+  current_pose_buf_.push_back(*current_pose_msg);
+  current_twist_buf_.push_back(*current_twist_msg);
+  command_twist_buf_.push_back(*command_twist_msg);
+  current_twist_marker_array_.markers.clear();
+  command_twist_marker_array_.markers.clear();
+  createVelocityMarker(current_pose_buf_, current_twist_buf_, "current_velocity", current_twist_color_,
+                       current_twist_marker_array_);
+  createVelocityMarker(current_pose_buf_, command_twist_buf_, "twist_cmd", command_twist_color_,
+                       command_twist_marker_array_);
+  publishVelocityMarker();
+}
+
+void WaypointVelocityVisualizer::publishVelocityMarker()
+{
+  velocity_marker_array_.markers.clear();
+  velocity_marker_array_.markers.insert(velocity_marker_array_.markers.end(),
+                                        lane_waypoints_array_marker_array_.markers.begin(),
+                                        lane_waypoints_array_marker_array_.markers.end());
+  velocity_marker_array_.markers.insert(velocity_marker_array_.markers.end(),
+                                        final_waypoints_marker_array_.markers.begin(),
+                                        final_waypoints_marker_array_.markers.end());
+  velocity_marker_array_.markers.insert(velocity_marker_array_.markers.end(),
+                                        current_twist_marker_array_.markers.begin(),
+                                        current_twist_marker_array_.markers.end());
+  velocity_marker_array_.markers.insert(velocity_marker_array_.markers.end(),
+                                        command_twist_marker_array_.markers.begin(),
+                                        command_twist_marker_array_.markers.end());
+  velocity_marker_pub_.publish(velocity_marker_array_);
+}
+
+void WaypointVelocityVisualizer::createVelocityMarker(const std::vector<nav_msgs::Odometry> waypoints,
+                                                      const std::string& ns, const std_msgs::ColorRGBA& color,
+                                                      visualization_msgs::MarkerArray& markers)
+{
+  if (use_bar_plot_)
+    createVelocityBarMarker(waypoints, ns, color, markers);
+  if (use_line_plot_)
+    createVelocityLineMarker(waypoints, ns, color, markers);
+  if (use_text_plot_)
+    createVelocityTextMarker(waypoints, ns, color, markers);
+}
+
+void WaypointVelocityVisualizer::createVelocityMarker(const autoware_msgs::Lane& lane, const std::string& ns,
+                                                      const std_msgs::ColorRGBA& color,
+                                                      visualization_msgs::MarkerArray& markers)
+{
+  std::vector<nav_msgs::Odometry> waypoints;
+  for (const auto& wp : lane.waypoints)
+  {
+    nav_msgs::Odometry odom;
+    odom.pose.pose = wp.pose.pose;
+    odom.twist.twist = wp.twist.twist;
+    waypoints.push_back(odom);
+  }
+  createVelocityMarker(waypoints, ns, color, markers);
+}
+
+void WaypointVelocityVisualizer::createVelocityMarker(const boost::circular_buffer<geometry_msgs::PoseStamped>& poses,
+                                                      const boost::circular_buffer<geometry_msgs::TwistStamped>& twists,
+                                                      const std::string& ns, const std_msgs::ColorRGBA& color,
+                                                      visualization_msgs::MarkerArray& markers)
+{
+  assert(poses.size() == twists.size());
+  std::vector<nav_msgs::Odometry> waypoints;
+  for (unsigned int i = 0; i < poses.size(); ++i)
+  {
+    nav_msgs::Odometry odom;
+    odom.pose.pose = poses[i].pose;
+    odom.twist.twist = twists[i].twist;
+    waypoints.push_back(odom);
+  }
+  createVelocityMarker(waypoints, ns, color, markers);
+}
+
+void WaypointVelocityVisualizer::createVelocityBarMarker(const std::vector<nav_msgs::Odometry>& waypoints,
+                                                         const std::string& ns, const std_msgs::ColorRGBA& color,
+                                                         visualization_msgs::MarkerArray& markers)
+{
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time();
+  marker.ns = ns + "/bar";
+  marker.type = visualization_msgs::Marker::CYLINDER;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.scale.x = 0.1;
+  marker.scale.y = 0.1;
+  marker.color = color;
+
+  unsigned int count = 0;
+  for (const auto& wp : waypoints)
+  {
+    double h = plot_height_ratio_ * wp.twist.twist.linear.x;
+    marker.id = count++;
+    marker.pose = wp.pose.pose;
+    marker.pose.position.z += h / 2.0;
+    // When the the cylinder height is 0 or less, a warning occurs in RViz.
+    marker.scale.z = fabs(h) + 1e-6;
+    markers.markers.push_back(marker);
+  }
+}
+
+void WaypointVelocityVisualizer::createVelocityLineMarker(const std::vector<nav_msgs::Odometry>& waypoints,
+                                                          const std::string& ns, const std_msgs::ColorRGBA& color,
+                                                          visualization_msgs::MarkerArray& markers)
+{
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time();
+  marker.ns = ns + "/line";
+  marker.type = visualization_msgs::Marker::LINE_STRIP;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.scale.x = 0.1;
+  marker.color = color;
+
+  for (const auto& wp : waypoints)
+  {
+    geometry_msgs::Point p = wp.pose.pose.position;
+    p.z += plot_height_ratio_ * wp.twist.twist.linear.x;
+    marker.points.push_back(p);
+  }
+  markers.markers.push_back(marker);
+}
+
+void WaypointVelocityVisualizer::createVelocityTextMarker(const std::vector<nav_msgs::Odometry>& waypoints,
+                                                          const std::string& ns, const std_msgs::ColorRGBA& color,
+                                                          visualization_msgs::MarkerArray& markers)
+{
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time();
+  marker.ns = ns + "/text";
+  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.scale.z = 0.2;
+  marker.color = color;
+
+  unsigned int count = 0;
+  for (const auto& wp : waypoints)
+  {
+    marker.id = count++;
+    geometry_msgs::Point p = wp.pose.pose.position;
+    p.z += plot_height_ratio_ * wp.twist.twist.linear.x + plot_height_shift_;
+    marker.pose.position = p;
+    std::ostringstream oss;
+    oss << std::fixed << std::setprecision(1) << mps2kmph(wp.twist.twist.linear.x);
+    marker.text = oss.str();
+    markers.markers.push_back(marker);
+  }
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "waypoint_velocity_visualizer");
+  WaypointVelocityVisualizer node;
+  ros::spin();
+  return 0;
+}

src/ros/catkin/src/waypoint_maker/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>waypoint_maker</name>
+  <version>1.12.0</version>
+  <description>The waypoint_maker package</description>
+  <maintainer email="h_ohta@ertl.jp">h_ohta</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>amathutils_lib</depend>
+  <depend>autoware_config_msgs</depend>
+  <depend>autoware_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>gnss</depend>
+  <depend>lane_planner</depend>
+  <depend>libwaypoint_follower</depend>
+  <depend>nav_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>tablet_socket_msgs</depend>
+  <depend>tf</depend>
+  <depend>vector_map</depend>
+</package>