add some ros module from autoware vectormap.

src/ros/catkin/src/gnss/CHANGELOG.rst

@@ -0,0 +1,199 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package gnss
+^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* add constructor (`#1913 <https://github.com/CPFL/Autoware/issues/1913>`_)
+* Fix license notice in corresponding package.xml
+* Contributors: YamatoAndo, amc-nu
+
+1.10.0 (2019-01-17)
+-------------------
+* 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
+* Contributors: Esteve Fernandez
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+
+1.8.0 (2018-08-31)
+------------------
+
+1.7.0 (2018-05-18)
+------------------
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* [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
+* Contributors: Abraham Monrroy, Kosuke Murakami
+
+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
+* support all plane(1-19) in geo_pos_conv
+* Contributors: Yamato ANDO, yukikitsukawa
+
+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
+* install target
+* Contributors: Dejan Pangercic, 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)
+------------------
+
+1.2.0 (2017-06-07)
+------------------
+
+1.1.2 (2017-02-27 23:10)
+------------------------
+
+1.1.1 (2017-02-27 22:25)
+------------------------
+
+1.1.0 (2017-02-24)
+------------------
+
+1.0.1 (2017-01-14)
+------------------
+
+1.0.0 (2016-12-22)
+------------------
+* Initial commit for public release
+* Contributors: Shinpei Kato

src/ros/catkin/src/gnss/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(gnss)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    roscpp
+    roslint
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES gnss
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(gnss
+  src/geo_pos_conv.cpp
+)
+
+target_link_libraries(gnss
+  ${catkin_LIBRARIES}
+)
+
+file(GLOB_RECURSE ROSLINT_FILES
+  LIST_DIRECTORIES false
+  *.cpp *.h *.hpp
+)
+
+list(APPEND ROSLINT_CPP_OPTS
+  "--extensions=cc,h,hpp,cpp,cu,cuh"
+  "--filter=-build/c++14"
+)
+
+roslint_cpp(${ROSLINT_FILES})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.hpp"
+)
+
+install(TARGETS gnss
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+if(CATKIN_ENABLE_TESTING)
+  roslint_add_test()
+
+  find_package(rostest REQUIRED)
+  find_package(rosunit REQUIRED)
+
+  add_rostest_gtest(test_gnss
+    test/test_gnss.test test/test_gnss.cpp
+    src/geo_pos_conv.cpp
+  )
+  target_link_libraries(test_gnss ${catkin_LIBRARIES})
+  add_dependencies(test_gnss ${catkin_EXPORTED_TARGETS})
+endif()

src/ros/catkin/src/gnss/include/gnss/geo_pos_conv.hpp

@@ -0,0 +1,56 @@
+/*
+ * 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 GNSS_GEO_POS_CONV_HPP
+#define GNSS_GEO_POS_CONV_HPP
+
+class geo_pos_conv
+{
+private:
+  double m_x;  // m
+  double m_y;  // m
+  double m_z;  // m
+
+  double m_lat;  // latitude
+  double m_lon;  // longitude
+  double m_h;
+
+  double m_PLato;  // plane lat
+  double m_PLo;    // plane lon
+
+public:
+  geo_pos_conv();
+  double x() const;
+  double y() const;
+  double z() const;
+
+  void set_plane(double lat, double lon);
+  void set_plane(int num);
+  void set_xyz(double cx, double cy, double cz);
+
+  // set llh in radians
+  void set_llh(double lat, double lon, double h);
+
+  // set llh in nmea degrees
+  void set_llh_nmea_degrees(double latd, double lond, double h);
+
+  void llh_to_xyz(double lat, double lon, double ele);
+
+  void conv_llh2xyz(void);
+  void conv_xyz2llh(void);
+};
+
+#endif  // GNSS_GEO_POSE_CONV_HPP

src/ros/catkin/src/gnss/package.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>gnss</name>
+  <version>1.12.0</version>
+  <description>The gnss package</description>
+  <maintainer email="tomo@axe.bz">Tomomi HORII</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roscpp</depend>
+  <depend>roslint</depend>
+</package>

src/ros/catkin/src/gnss/src/geo_pos_conv.cpp

@@ -0,0 +1,356 @@
+/*
+ * 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 <gnss/geo_pos_conv.hpp>
+
+#include <cmath>
+
+geo_pos_conv::geo_pos_conv()
+    : m_x(0)
+    , m_y(0)
+    , m_z(0)
+    , m_lat(0)
+    , m_lon(0)
+    , m_h(0)
+    , m_PLato(0)
+    , m_PLo(0)
+{
+}
+
+double geo_pos_conv::x() const
+{
+  return m_x;
+}
+
+double geo_pos_conv::y() const
+{
+  return m_y;
+}
+
+double geo_pos_conv::z() const
+{
+  return m_z;
+}
+
+void geo_pos_conv::set_plane(double lat, double lon)
+{
+  m_PLato = lat;
+  m_PLo = lon;
+}
+
+void geo_pos_conv::set_plane(int num)
+{
+  int lon_deg, lon_min, lat_deg, lat_min;  // longitude and latitude of origin of each plane in Japan
+  if (num == 0)
+  {
+    lon_deg = 0;
+    lon_min = 0;
+    lat_deg = 0;
+    lat_min = 0;
+  }
+  else if (num == 1)
+  {
+    lon_deg = 33;
+    lon_min = 0;
+    lat_deg = 129;
+    lat_min = 30;
+  }
+  else if (num == 2)
+  {
+    lon_deg = 33;
+    lon_min = 0;
+    lat_deg = 131;
+    lat_min = 0;
+  }
+  else if (num == 3)
+  {
+    lon_deg = 36;
+    lon_min = 0;
+    lat_deg = 132;
+    lat_min = 10;
+  }
+  else if (num == 4)
+  {
+    lon_deg = 33;
+    lon_min = 0;
+    lat_deg = 133;
+    lat_min = 30;
+  }
+  else if (num == 5)
+  {
+    lon_deg = 36;
+    lon_min = 0;
+    lat_deg = 134;
+    lat_min = 20;
+  }
+  else if (num == 6)
+  {
+    lon_deg = 36;
+    lon_min = 0;
+    lat_deg = 136;
+    lat_min = 0;
+  }
+  else if (num == 7)
+  {
+    lon_deg = 36;
+    lon_min = 0;
+    lat_deg = 137;
+    lat_min = 10;
+  }
+  else if (num == 8)
+  {
+    lon_deg = 36;
+    lon_min = 0;
+    lat_deg = 138;
+    lat_min = 30;
+  }
+  else if (num == 9)
+  {
+    lon_deg = 36;
+    lon_min = 0;
+    lat_deg = 139;
+    lat_min = 50;
+  }
+  else if (num == 10)
+  {
+    lon_deg = 40;
+    lon_min = 0;
+    lat_deg = 140;
+    lat_min = 50;
+  }
+  else if (num == 11)
+  {
+    lon_deg = 44;
+    lon_min = 0;
+    lat_deg = 140;
+    lat_min = 15;
+  }
+  else if (num == 12)
+  {
+    lon_deg = 44;
+    lon_min = 0;
+    lat_deg = 142;
+    lat_min = 15;
+  }
+  else if (num == 13)
+  {
+    lon_deg = 44;
+    lon_min = 0;
+    lat_deg = 144;
+    lat_min = 15;
+  }
+  else if (num == 14)
+  {
+    lon_deg = 26;
+    lon_min = 0;
+    lat_deg = 142;
+    lat_min = 0;
+  }
+  else if (num == 15)
+  {
+    lon_deg = 26;
+    lon_min = 0;
+    lat_deg = 127;
+    lat_min = 30;
+  }
+  else if (num == 16)
+  {
+    lon_deg = 26;
+    lon_min = 0;
+    lat_deg = 124;
+    lat_min = 0;
+  }
+  else if (num == 17)
+  {
+    lon_deg = 26;
+    lon_min = 0;
+    lat_deg = 131;
+    lat_min = 0;
+  }
+  else if (num == 18)
+  {
+    lon_deg = 20;
+    lon_min = 0;
+    lat_deg = 136;
+    lat_min = 0;
+  }
+  else if (num == 19)
+  {
+    lon_deg = 26;
+    lon_min = 0;
+    lat_deg = 154;
+    lat_min = 0;
+  }
+
+  // swap longitude and latitude
+  m_PLo = M_PI * (static_cast<double>(lat_deg) + static_cast<double>(lat_min) / 60.0) / 180.0;
+  m_PLato = M_PI * (static_cast<double>(lon_deg) + static_cast<double>(lon_min) / 60.0) / 180;
+}
+
+void geo_pos_conv::set_xyz(double cx, double cy, double cz)
+{
+  m_x = cx;
+  m_y = cy;
+  m_z = cz;
+  conv_xyz2llh();
+}
+
+void geo_pos_conv::set_llh_nmea_degrees(double latd, double lond, double h)
+{
+  double lat, lad, lod, lon;
+  // 1234.56 -> 12'34.56 -> 12+ 34.56/60
+
+  if (latd > 0)
+  {
+    lad = floor(latd / 100.);
+  }
+  else
+  {
+    lad = ceil(latd / 100.);
+  }
+  lat = latd - lad * 100.;
+
+  if (lond > 0)
+  {
+    lod = floor(lond / 100.);
+  }
+  else
+  {
+    lod = ceil(lond / 100.);
+  }
+  lon = lond - lod * 100.;
+
+  // Changing Longitude and Latitude to Radians
+  m_lat = (lad + lat / 60.0) * M_PI / 180;
+  m_lon = (lod + lon / 60.0) * M_PI / 180;
+  m_h = h;
+
+  conv_llh2xyz();
+}
+
+void geo_pos_conv::llh_to_xyz(double lat, double lon, double ele)
+{
+  m_lat = lat * M_PI / 180;
+  m_lon = lon * M_PI / 180;
+  m_h = ele;
+
+  conv_llh2xyz();
+}
+
+void geo_pos_conv::conv_llh2xyz(void)
+{
+  double PS;   //
+  double PSo;  //
+  double PDL;  //
+  double Pt;   //
+  double PN;   //
+  double PW;   //
+
+  double PB1, PB2, PB3, PB4, PB5, PB6, PB7, PB8, PB9;
+  double PA, PB, PC, PD, PE, PF, PG, PH, PI;
+  double Pe;   //
+  double Pet;  //
+  double Pnn;  //
+  double AW, FW, Pmo;
+
+  Pmo = 0.9999;
+
+  /*WGS84 Parameters*/
+  AW = 6378137.0;            // Semimajor Axis
+  FW = 1.0 / 298.257222101;  // 298.257223563 //Geometrical flattening
+
+  Pe = static_cast<double>(std::sqrt(2.0 * FW - std::pow(FW, 2)));
+  Pet = static_cast<double>(std::sqrt(std::pow(Pe, 2) / (1.0 - std::pow(Pe, 2))));
+
+  PA = static_cast<double>(1.0 + 3.0 / 4.0 * std::pow(Pe, 2) + 45.0 / 64.0 * std::pow(Pe, 4) +
+    175.0 / 256.0 * std::pow(Pe, 6) + 11025.0 / 16384.0 * std::pow(Pe, 8) + 43659.0 / 65536.0 *
+    std::pow(Pe, 10) + 693693.0 / 1048576.0 * std::pow(Pe, 12) + 19324305.0 / 29360128.0 * std::pow(Pe, 14) +
+    4927697775.0 / 7516192768.0 * std::pow(Pe, 16));
+
+  PB = static_cast<double>(3.0 / 4.0 * std::pow(Pe, 2) + 15.0 / 16.0 * std::pow(Pe, 4) + 525.0 / 512.0 *
+    std::pow(Pe, 6) + 2205.0 / 2048.0 * std::pow(Pe, 8) + 72765.0 / 65536.0 * std::pow(Pe, 10) + 297297.0 / 262144.0 *
+    std::pow(Pe, 12) + 135270135.0 / 117440512.0 * std::pow(Pe, 14) + 547521975.0 / 469762048.0 * std::pow(Pe, 16));
+
+  PC = static_cast<double>(15.0 / 64.0 * std::pow(Pe, 4) + 105.0 / 256.0 * std::pow(Pe, 6) + 2205.0 / 4096.0 *
+    std::pow(Pe, 8) + 10395.0 / 16384.0 * std::pow(Pe, 10) + 1486485.0 / 2097152.0 * std::pow(Pe, 12) +
+    45090045.0 / 58720256.0 * std::pow(Pe, 14) + 766530765.0 / 939524096.0 * std::pow(Pe, 16));
+
+  PD = static_cast<double>(35.0 / 512.0 * std::pow(Pe, 6) + 315.0 / 2048.0 * std::pow(Pe, 8) + 31185.0 / 131072.0 *
+    std::pow(Pe, 10) + 165165.0 / 524288.0 * std::pow(Pe, 12) + 45090045.0 / 117440512.0 * std::pow(Pe, 14) +
+    209053845.0 / 469762048.0 * std::pow(Pe, 16));
+
+  PE = static_cast<double>(315.0 / 16384.0 * std::pow(Pe, 8) + 3465.0 / 65536.0 * std::pow(Pe, 10) +
+    99099.0 / 1048576.0 * std::pow(Pe, 12) + 4099095.0 / 29360128.0 * std::pow(Pe, 14) + 348423075.0 / 1879048192.0 *
+    std::pow(Pe, 16));
+
+  PF = static_cast<double>(693.0 / 131072.0 * std::pow(Pe, 10) + 9009.0 / 524288.0 * std::pow(Pe, 12) +
+    4099095.0 / 117440512.0 * std::pow(Pe, 14) + 26801775.0 / 469762048.0 * std::pow(Pe, 16));
+
+  PG = static_cast<double>(3003.0 / 2097152.0 * std::pow(Pe, 12) + 315315.0 / 58720256.0 * std::pow(Pe, 14) +
+    11486475.0 / 939524096.0 * std::pow(Pe, 16));
+
+  PH = static_cast<double>(45045.0 / 117440512.0 * std::pow(Pe, 14) + 765765.0 / 469762048.0 * std::pow(Pe, 16));
+
+  PI = static_cast<double>(765765.0 / 7516192768.0 * std::pow(Pe, 16));
+
+  PB1 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PA;
+  PB2 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PB / -2.0;
+  PB3 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PC / 4.0;
+  PB4 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PD / -6.0;
+  PB5 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PE / 8.0;
+  PB6 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PF / -10.0;
+  PB7 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PG / 12.0;
+  PB8 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PH / -14.0;
+  PB9 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PI / 16.0;
+
+  PS = static_cast<double>(PB1) * m_lat + PB2 * std::sin(2.0 * m_lat) + PB3 * std::sin(4.0 * m_lat) + PB4 *
+    std::sin(6.0 * m_lat) + PB5 * std::sin(8.0 * m_lat) + PB6 * std::sin(10.0 * m_lat) + PB7 * std::sin(12.0 * m_lat) +
+    PB8 * std::sin(14.0 * m_lat) + PB9 * std::sin(16.0 * m_lat);
+
+  PSo = static_cast<double>(PB1) * m_PLato + PB2 * std::sin(2.0 * m_PLato) + PB3 * std::sin(4.0 * m_PLato) + PB4 *
+    std::sin(6.0 * m_PLato) + PB5 * std::sin(8.0 * m_PLato) + PB6 * std::sin(10.0 * m_PLato) + PB7 *
+    std::sin(12.0 * m_PLato) + PB8 * std::sin(14.0 * m_PLato) + PB9 * std::sin(16.0 * m_PLato);
+
+  PDL = static_cast<double>(m_lon) - m_PLo;
+  Pt = static_cast<double>(std::tan(m_lat));
+  PW = static_cast<double>(std::sqrt(1.0 - std::pow(Pe, 2) * std::pow(std::sin(m_lat), 2)));
+  PN = static_cast<double>(AW) / PW;
+  Pnn = static_cast<double>(std::sqrt(std::pow(Pet, 2) * std::pow(std::cos(m_lat), 2)));
+
+  m_x = static_cast<double>(((PS - PSo) + (1.0 / 2.0) * PN * std::pow(std::cos(m_lat), 2.0) * Pt * std::pow(PDL, 2.0) +
+    (1.0 / 24.0) * PN * std::pow(std::cos(m_lat), 4) * Pt *
+    (5.0 - std::pow(Pt, 2) + 9.0 * std::pow(Pnn, 2) + 4.0 * std::pow(Pnn, 4)) * std::pow(PDL, 4) -
+    (1.0 / 720.0) * PN * std::pow(std::cos(m_lat), 6) * Pt *
+    (-61.0 + 58.0 * std::pow(Pt, 2) - std::pow(Pt, 4) - 270.0 * std::pow(Pnn, 2) + 330.0 *
+      std::pow(Pt, 2) * std::pow(Pnn, 2)) *
+    std::pow(PDL, 6) - (1.0 / 40320.0) * PN * std::pow(std::cos(m_lat), 8) * Pt *
+    (-1385.0 + 3111 * std::pow(Pt, 2) - 543 * std::pow(Pt, 4) + std::pow(Pt, 6)) * std::pow(PDL, 8)) * Pmo);
+
+  m_y = static_cast<double>((PN * std::cos(m_lat) * PDL -
+    1.0 / 6.0 * PN * std::pow(std::cos(m_lat), 3) * (-1 + std::pow(Pt, 2) - std::pow(Pnn, 2)) * std::pow(PDL, 3) -
+    1.0 / 120.0 * PN * std::pow(std::cos(m_lat), 5) *
+    (-5.0 + 18.0 * std::pow(Pt, 2) - std::pow(Pt, 4) - 14.0 * std::pow(Pnn, 2) + 58.0 * std::pow(Pt, 2) *
+      std::pow(Pnn, 2)) * std::pow(PDL, 5) -
+    1.0 / 5040.0 * PN * std::pow(std::cos(m_lat), 7) *
+    (-61.0 + 479.0 * std::pow(Pt, 2) - 179.0 * std::pow(Pt, 4) + std::pow(Pt, 6)) * std::pow(PDL, 7)) * Pmo);
+
+  m_z = m_h;
+}
+
+void geo_pos_conv::conv_xyz2llh(void)
+{
+  // n/a
+}

src/ros/catkin/src/gnss/test/test_gnss.cpp

@@ -0,0 +1,67 @@
+/*
+ * 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 <gtest/gtest.h>
+
+#include "gnss/geo_pos_conv.hpp"
+
+TEST(TestSuite, llhNmeaDegreesTest)
+{
+  geo_pos_conv geo_;
+
+  geo_.set_llh_nmea_degrees(0.000000, 0.000000, 10.124025);
+  ASSERT_FLOAT_EQ(0.000000, geo_.geo_pos_conv::x());
+  ASSERT_FLOAT_EQ(0.000000, geo_.geo_pos_conv::y());
+  ASSERT_FLOAT_EQ(10.124025, geo_.geo_pos_conv::z());
+
+  geo_.set_llh_nmea_degrees(1.000000, 1.000000, 10.124025);
+  ASSERT_FLOAT_EQ(1842.720386, geo_.geo_pos_conv::x());
+  ASSERT_FLOAT_EQ(1855.139262, geo_.geo_pos_conv::y());
+  ASSERT_FLOAT_EQ(10.124025, geo_.geo_pos_conv::z());
+
+  geo_.set_llh_nmea_degrees(-1.000000, -1.000000, 10.124025);
+  ASSERT_FLOAT_EQ(-1842.720386, geo_.geo_pos_conv::x());
+  ASSERT_FLOAT_EQ(-1855.139262, geo_.geo_pos_conv::y());
+  ASSERT_FLOAT_EQ(10.124025, geo_.geo_pos_conv::z());
+
+  geo_.set_llh_nmea_degrees(10.000000, 10.000000, 10.124025);
+  ASSERT_FLOAT_EQ(18427.282074, geo_.geo_pos_conv::x());
+  ASSERT_FLOAT_EQ(18551.341517, geo_.geo_pos_conv::y());
+  ASSERT_FLOAT_EQ(10.124025, geo_.geo_pos_conv::z());
+
+  geo_.set_llh_nmea_degrees(-10.000000, -10.000000, 10.124025);
+  ASSERT_FLOAT_EQ(-18427.282074, geo_.geo_pos_conv::x());
+  ASSERT_FLOAT_EQ(-18551.341517, geo_.geo_pos_conv::y());
+  ASSERT_FLOAT_EQ(10.124025, geo_.geo_pos_conv::z());
+
+  geo_.set_llh_nmea_degrees(100.000000, 100.000000, 10.124025);
+  ASSERT_FLOAT_EQ(110580.283099, geo_.geo_pos_conv::x());
+  ASSERT_FLOAT_EQ(111297.204780, geo_.geo_pos_conv::y());
+  ASSERT_FLOAT_EQ(10.124025, geo_.geo_pos_conv::z());
+
+  geo_.set_llh_nmea_degrees(-100.000000, -100.000000, 10.124025);
+  ASSERT_FLOAT_EQ(-110580.283099, geo_.geo_pos_conv::x());
+  ASSERT_FLOAT_EQ(-111297.204780, geo_.geo_pos_conv::y());
+  ASSERT_FLOAT_EQ(10.124025, geo_.geo_pos_conv::z());
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "test_gnss");
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/gnss/test/test_gnss.test

@@ -0,0 +1,4 @@
+<?xml version="1.0"?>
+<launch>
+  <test test-name="test_gnss" pkg="gnss" type="test_gnss" name="test" />
+</launch>

src/ros/catkin/src/lane_planner/CHANGELOG.rst

@@ -0,0 +1,395 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package lane_planner
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* [Feature] Rebuild decision maker (`#1609 <https://github.com/CPFL/Autoware/issues/1609>`_)
+* [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
+* Fix license notice in corresponding package.xml
+* Contributors: Abraham Monrroy Cano, amc-nu, s-azumi
+
+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)
+------------------
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* [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
+* 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
+* Contributors: Abraham Monrroy, Kosuke Murakami
+
+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
+* Fixed:
+  - callback
+  - laneshift
+  Added:
+  - publisher for laneid
+  - new lanechange flag
+  - new param for decisionMaker
+* apply clang-format
+* fix a segv bug when currentpose was changed a lot
+* Support to lanechange similar to state_machine(old) package
+* add path velocity smoothing
+* 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
+* Add decision packages into runtime_manager
+* 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)
+------------------
+* convert to autoware_msgs
+* Contributors: YamatoAndo
+
+1.2.0 (2017-06-07)
+------------------
+* fix circular-dependency
+* Contributors: Shohei Fujii
+
+1.1.2 (2017-02-27 23:10)
+------------------------
+* output log
+* publish closest waypoint as -1 when cannot find each closest waypoint in each lane
+* Contributors: h_ohta
+
+1.1.1 (2017-02-27 22:25)
+------------------------
+
+1.1.0 (2017-02-24)
+------------------
+* Fix style
+* Fix hermite curve
+* Remove unused
+* Fix style
+* Change transparency of lane for change
+* Change buffer size
+* every time find neighbor lanes
+* Update README for lane_planner
+* parameter from runtime manager
+* Fix definition of function
+* Change copy to move
+* add error avoidance
+* optimize for RVO
+* Remove unused variable
+* Create new lane for lane change
+* Add hermite curve library
+* Remove debug code
+* Fix bug for searching closest wapoint
+* Subscribe state
+* Publish change flag as topic, which keeps the same value until lane change is finished
+* right and left lane index is -1 when closest waypoint on each lane is -1
+* Update README.md
+* Update README.md
+* Apply clang-format
+* Refactoring code
+* Update visualization
+* Add ROS_WARN
+* Update interface.yaml for each packages
+* Update README.md for lane_planner
+* Initialize the closest waypoint number when the vehicle is outside of a search distance
+* Delete useless braces
+* initial commit for README.md for each packages
+* Fix Indent
+* Sort definitions
+* Rewrite visualizer
+* Rewrite lane change processing adding state and new tuple
+* Add ROS_INFO about current change_flag
+* Move ROS_INFO
+* Modify value which will be added into tuple
+* Add ChangeFlag value into tuple
+* Remove unused code
+* Add state variable
+* Add include
+* Change output to log
+* Edit Comment out and WARN message
+* Change processing order, Fix not getting neighbor lanes when current lane index is fixed
+* Fix keeping storing lane array infinitely in vector
+* Fix comment
+* Add lane initialization when subscribed lane array
+* Rewrite to change local planning to global planning
+* Create run function
+* bring together initializer for ROS
+* Fix include guard
+* Delete comment out
+* Add launch file for lane_select, fix to use ros parameter
+* apply clang-format
+* Rewrite lane_select node and add new function
+* Contributors: Hiroki Ohta, h_ohta
+
+1.0.1 (2017-01-14)
+------------------
+
+1.0.0 (2016-12-22)
+------------------
+* Create red and green lanes from waypoint_saver CSV
+* Fix codes to use map_file messages and old vector_map_info topics
+* Add module graph tool
+* Publish cached waypoint
+  If configure lane_rule, publish cached waypoint.
+* Fix lane_select bug.
+  /traffic_waypoints_array よりも先に /config/lane_select が来ると、
+  g_lane_array.lanes が空で落ちるため、チェックを追加。
+* Switch signal detection source by Runtime Manager configuration
+* Correct runtime manager dependencies
+* Improve handling junction lane
+* Create lane_navi.launch
+* Compute yaw in lane_navi and waypoint_clicker
+* Change subscribe topic
+* Rename topics of LaneArray message
+* Delete old API
+* Rewrite lane_stop by new API
+* Rewrite lane_rule by new API
+* Rewrite lane_navi by new API
+* Add new API for multiple lanes
+* Change two lanes in lane_select
+* Add number_of_zeros_behind parameter
+* Rename number_of_zeros parameter
+* Use c++11 option instead of c++0x
+  We can use newer compilers which support 'c++11' option
+* Make any pramaters configurable
+* Support direction angle
+* Move error variable declaration
+* Add utility for direction angle
+* Fix velocity computation on crossroads
+* changed topic name
+* Fix subscribing topic names
+* Cache current waypoints
+* Publish without change in default of vmap
+* Smooth acceleration and deceleration at crossroads
+* Initial commit for public release
+* Contributors: Hiroki Ohta, Shinpei Kato, Syohei YOSHIDA, USUDA Hisashi, syouji

src/ros/catkin/src/lane_planner/CMakeLists.txt

@@ -0,0 +1,134 @@
+cmake_minimum_required(VERSION 2.8.12)
+project(lane_planner)
+
+find_package(autoware_build_flags REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  autoware_config_msgs
+  autoware_msgs
+  gnss
+  libwaypoint_follower
+  roscpp
+  std_msgs
+  tablet_socket_msgs
+  vector_map
+)
+
+set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}")
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES lane_planner
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(
+  lane_planner
+  lib/lane_planner/lane_planner_vmap.cpp
+)
+
+target_link_libraries(
+  lane_planner
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(
+  lane_planner
+  ${catkin_EXPORTED_TARGETS}
+)
+
+add_executable(
+  lane_navi
+  nodes/lane_navi/lane_navi.cpp
+)
+
+target_link_libraries(
+  lane_navi
+  lane_planner
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(
+  lane_navi
+  ${catkin_EXPORTED_TARGETS}
+)
+
+add_executable(
+  lane_rule
+  nodes/lane_rule/lane_rule.cpp
+)
+
+target_link_libraries(
+  lane_rule
+  lane_planner
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(
+  lane_rule
+  ${catkin_EXPORTED_TARGETS}
+)
+
+add_executable(
+  lane_stop
+  nodes/lane_stop/lane_stop.cpp
+)
+
+target_link_libraries(lane_stop lane_planner ${catkin_LIBRARIES})
+add_dependencies(
+  lane_stop
+  ${catkin_EXPORTED_TARGETS}
+)
+
+add_executable(
+  lane_select
+  nodes/lane_select/lane_select_node.cpp
+  nodes/lane_select/lane_select_core.cpp
+  nodes/lane_select/hermite_curve.cpp
+)
+
+target_link_libraries(lane_select ${catkin_LIBRARIES})
+
+add_dependencies(
+  lane_select
+  ${catkin_EXPORTED_TARGETS}
+)
+
+install(
+  DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(
+  TARGETS
+    lane_navi
+    lane_planner
+    lane_rule
+    lane_select
+    lane_stop
+  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
+)
+
+if(CATKIN_ENABLE_TESTING)
+    find_package(rostest REQUIRED)
+    add_rostest_gtest(test-lane_select
+      test/test_lane_select.test
+      test/src/test_lane_select.cpp
+      nodes/lane_select/lane_select_core.cpp
+      nodes/lane_select/hermite_curve.cpp
+    )
+    add_dependencies(test-lane_select ${catkin_EXPORTED_TARGETS})
+    target_link_libraries(test-lane_select
+    ${catkin_LIBRARIES})
+endif()

src/ros/catkin/src/lane_planner/README.md

@@ -0,0 +1,92 @@
+# lane_planner
+
+## Package
+
+This package has following nodes.
+- lane_navi
+- lane_rule
+- lane_select
+- lane_stop
+
+## Nodes
+
+### lane_rule_lanelet2
+#### Overview:<br>
+`lane_rule_lanelet2` detects stoplines asociated with traffic lights that intersect with waypoints, and changes velocity of waypoints so that vehicle can stop before stopline when traffic_light is red. (It actually publishes both green_waypoints and red_waypoints, and `lane_stop` will choose which waypoint to use according to the recognized traffic_light color).
+Stop sign stoplines are not considered.
+
+#### Subscribed Topics
+|topic| type | Description|
+|----------|-----|--------|
+|`/lanelet_map_bin`|*lanelet_msgs/MapBin*|binary data of lanelet2 map|
+|`/lane_waypoints_array`|*autoware_msgs/LaneArray*|global waypoints|
+|`/config/lane_rule`|*autoware_config_msgs/ConfigLaneRule*|topics to update parameters at runtime. Note that `stopline_search_radius` member is not used in this node.|
+
+#### Published Topics
+
+|topic| type | Description|
+|----------|-----|--------|
+|`/traffic_waypoints_array`|*autoware_msgs/LaneArray*|waypoints with no change|
+|`/red_waypoints_array`|*autoware_msgs/LaneArray*|waypoints that are meant to be used when traffic light is red|
+|`/green_waypoints_array`|*autoware_msgs/LaneArray*|waypoints that are meant to be used when traffic light is green|
+
+#### Parameters
+
+|Parameter| Type| Description|Default|
+|----------|-----|--------|---|
+|`~/sub_waypoint_queue_size`|*int*|queue size of subscriber for `/lane_waypoints_array`|`1`|
+|`~/sub_config_queue_size`|*int*|queue size of subscriber for `/config/lane_rule`|`1`|
+|`~/pub_waypoint_queue_size`|*int*|queue size of publishers|`1`|
+|`~/pub_waypoint_latch`|*bool*|whether or not to latch published topics|`true`|
+|`~/frame_id`|*string*|frame_id of waypoints|`"map"`|
+|`~/acceleration`|*double*|acceleration/decleration used to stop at stoplines|`1`|
+|`~/number_of_smoothing_count`|*int*|number of iteration for smoothing out waypoint velocities|`0`|
+|`~/number_of_zeros_ahead`|*int*|number of waypoints with 0 m/s velocity before stoplines|`0`|
+|`~/number_of_zeros_behind`|*int*|number of waypoints with 0 m/s velocity after stoplines|`0`|
+
+### lane_select
+
+1. 概要
+    1. 複数経路を受け取る
+    1. 全ての経路に対して現在位置からの最近傍点を算出する
+    1. 一番近い最近傍点を持つ車線を現在走行している経路と設定
+    1. 現在経路に対する左右の経路を検出
+    1. 現在経路の最近傍点が持つ車線変更フラグをその経路の車線変更フラグとして保持
+    1. 最近傍の右折or左折のフラグを持つ点を探し、その点と車線変更を行う予定の車線の目標点をエルミート補間した経路を生成、その経路と車線変更予定の目標点以降の経路を結合した経路を車線変更用の経路として定義
+    1. 車線変更を行わない場合、現在経路、それに対する最近傍点、車線変更フラグをそれぞれpublishする
+    1. 車線変更を行う場合、車線変更用の経路、それに対する最近傍点、車線変更フラグをそれぞれpublishする
+
+1. 注意
+    - 車線変更を行うには、ver3フォーマットの経路ファイルを複数個読み込まれている必要がある。（waypoint_makerパッケージ参照）
+
+1. Subscribed Topics
+
+    - traffic_waypoints_array (waypoint_follower/LaneArray)
+    - current_pose (geometry_msgs/PoseStamped)
+    - current_velocity (geometry_msgs/TwistStamped)
+    - state (std_msgs/String)
+    - config/lane_select (runtime_manager/ConfigLaneSelect)
+
+1. Published Topics
+
+    - base_waypoints (waypoint_follower/lane)
+    - closest_waypoint (std_msgs/Int32)
+    - change_flag (std_msgs/Int32)
+    - lane_select_marker (visualization_msgs/MarkerArray) : for debug
+
+1. Parameter from Runtime Manager
+
+    - Distance threshold to neighbor lanes<br>
+    現在経路の周りの有効な車線を検出する際のしきい値を表す。このしきい値より遠い距離にある車線は車線として認識しない。
+
+    - Lane Change Interval After Lane Merge
+    車線変更を行ったあとに何メートル走ったらまた車線変更を行えるようになるかの値を表す。
+
+    - Lane Change Target Ratio
+    車線変更を行う予定の車線上の目標点を速度(m/s)に比例した距離で定義する際に使用する値。
+    目標点探索の起点は車線変更予定の車線上の点において、右折or左折の車線変更フラグを持つ点の最近傍点。
+    - Lane Change Target Minimum
+    車線変更を行う予定の車線上の目標点までの最低距離を表す。
+    目標点探索の起点は車線変更予定の車線上の点において、右折or左折の車線変更フラグを持つ点の最近傍点。
+    - Vector Length of Hermite Curve
+    エルミート曲線で補完する際のベクトルの大きさを表す。

src/ros/catkin/src/lane_planner/include/hermite_curve.h

@@ -0,0 +1,58 @@
+/*
+ * 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 HERMITE_CURVE_H
+#define HERMITE_CURVE_H
+
+// C++ includes
+#include <cmath>
+#include <cstdio>
+#include <iostream>
+#include <vector>
+
+// ROS includes
+#include <geometry_msgs/Pose.h>
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+
+#include "autoware_msgs/Waypoint.h"
+
+namespace lane_planner
+{
+struct Element2D
+{
+  Element2D(double x, double y)
+  {
+    this->x = x;
+    this->y = y;
+  }
+  void set(double x, double y)
+  {
+    this->x = x;
+    this->y = y;
+  }
+  double x;
+  double y;
+};
+
+std::vector<Element2D> generateHermiteCurve(const Element2D &p0, const Element2D &v0, const Element2D &p1,
+                                            const Element2D &v1, const double vlength = 20);
+std::vector<autoware_msgs::Waypoint> generateHermiteCurveForROS(const geometry_msgs::Pose &start,
+                                                                const geometry_msgs::Pose &end, const double velocity,
+                                                                const double vlength);
+void createVectorFromPose(const geometry_msgs::Pose &p, tf::Vector3 *v);
+}  // namespace
+#endif  // HERMITE_CURVE_H

src/ros/catkin/src/lane_planner/include/lane_planner/lane_planner_vmap.hpp

@@ -0,0 +1,86 @@
+/*
+ * 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 LANE_PLANNER_VMAP_HPP
+#define LANE_PLANNER_VMAP_HPP
+
+#include <string>
+#include <vector>
+
+#include <geometry_msgs/Point.h>
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+
+#include <vector_map/vector_map.h>
+#include <tablet_socket_msgs/route_cmd.h>
+#include "autoware_msgs/DTLane.h"
+#include "autoware_msgs/Lane.h"
+
+namespace lane_planner {
+
+namespace vmap {
+
+constexpr int LNO_ALL = -1;
+constexpr int LNO_CROSSING = 0;
+constexpr int LNO_MOSTLEFT = 1;
+
+constexpr int TRAFFIC_LIGHT_RED = 0;
+constexpr int TRAFFIC_LIGHT_GREEN = 1;
+constexpr int TRAFFIC_LIGHT_UNKNOWN = 2;
+
+constexpr double RADIUS_MAX = 90000000000;
+
+struct VectorMap {
+  std::vector<vector_map::Point> points;
+  std::vector<vector_map::Lane> lanes;
+  std::vector<vector_map::Node> nodes;
+  std::vector<vector_map::StopLine> stoplines;
+  std::vector<vector_map::DTLane> dtlanes;
+};
+
+void write_waypoints(const std::vector<vector_map::Point>& points, double velocity, const std::string& path);
+
+double compute_reduction(const vector_map::DTLane& d, double w);
+
+bool is_straight_dtlane(const vector_map::DTLane& dtlane);
+bool is_curve_dtlane(const vector_map::DTLane& dtlane);
+bool is_crossroad_dtlane(const vector_map::DTLane& dtlane);
+bool is_clothoid_dtlane(const vector_map::DTLane& dtlane);
+bool is_connection_dtlane(const VectorMap& fine_vmap, int index);
+
+geometry_msgs::Point create_geometry_msgs_point(const vector_map::Point& vp);
+vector_map::Point create_vector_map_point(const geometry_msgs::Point& gp);
+autoware_msgs::DTLane create_waypoint_follower_dtlane(const vector_map::DTLane& vd);
+vector_map::DTLane create_vector_map_dtlane(const autoware_msgs::DTLane& wd);
+
+VectorMap create_lane_vmap(const VectorMap& vmap, int lno);
+VectorMap create_coarse_vmap_from_lane(const autoware_msgs::Lane& lane);
+VectorMap create_coarse_vmap_from_route(const tablet_socket_msgs::route_cmd& route);
+VectorMap create_fine_vmap(const VectorMap& lane_vmap, int lno, const VectorMap& coarse_vmap, double search_radius,
+         int waypoint_max);
+
+std::vector<vector_map::Point> create_branching_points(const VectorMap& vmap);
+std::vector<vector_map::Point> create_merging_points(const VectorMap& vmap);
+
+void publish_add_marker(const ros::Publisher& pub, const visualization_msgs::Marker& marker,
+      const std::vector<vector_map::Point>& points);
+void publish_delete_marker(const ros::Publisher& pub, const visualization_msgs::Marker& marker);
+
+} // namespace vmap
+
+} // namespace lane_planner
+
+#endif // LANE_PLANNER_VMAP_HPP

src/ros/catkin/src/lane_planner/include/lane_select_core.h

@@ -0,0 +1,161 @@
+/*
+ * 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 LANE_SELECT_CORE_H
+#define LANE_SELECT_CORE_H
+
+// ROS includes
+#include <ros/ros.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
+#include <tf/transform_datatypes.h>
+#include <visualization_msgs/MarkerArray.h>
+
+// C++ includes
+#include <iostream>
+#include <numeric>
+#include <tuple>
+
+// User defined includes
+#include "autoware_config_msgs/ConfigLaneSelect.h"
+#include "autoware_msgs/LaneArray.h"
+#include "autoware_msgs/State.h"
+#include "autoware_msgs/VehicleLocation.h"
+#include "hermite_curve.h"
+#include "libwaypoint_follower/libwaypoint_follower.h"
+
+namespace lane_planner
+{
+enum class ChangeFlag : int32_t
+{
+  straight,
+  right,
+  left,
+
+  unknown = -1,
+};
+
+template <class T>
+typename std::underlying_type<T>::type enumToInteger(T t)
+{
+  return static_cast<typename std::underlying_type<T>::type>(t);
+}
+
+class LaneSelectNode
+{
+friend class LaneSelectTestClass;
+
+public:
+  LaneSelectNode();
+  ~LaneSelectNode();
+
+  void run();
+
+private:
+  // handle
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+
+  // publisher
+  ros::Publisher pub1_, pub2_, pub3_, pub4_, pub5_;
+  ros::Publisher vis_pub1_;
+
+  // subscriber
+  ros::Subscriber sub1_, sub2_, sub3_, sub4_, sub5_, sub6_;
+
+  // variables
+  int32_t lane_array_id_;
+  int32_t current_lane_idx_;  // the index of the lane we are driving
+  int32_t right_lane_idx_;
+  int32_t left_lane_idx_;
+  std::vector<std::tuple<autoware_msgs::Lane, int32_t, ChangeFlag>> tuple_vec_;  // lane, closest_waypoint,
+                                                                                 // change_flag
+  std::tuple<autoware_msgs::Lane, int32_t, ChangeFlag> lane_for_change_;
+  bool is_lane_array_subscribed_, is_current_pose_subscribed_, is_current_velocity_subscribed_,
+      is_current_state_subscribed_, is_config_subscribed_;
+
+  // parameter from runtime manager
+  double distance_threshold_, lane_change_interval_, lane_change_target_ratio_, lane_change_target_minimum_,
+      vlength_hermite_curve_;
+  int search_closest_waypoint_minimum_dt_;
+
+  // topics
+  geometry_msgs::PoseStamped current_pose_;
+  geometry_msgs::TwistStamped current_velocity_;
+  std::string current_state_;
+
+  // callbacks
+  void callbackFromLaneArray(const autoware_msgs::LaneArrayConstPtr& msg);
+  void callbackFromPoseStamped(const geometry_msgs::PoseStampedConstPtr& msg);
+  void callbackFromTwistStamped(const geometry_msgs::TwistStampedConstPtr& msg);
+  void callbackFromState(const std_msgs::StringConstPtr& msg);
+  void callbackFromDecisionMakerState(const std_msgs::StringConstPtr& msg);
+  void callbackFromConfig(const autoware_config_msgs::ConfigLaneSelectConstPtr& msg);
+
+  // initializer
+  void initForROS();
+  void initForLaneSelect();
+
+  // visualizer
+  void publishVisualizer();
+  visualization_msgs::Marker createCurrentLaneMarker();
+  visualization_msgs::Marker createRightLaneMarker();
+  visualization_msgs::Marker createLeftLaneMarker();
+  visualization_msgs::Marker createClosestWaypointsMarker();
+  visualization_msgs::Marker createChangeLaneMarker();
+
+  // functions
+  void resetLaneIdx();
+  void resetSubscriptionFlag();
+  bool isAllTopicsSubscribed();
+  void processing();
+  void publishLane(const autoware_msgs::Lane& lane);
+  void publishLaneID(const autoware_msgs::Lane& lane);
+  void publishClosestWaypoint(const int32_t clst_wp);
+  void publishChangeFlag(const ChangeFlag flag);
+  void publishVehicleLocation(const int32_t clst_wp, const int32_t larray_id);
+  bool getClosestWaypointNumberForEachLanes();
+  int32_t findMostClosestLane(const std::vector<uint32_t> idx_vec, const geometry_msgs::Point p);
+  void findCurrentLane();
+  void findNeighborLanes();
+  void changeLane();
+  void updateChangeFlag();
+  void createLaneForChange();
+  int32_t getClosestLaneChangeWaypointNumber(const std::vector<autoware_msgs::Waypoint>& wps, int32_t cl_wp);
+
+  // spinOnce for test
+  void spinOnce()
+  {
+    ros::spinOnce();
+  }
+};
+
+int32_t getClosestWaypointNumber(const autoware_msgs::Lane& current_lane, const geometry_msgs::Pose& current_pose,
+                                 const geometry_msgs::Twist& current_velocity, const int32_t previous_number,
+                                 const double distance_threshold, const int search_closest_waypoint_minimum_dt);
+
+double getTwoDimensionalDistance(const geometry_msgs::Point& target1, const geometry_msgs::Point& target2);
+
+geometry_msgs::Point convertPointIntoRelativeCoordinate(const geometry_msgs::Point& input_point,
+                                                        const geometry_msgs::Pose& pose);
+
+geometry_msgs::Point convertPointIntoWorldCoordinate(const geometry_msgs::Point& input_point,
+                                                     const geometry_msgs::Pose& pose);
+double getRelativeAngle(const geometry_msgs::Pose& waypoint_pose, const geometry_msgs::Pose& current_pose);
+bool getLinearEquation(geometry_msgs::Point start, geometry_msgs::Point end, double* a, double* b, double* c);
+double getDistanceBetweenLineAndPoint(geometry_msgs::Point point, double sa, double b, double c);
+}
+#endif  // LANE_SELECT_CORE_H

src/ros/catkin/src/lane_planner/interface.yaml

@@ -0,0 +1,15 @@
+- name: /lane_navi
+  publish: [/lane_waypoints_array]
+  subscribe: [/route_cmd, /vector_map_info/point, /vector_map_info/lane, /vector_map_info/node]
+- name: /lane_rule
+  publish: [/traffic_waypoints_array, /red_waypoints_array, /green_waypoints_array]
+  subscribe: [/lane_waypoints_array, /vector_map_info/point, /vector_map_info/lane,
+    /vector_map_info/node, /vector_map_info/stop_line, /vector_map_info/dtlane, /config/lane_rule]
+- name: /lane_select
+  publish: [/base_waypoints, /closest_waypoint]
+  subscribe: [/traffic_waypoints_array, /current_pose, /current_velocity, /state,
+    /config/lane_select, /decisionmaker/states]
+- name: /lane_stop
+  publish: [/traffic_waypoints_array]
+  subscribe: [/light_color, /light_color_managed, /red_waypoints_array,
+    /green_waypoints_array, /config/lane_stop]

src/ros/catkin/src/lane_planner/launch/lane_navi.launch

@@ -0,0 +1,14 @@
+<!-- -->
+<launch>
+  <arg name="velocity" default="40" />
+  <arg name="output_file" default="/tmp/lane_waypoint.csv" />
+
+  <node pkg="lane_planner" type="lane_navi" name="lane_navi" output="screen">
+      <param name="velocity" value="$(arg velocity)" />
+      <param name="output_file" value="$(arg output_file)" />
+  </node>
+  
+  <node pkg="waypoint_maker" type="waypoint_marker_publisher" name="waypoint_marker_publisher"/>
+
+
+</launch>

src/ros/catkin/src/lane_planner/launch/lane_rule_option.launch

@@ -0,0 +1,7 @@
+<!-- -->
+<launch>
+  <arg name="use_ll2" default="true" />
+
+  <node if="$(arg use_ll2)" pkg="lane_planner" type="lane_rule_lanelet2" name="lane_rule" output="screen" />
+  <node unless="$(arg use_ll2)" pkg="lane_planner" type="lane_rule" name="lane_rule" output="screen" />
+</launch>

src/ros/catkin/src/lane_planner/launch/lane_select.launch

@@ -0,0 +1,8 @@
+<!-- -->
+<launch>
+  <arg name="search_closest_waypoint_minimum_dt" default="5" doc="Minimum number of lookahead waypoints when searching closest_waypoint"/>
+
+  <node pkg="lane_planner" type="lane_select" name="lane_select" output="log">
+    <param name="search_closest_waypoint_minimum_dt" value="$(arg search_closest_waypoint_minimum_dt)" />
+  </node>
+</launch>

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

@@ -0,0 +1,237 @@
+/*
+ * 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 <sstream>
+
+#include <ros/console.h>
+#include <tf/transform_datatypes.h>
+
+#include <vector_map/vector_map.h>
+#include "autoware_msgs/LaneArray.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;
+
+ros::Publisher waypoint_pub;
+
+lane_planner::vmap::VectorMap all_vmap;
+lane_planner::vmap::VectorMap lane_vmap;
+tablet_socket_msgs::route_cmd cached_route;
+
+std::vector<std::string> split(const std::string& str, char delim)
+{
+  std::stringstream ss(str);
+  std::string s;
+  std::vector<std::string> vec;
+  while (std::getline(ss, s, delim))
+    vec.push_back(s);
+
+  if (!str.empty() && str.back() == delim)
+    vec.push_back(std::string());
+
+  return vec;
+}
+
+std::string join(const std::vector<std::string>& vec, char delim)
+{
+  std::string str;
+  for (size_t i = 0; i < vec.size(); ++i) {
+    str += vec[i];
+    if (i != (vec.size() - 1))
+      str += delim;
+  }
+
+  return str;
+}
+
+int count_lane(const lane_planner::vmap::VectorMap& vmap)
+{
+  int lcnt = -1;
+
+  for (const vector_map::Lane& l : vmap.lanes) {
+    if (l.lcnt > lcnt)
+      lcnt = l.lcnt;
+  }
+
+  return lcnt;
+}
+
+void create_waypoint(const tablet_socket_msgs::route_cmd& msg)
+{
+  std_msgs::Header header;
+  header.stamp = ros::Time::now();
+  header.frame_id = frame_id;
+
+  if (all_vmap.points.empty() || all_vmap.lanes.empty() || all_vmap.nodes.empty()) {
+    cached_route.header = header;
+    cached_route.point = msg.point;
+    return;
+  }
+
+  lane_planner::vmap::VectorMap coarse_vmap = lane_planner::vmap::create_coarse_vmap_from_route(msg);
+  if (coarse_vmap.points.size() < 2)
+    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)
+    return;
+  fine_vmaps.push_back(fine_mostleft_vmap);
+
+  int lcnt = count_lane(fine_mostleft_vmap);
+  for (int i = lane_planner::vmap::LNO_MOSTLEFT + 1; i <= lcnt; ++i) {
+    lane_planner::vmap::VectorMap v =
+      lane_planner::vmap::create_fine_vmap(lane_vmap, i, coarse_vmap, search_radius, waypoint_max);
+    if (v.points.size() < 2)
+      continue;
+    fine_vmaps.push_back(v);
+  }
+
+  autoware_msgs::LaneArray lane_waypoint;
+  for (const lane_planner::vmap::VectorMap& v : fine_vmaps) {
+    autoware_msgs::Lane l;
+    l.header = header;
+    l.increment = 1;
+
+    size_t last_index = v.points.size() - 1;
+    for (size_t i = 0; i < v.points.size(); ++i) {
+      double yaw;
+      if (i == last_index) {
+        geometry_msgs::Point p1 =
+          lane_planner::vmap::create_geometry_msgs_point(v.points[i]);
+        geometry_msgs::Point p2 =
+          lane_planner::vmap::create_geometry_msgs_point(v.points[i - 1]);
+        yaw = atan2(p2.y - p1.y, p2.x - p1.x);
+        yaw -= M_PI;
+      } else {
+        geometry_msgs::Point p1 =
+          lane_planner::vmap::create_geometry_msgs_point(v.points[i]);
+        geometry_msgs::Point p2 =
+          lane_planner::vmap::create_geometry_msgs_point(v.points[i + 1]);
+        yaw = atan2(p2.y - p1.y, p2.x - p1.x);
+      }
+
+      autoware_msgs::Waypoint w;
+      w.pose.header = header;
+      w.pose.pose.position = lane_planner::vmap::create_geometry_msgs_point(v.points[i]);
+      w.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
+      w.twist.header = header;
+      w.twist.twist.linear.x = velocity / 3.6; // to m/s
+      l.waypoints.push_back(w);
+    }
+    lane_waypoint.lanes.push_back(l);
+  }
+  waypoint_pub.publish(lane_waypoint);
+
+  for (size_t i = 0; i < fine_vmaps.size(); ++i) {
+    std::stringstream ss;
+    ss << "_" << i;
+
+    std::vector<std::string> v1 = split(output_file, '/');
+    std::vector<std::string> v2 = split(v1.back(), '.');
+    v2[0] = v2.front() + ss.str();
+    v1[v1.size() - 1] = join(v2, '.');
+    std::string path = join(v1, '/');
+
+    lane_planner::vmap::write_waypoints(fine_vmaps[i].points, velocity, path);
+  }
+}
+
+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);
+
+  if (!cached_route.point.empty()) {
+    create_waypoint(cached_route);
+    cached_route.point.clear();
+    cached_route.point.shrink_to_fit();
+  }
+}
+
+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, "lane_navi");
+
+  ros::NodeHandle n;
+
+  int sub_vmap_queue_size;
+  n.param<int>("/lane_navi/sub_vmap_queue_size", sub_vmap_queue_size, 1);
+  int sub_route_queue_size;
+  n.param<int>("/lane_navi/sub_route_queue_size", sub_route_queue_size, 1);
+  int pub_waypoint_queue_size;
+  n.param<int>("/lane_navi/pub_waypoint_queue_size", pub_waypoint_queue_size, 1);
+  bool pub_waypoint_latch;
+  n.param<bool>("/lane_navi/pub_waypoint_latch", pub_waypoint_latch, true);
+
+  n.param<int>("/lane_navi/waypoint_max", waypoint_max, 10000);
+  n.param<double>("/lane_navi/search_radius", search_radius, 10);
+  n.param<double>("/lane_navi/velocity", velocity, 40);
+  n.param<std::string>("/lane_navi/frame_id", frame_id, "map");
+  n.param<std::string>("/lane_navi/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_pub = n.advertise<autoware_msgs::LaneArray>("/lane_waypoints_array", pub_waypoint_queue_size,
+                 pub_waypoint_latch);
+
+  ros::Subscriber route_sub = n.subscribe("/route_cmd", sub_route_queue_size, create_waypoint);
+  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/lane_planner/nodes/lane_rule/lane_rule.cpp

@@ -0,0 +1,653 @@
+/*
+ * 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.
+ */
+
+// #define DEBUG
+
+#ifdef DEBUG
+#include <sstream>
+#endif  // DEBUG
+
+#include <ros/console.h>
+
+#include <vector_map/vector_map.h>
+#include "autoware_config_msgs/ConfigLaneRule.h"
+#include "autoware_msgs/LaneArray.h"
+
+#include <lane_planner/lane_planner_vmap.hpp>
+
+namespace
+{
+double config_acceleration = 1;            // m/s^2
+double config_stopline_search_radius = 1;  // meter
+int config_number_of_zeros_ahead = 0;
+int config_number_of_zeros_behind = 0;
+int config_number_of_smoothing_count = 0;
+
+int waypoint_max;
+double search_radius;  // meter
+double curve_weight;
+double crossroad_weight;
+double clothoid_weight;
+std::string frame_id;
+
+ros::Publisher traffic_pub;
+ros::Publisher red_pub;
+ros::Publisher green_pub;
+
+lane_planner::vmap::VectorMap all_vmap;
+lane_planner::vmap::VectorMap lane_vmap;
+double curve_radius_min;
+double crossroad_radius_min;
+double clothoid_radius_min;
+autoware_msgs::LaneArray cached_waypoint;
+
+#ifdef DEBUG
+visualization_msgs::Marker debug_marker;
+ros::Publisher marker_pub;
+int marker_cnt;
+#endif  // DEBUG
+
+autoware_msgs::Lane create_new_lane(const autoware_msgs::Lane& lane, const std_msgs::Header& header)
+{
+  autoware_msgs::Lane l = lane;
+  l.header = header;
+
+  for (autoware_msgs::Waypoint& w : l.waypoints)
+  {
+    w.pose.header = header;
+    w.twist.header = header;
+  }
+
+  return l;
+}
+
+autoware_msgs::Lane apply_acceleration(const autoware_msgs::Lane& lane, double acceleration, size_t start_index,
+                                       size_t fixed_cnt, double fixed_vel)
+{
+  autoware_msgs::Lane l = lane;
+
+  if (fixed_cnt == 0)
+    return l;
+
+  double square_vel = fixed_vel * fixed_vel;
+  double distance = 0;
+  for (size_t i = start_index; i < l.waypoints.size(); ++i)
+  {
+    if (i - start_index < fixed_cnt)
+    {
+      l.waypoints[i].twist.twist.linear.x = fixed_vel;
+      continue;
+    }
+
+    geometry_msgs::Point a = l.waypoints[i - 1].pose.pose.position;
+    geometry_msgs::Point b = l.waypoints[i].pose.pose.position;
+    distance += hypot(b.x - a.x, b.y - a.y);
+
+    const int sgn = (l.waypoints[i].twist.twist.linear.x < 0.0) ? -1 : 1;
+    double v = sqrt(square_vel + 2 * acceleration * distance);
+    if (v < l.waypoints[i].twist.twist.linear.x)
+    {
+      l.waypoints[i].twist.twist.linear.x = sgn * v;
+    }
+    else
+    {
+      break;
+    }
+  }
+
+  return l;
+}
+
+autoware_msgs::Lane apply_crossroad_acceleration(const autoware_msgs::Lane& lane, double acceleration)
+{
+  autoware_msgs::Lane l = lane;
+
+  bool crossroad = false;
+  std::vector<size_t> start_indexes;
+  std::vector<size_t> end_indexes;
+  for (size_t i = 0; i < l.waypoints.size(); ++i)
+  {
+    vector_map::DTLane dtlane = lane_planner::vmap::create_vector_map_dtlane(l.waypoints[i].dtlane);
+    if (i == 0)
+    {
+      crossroad = lane_planner::vmap::is_crossroad_dtlane(dtlane);
+      continue;
+    }
+    if (crossroad)
+    {
+      if (!lane_planner::vmap::is_crossroad_dtlane(dtlane))
+      {
+        end_indexes.push_back(i - 1);
+        crossroad = false;
+      }
+    }
+    else
+    {
+      if (lane_planner::vmap::is_crossroad_dtlane(dtlane))
+      {
+        start_indexes.push_back(i);
+        crossroad = true;
+      }
+    }
+  }
+  if (start_indexes.empty() && end_indexes.empty())
+    return l;
+
+  for (const size_t i : end_indexes)
+    l = apply_acceleration(l, acceleration, i, 1, l.waypoints[i].twist.twist.linear.x);
+
+  std::reverse(l.waypoints.begin(), l.waypoints.end());
+
+  std::vector<size_t> reverse_start_indexes;
+  for (const size_t i : start_indexes)
+    reverse_start_indexes.push_back(l.waypoints.size() - i - 1);
+  std::reverse(reverse_start_indexes.begin(), reverse_start_indexes.end());
+
+  for (const size_t i : reverse_start_indexes)
+    l = apply_acceleration(l, acceleration, i, 1, l.waypoints[i].twist.twist.linear.x);
+
+  std::reverse(l.waypoints.begin(), l.waypoints.end());
+
+  return l;
+}
+
+autoware_msgs::Lane apply_stopline_acceleration(const autoware_msgs::Lane& lane, double acceleration,
+                                                const lane_planner::vmap::VectorMap& fine_vmap, size_t ahead_cnt,
+                                                size_t behind_cnt)
+{
+  autoware_msgs::Lane l = lane;
+
+  std::vector<size_t> indexes;
+  for (size_t i = 0; i < fine_vmap.stoplines.size(); ++i)
+  {
+    if (fine_vmap.stoplines[i].id >= 0)
+      indexes.push_back(i);
+  }
+  if (indexes.empty())
+    return l;
+
+  for (const size_t i : indexes)
+    l = apply_acceleration(l, acceleration, i, behind_cnt + 1, 0);
+
+  std::reverse(l.waypoints.begin(), l.waypoints.end());
+
+  std::vector<size_t> reverse_indexes;
+  for (const size_t i : indexes)
+    reverse_indexes.push_back(l.waypoints.size() - i - 1);
+  std::reverse(reverse_indexes.begin(), reverse_indexes.end());
+
+  for (const size_t i : reverse_indexes)
+    l = apply_acceleration(l, acceleration, i, ahead_cnt + 1, 0);
+
+  std::reverse(l.waypoints.begin(), l.waypoints.end());
+
+  return l;
+}
+
+std::vector<vector_map::Point> create_stop_points(const lane_planner::vmap::VectorMap& vmap)
+{
+  std::vector<vector_map::Point> stop_points;
+  for (const vector_map::StopLine& s : vmap.stoplines)
+  {
+    for (const vector_map::Lane& l : vmap.lanes)
+    {
+      if (l.lnid != s.linkid)
+        continue;
+      for (const vector_map::Node& n : vmap.nodes)
+      {
+        if (n.nid != l.bnid)
+          continue;
+        for (const vector_map::Point& p : vmap.points)
+        {
+          if (p.pid != n.pid)
+            continue;
+          bool hit = false;
+          for (const vector_map::Point& sp : stop_points)
+          {
+            if (sp.pid == p.pid)
+            {
+              hit = true;
+              break;
+            }
+          }
+          if (!hit)
+            stop_points.push_back(p);
+        }
+      }
+    }
+  }
+
+  return stop_points;
+}
+
+std::vector<size_t> create_stop_indexes(const lane_planner::vmap::VectorMap& vmap, const autoware_msgs::Lane& lane,
+                                        double stopline_search_radius)
+{
+  std::vector<size_t> stop_indexes;
+  for (const vector_map::Point& p : create_stop_points(vmap))
+  {
+    size_t index = SIZE_MAX;
+    double distance = DBL_MAX;
+    for (size_t i = 0; i < lane.waypoints.size(); ++i)
+    {
+      vector_map::Point point = lane_planner::vmap::create_vector_map_point(lane.waypoints[i].pose.pose.position);
+      double d = hypot(p.bx - point.bx, p.ly - point.ly);
+      if (d <= distance)
+      {
+        index = i;
+        distance = d;
+      }
+    }
+    if (index != SIZE_MAX && distance <= stopline_search_radius)
+    {
+      stop_indexes.push_back(index);
+    }
+  }
+  std::sort(stop_indexes.begin(), stop_indexes.end());
+
+  return stop_indexes;
+}
+
+autoware_msgs::Lane apply_stopline_acceleration(const autoware_msgs::Lane& lane, double acceleration,
+                                                double stopline_search_radius, size_t ahead_cnt, size_t behind_cnt)
+{
+  autoware_msgs::Lane l = lane;
+
+  std::vector<size_t> indexes = create_stop_indexes(lane_vmap, l, stopline_search_radius);
+  if (indexes.empty())
+    return l;
+
+  for (const size_t i : indexes)
+    l = apply_acceleration(l, acceleration, i, behind_cnt + 1, 0);
+
+  std::reverse(l.waypoints.begin(), l.waypoints.end());
+
+  std::vector<size_t> reverse_indexes;
+  for (const size_t i : indexes)
+    reverse_indexes.push_back(l.waypoints.size() - i - 1);
+  std::reverse(reverse_indexes.begin(), reverse_indexes.end());
+
+  for (const size_t i : reverse_indexes)
+    l = apply_acceleration(l, acceleration, i, ahead_cnt + 1, 0);
+
+  std::reverse(l.waypoints.begin(), l.waypoints.end());
+
+  return l;
+}
+
+bool is_fine_vmap(const lane_planner::vmap::VectorMap& fine_vmap, const autoware_msgs::Lane& lane)
+{
+  if (fine_vmap.points.size() != lane.waypoints.size())
+    return false;
+
+  for (size_t i = 0; i < fine_vmap.points.size(); ++i)
+  {
+    vector_map::Point point = lane_planner::vmap::create_vector_map_point(lane.waypoints[i].pose.pose.position);
+    double distance = hypot(fine_vmap.points[i].bx - point.bx, fine_vmap.points[i].ly - point.ly);
+    if (distance > 0.1)
+      return false;
+  }
+
+  return true;
+}
+
+double create_reduction(const lane_planner::vmap::VectorMap& fine_vmap, int index)
+{
+  const vector_map::DTLane& dtlane = fine_vmap.dtlanes[index];
+
+  if (lane_planner::vmap::is_straight_dtlane(dtlane))
+    return 1;
+
+  if (lane_planner::vmap::is_curve_dtlane(dtlane))
+  {
+    if (lane_planner::vmap::is_crossroad_dtlane(dtlane))
+      return lane_planner::vmap::compute_reduction(dtlane, crossroad_radius_min * crossroad_weight);
+
+    if (lane_planner::vmap::is_connection_dtlane(fine_vmap, index))
+      return 1;
+
+    return lane_planner::vmap::compute_reduction(dtlane, curve_radius_min * curve_weight);
+  }
+
+  if (lane_planner::vmap::is_clothoid_dtlane(dtlane))
+    return lane_planner::vmap::compute_reduction(dtlane, clothoid_radius_min * clothoid_weight);
+
+  return 1;
+}
+
+#ifdef DEBUG
+std_msgs::ColorRGBA create_color(int index)
+{
+  std_msgs::ColorRGBA color;
+  switch (index)
+  {
+    case 0:
+      color.r = 0;
+      color.g = 0;
+      color.b = 0;
+      break;
+    case 1:
+      color.r = 0;
+      color.g = 0;
+      color.b = 1;
+      break;
+    case 2:
+      color.r = 0;
+      color.g = 1;
+      color.b = 0;
+      break;
+    case 3:
+      color.r = 0;
+      color.g = 1;
+      color.b = 1;
+      break;
+    case 4:
+      color.r = 1;
+      color.g = 0;
+      color.b = 0;
+      break;
+    case 5:
+      color.r = 1;
+      color.g = 0;
+      color.b = 1;
+      break;
+    case 6:
+      color.r = 1;
+      color.g = 1;
+      color.b = 0;
+      break;
+    default:
+      color.r = 1;
+      color.g = 1;
+      color.b = 1;
+  }
+  color.a = 1;
+
+  return color;
+}
+#endif  // DEBUG
+
+void create_waypoint(const autoware_msgs::LaneArray& msg)
+{
+  std_msgs::Header header;
+  header.stamp = ros::Time::now();
+  header.frame_id = frame_id;
+
+  cached_waypoint.lanes.clear();
+  cached_waypoint.lanes.shrink_to_fit();
+  cached_waypoint.id = msg.id;
+  for (const autoware_msgs::Lane& l : msg.lanes)
+    cached_waypoint.lanes.push_back(create_new_lane(l, header));
+  if (all_vmap.points.empty() || all_vmap.lanes.empty() || all_vmap.nodes.empty() || all_vmap.stoplines.empty() ||
+      all_vmap.dtlanes.empty())
+  {
+    traffic_pub.publish(cached_waypoint);
+    return;
+  }
+
+#ifdef DEBUG
+  marker_cnt = msg.lanes.size();
+#endif  // DEBUG
+
+  autoware_msgs::LaneArray traffic_waypoint;
+  autoware_msgs::LaneArray red_waypoint;
+  autoware_msgs::LaneArray green_waypoint;
+  traffic_waypoint.id = red_waypoint.id = green_waypoint.id = msg.id;
+  for (size_t i = 0; i < msg.lanes.size(); ++i)
+  {
+    autoware_msgs::Lane lane = create_new_lane(msg.lanes[i], header);
+
+    lane_planner::vmap::VectorMap coarse_vmap = lane_planner::vmap::create_coarse_vmap_from_lane(lane);
+    if (coarse_vmap.points.size() < 2)
+    {
+      traffic_waypoint.lanes.push_back(lane);
+      continue;
+    }
+
+    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 || !is_fine_vmap(fine_vmap, lane))
+    {
+      traffic_waypoint.lanes.push_back(lane);
+      green_waypoint.lanes.push_back(lane);
+      lane = apply_stopline_acceleration(lane, config_acceleration, config_stopline_search_radius,
+                                         config_number_of_zeros_ahead, config_number_of_zeros_behind);
+      red_waypoint.lanes.push_back(lane);
+      continue;
+    }
+
+    for (size_t j = 0; j < lane.waypoints.size(); ++j)
+    {
+      lane.waypoints[j].twist.twist.linear.x *= create_reduction(fine_vmap, j);
+      if (fine_vmap.dtlanes[j].did >= 0)
+      {
+        lane.waypoints[j].dtlane = lane_planner::vmap::create_waypoint_follower_dtlane(fine_vmap.dtlanes[j]);
+      }
+    }
+
+    /* velocity smoothing */
+    for (int k = 0; k < config_number_of_smoothing_count; ++k)
+    {
+      autoware_msgs::Lane temp_lane = lane;
+      if (lane.waypoints.size() >= 3)
+      {
+        for (size_t j = 1; j < lane.waypoints.size() - 1; ++j)
+        {
+          if (lane.waypoints.at(j).twist.twist.linear.x != 0)
+          {
+            lane.waypoints[j].twist.twist.linear.x =
+                (temp_lane.waypoints.at(j - 1).twist.twist.linear.x + temp_lane.waypoints.at(j).twist.twist.linear.x +
+                 temp_lane.waypoints.at(j + 1).twist.twist.linear.x) /
+                3;
+          }
+        }
+      }
+    }
+
+    lane = apply_crossroad_acceleration(lane, config_acceleration);
+
+    traffic_waypoint.lanes.push_back(lane);
+    green_waypoint.lanes.push_back(lane);
+
+    lane = apply_stopline_acceleration(lane, config_acceleration, fine_vmap, config_number_of_zeros_ahead,
+                                       config_number_of_zeros_behind);
+
+    red_waypoint.lanes.push_back(lane);
+
+#ifdef DEBUG
+    std::stringstream ss;
+    ss << "_" << i;
+
+    visualization_msgs::Marker m = debug_marker;
+    m.ns = "lane" + ss.str();
+    m.color = create_color(i);
+
+    lane_planner::vmap::publish_add_marker(marker_pub, m, fine_vmap.points);
+#endif  // DEBUG
+  }
+
+  traffic_pub.publish(traffic_waypoint);
+  red_pub.publish(red_waypoint);
+  green_pub.publish(green_waypoint);
+}
+
+void update_values()
+{
+  if (all_vmap.points.empty() || all_vmap.lanes.empty() || all_vmap.nodes.empty() || all_vmap.stoplines.empty() ||
+      all_vmap.dtlanes.empty())
+    return;
+
+  lane_vmap = lane_planner::vmap::create_lane_vmap(all_vmap, lane_planner::vmap::LNO_ALL);
+
+  curve_radius_min = lane_planner::vmap::RADIUS_MAX;
+  crossroad_radius_min = lane_planner::vmap::RADIUS_MAX;
+  clothoid_radius_min = lane_planner::vmap::RADIUS_MAX;
+  for (const vector_map::DTLane& d : lane_vmap.dtlanes)
+  {
+    double radius_min = fabs(d.r);
+    if (lane_planner::vmap::is_curve_dtlane(d))
+    {
+      if (lane_planner::vmap::is_crossroad_dtlane(d))
+      {
+        if (radius_min < crossroad_radius_min)
+          crossroad_radius_min = radius_min;
+      }
+      else
+      {
+        if (radius_min < curve_radius_min)
+          curve_radius_min = radius_min;
+      }
+    }
+    else if (lane_planner::vmap::is_clothoid_dtlane(d))
+    {
+      if (radius_min < clothoid_radius_min)
+        clothoid_radius_min = radius_min;
+    }
+  }
+
+#ifdef DEBUG
+  for (int i = 0; i < marker_cnt; ++i)
+  {
+    std::stringstream ss;
+    ss << "_" << i;
+
+    visualization_msgs::Marker m = debug_marker;
+    m.ns = "lane" + ss.str();
+
+    lane_planner::vmap::publish_delete_marker(marker_pub, m);
+  }
+  marker_cnt = 0;
+#endif  // DEBUG
+
+  if (!cached_waypoint.lanes.empty())
+  {
+    autoware_msgs::LaneArray update_waypoint = cached_waypoint;
+    create_waypoint(update_waypoint);
+  }
+}
+
+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();
+}
+
+void cache_stopline(const vector_map::StopLineArray& msg)
+{
+  all_vmap.stoplines = msg.data;
+  update_values();
+}
+
+void cache_dtlane(const vector_map::DTLaneArray& msg)
+{
+  all_vmap.dtlanes = msg.data;
+  update_values();
+}
+
+void config_parameter(const autoware_config_msgs::ConfigLaneRule& msg)
+{
+  config_acceleration = msg.acceleration;
+  config_stopline_search_radius = msg.stopline_search_radius;
+  config_number_of_zeros_ahead = msg.number_of_zeros_ahead;
+  config_number_of_zeros_behind = msg.number_of_zeros_behind;
+  config_number_of_smoothing_count = msg.number_of_smoothing_count;
+
+  if (!cached_waypoint.lanes.empty())
+  {
+    autoware_msgs::LaneArray update_waypoint = cached_waypoint;
+    create_waypoint(update_waypoint);
+  }
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "lane_rule");
+
+  ros::NodeHandle n;
+  ros::NodeHandle pnh("~");
+
+  int sub_vmap_queue_size;
+  pnh.param<int>("sub_vmap_queue_size", sub_vmap_queue_size, 1);
+  int sub_waypoint_queue_size;
+  pnh.param<int>("sub_waypoint_queue_size", sub_waypoint_queue_size, 1);
+  int sub_config_queue_size;
+  pnh.param<int>("sub_config_queue_size", sub_config_queue_size, 1);
+  int pub_waypoint_queue_size;
+  pnh.param<int>("pub_waypoint_queue_size", pub_waypoint_queue_size, 1);
+  bool pub_waypoint_latch;
+  pnh.param<bool>("pub_waypoint_latch", pub_waypoint_latch, true);
+#ifdef DEBUG
+  int pub_marker_queue_size;
+  pnh.param<int>("pub_marker_queue_size", pub_marker_queue_size, 10);
+  bool pub_marker_latch;
+  pnh.param<bool>("pub_marker_latch", pub_marker_latch, true);
+#endif  // DEBUG
+
+  pnh.param<int>("waypoint_max", waypoint_max, 10000);
+  pnh.param<double>("search_radius", search_radius, 10);
+  pnh.param<double>("curve_weight", curve_weight, 0.6);
+  pnh.param<double>("crossroad_weight", crossroad_weight, 0.9);
+  pnh.param<double>("clothoid_weight", clothoid_weight, 0.215);
+  pnh.param<std::string>("frame_id", frame_id, "map");
+  pnh.param<double>("acceleration", config_acceleration, 1);
+  pnh.param<int>("number_of_smoothing_count", config_number_of_smoothing_count, 0);
+  pnh.param<int>("number_of_zeros_ahead", config_number_of_zeros_ahead, 0);
+  pnh.param<int>("number_of_zeros_behind", config_number_of_zeros_behind, 0);
+  pnh.param<double>("stopline_search_radius", config_stopline_search_radius, 1);
+
+  traffic_pub =
+      n.advertise<autoware_msgs::LaneArray>("/traffic_waypoints_array", pub_waypoint_queue_size, pub_waypoint_latch);
+  red_pub = n.advertise<autoware_msgs::LaneArray>("/red_waypoints_array", pub_waypoint_queue_size, pub_waypoint_latch);
+  green_pub =
+      n.advertise<autoware_msgs::LaneArray>("/green_waypoints_array", pub_waypoint_queue_size, pub_waypoint_latch);
+
+#ifdef DEBUG
+  debug_marker.header.frame_id = frame_id;
+  debug_marker.id = 0;
+  debug_marker.type = visualization_msgs::Marker::LINE_STRIP;
+  debug_marker.scale.x = 0.2;
+  debug_marker.scale.y = 0.2;
+  debug_marker.frame_locked = true;
+
+  marker_pub = n.advertise<visualization_msgs::Marker>("/waypoint_debug", pub_marker_queue_size, pub_marker_latch);
+#endif  // DEBUG
+
+  ros::Subscriber waypoint_sub = n.subscribe("/lane_waypoints_array", sub_waypoint_queue_size, create_waypoint);
+  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::Subscriber stopline_sub = n.subscribe("/vector_map_info/stop_line", sub_vmap_queue_size, cache_stopline);
+  ros::Subscriber dtlane_sub = n.subscribe("/vector_map_info/dtlane", sub_vmap_queue_size, cache_dtlane);
+  ros::Subscriber config_sub = n.subscribe("/config/lane_rule", sub_config_queue_size, config_parameter);
+
+  ros::spin();
+
+  return EXIT_SUCCESS;
+}

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

@@ -0,0 +1,475 @@
+/*
+ * Copyright 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.
+ *
+ * Authors: Simon Thompson
+ *
+ */
+
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include <tf/transform_listener.h>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_core/geometry/Point.h>
+#include <lanelet2_routing/RoutingGraphContainer.h>
+#include <lanelet2_traffic_rules/TrafficRules.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#include <lanelet2_extension/utility/message_conversion.h>
+#include <lanelet2_extension/utility/query.h>
+
+#include <string>
+#include <vector>
+
+#include <autoware_config_msgs/ConfigLaneRule.h>
+#include <autoware_lanelet2_msgs/MapBin.h>
+#include <autoware_msgs/Lane.h>
+#include <autoware_msgs/LaneArray.h>
+#include <autoware_msgs/Waypoint.h>
+
+// use of routing graph in detecting lanelets for each waypoint
+// folowing routing graph may be computationally cheaper if very large map
+// (computation cost may be in unique insert used to store connected lanelets)
+// for smaller maps - faster to just find nearest lanelet for each waypoint.
+#define LANE_RULES_USE_ROUTING_GRAPH false
+
+static double g_config_acceleration = 1;            // m/s^2
+static double g_config_stopline_search_radius = 1;  // meter
+static int g_config_number_of_zeros_ahead = 0;
+static int g_config_number_of_zeros_behind = 0;
+static int g_config_number_of_smoothing_count = 0;
+
+static std::string g_frame_id = "map";
+
+static ros::Publisher g_traffic_pub;
+static ros::Publisher g_red_pub;
+static ros::Publisher g_green_pub;
+static autoware_msgs::LaneArray g_cached_waypoint;
+
+static lanelet::LaneletMapPtr g_lanelet_map;
+static lanelet::routing::RoutingGraphPtr g_routing_graph;
+static bool g_loaded_lanelet_map = false;
+
+// create new lane with given lane and header
+autoware_msgs::Lane create_new_lane(const autoware_msgs::Lane& lane, const std_msgs::Header& header)
+{
+  autoware_msgs::Lane l = lane;
+  l.header = header;
+
+  for (autoware_msgs::Waypoint& w : l.waypoints)
+  {
+    w.pose.header = header;
+    w.twist.header = header;
+  }
+
+  return l;
+}
+
+// create array of waypoints in Eigen Vector3d type
+void create_waypoint_array(const autoware_msgs::Lane& lane, std::vector<Eigen::Vector3d>* waypoints)
+{
+  if (waypoints == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": waypoints is null pointer!");
+    return;
+  }
+
+  waypoints->clear();
+  for (const autoware_msgs::Waypoint& w : lane.waypoints)
+  {
+    geometry_msgs::Point gp = w.pose.pose.position;
+    Eigen::Vector3d p(gp.x, gp.y, gp.z);
+
+    waypoints->push_back(p);
+  }
+}
+
+// find nearest lanelets to a given point
+lanelet::Lanelets find_nearest_lanelets(const Eigen::Vector3d& p, int n)
+{
+  return (g_lanelet_map->laneletLayer.nearest(lanelet::BasicPoint2d(p.x(), p.y()), n));
+}
+
+// find nearest lanelet to a given point
+lanelet::Lanelet find_actual_nearest_lanelet(const Eigen::Vector3d& p)
+{
+  std::vector<std::pair<double, lanelet::Lanelet> > actual_nearest_lls =
+      lanelet::geometry::findNearest(g_lanelet_map->laneletLayer, lanelet::BasicPoint2d(p.x(), p.y()), 1);
+
+  if (actual_nearest_lls.empty())
+  {
+    lanelet::Lanelet empty_lanelet;
+    return empty_lanelet;
+  }
+  else
+  {
+    return actual_nearest_lls.front().second;
+  }
+}
+
+void insert_unique_lanelet(const lanelet::ConstLanelet& ll, lanelet::ConstLanelets* lls)
+{
+  if (lls == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": lls is null pointer!");
+    return;
+  }
+  if (std::find(lls->begin(), lls->end(), ll) == lls->end())
+    lls->push_back(ll);
+}
+
+void add_connecting_lanelets(const lanelet::ConstLanelet& lanelet, lanelet::ConstLanelets* candidate_lanelets)
+{
+  if (candidate_lanelets == nullptr)
+  {
+    ROS_ERROR_STREAM(__FUNCTION__ << ": candidate_lanelets is null pointer!");
+    return;
+  }
+
+  lanelet::Optional<lanelet::ConstLanelet> ll_opt;
+
+  ll_opt = g_routing_graph->right(lanelet);
+  if (!!ll_opt)
+    insert_unique_lanelet(ll_opt.get(), candidate_lanelets);
+
+  ll_opt = g_routing_graph->left(lanelet);
+  if (!!ll_opt)
+    insert_unique_lanelet(ll_opt.get(), candidate_lanelets);
+
+  lanelet::ConstLanelets following_ll = g_routing_graph->following(lanelet);
+  for (auto fll_i = following_ll.begin(); fll_i < following_ll.end(); fll_i++)
+    insert_unique_lanelet((*fll_i), candidate_lanelets);
+}
+
+std::vector<size_t> check_waypoints_for_stoplines(const std::vector<Eigen::Vector3d>& waypoints)
+{
+  lanelet::ConstLanelet current_lanelet = find_actual_nearest_lanelet(waypoints.front());
+  lanelet::ConstLanelets current_lanelets;
+  current_lanelets.push_back(current_lanelet);
+  std::vector<size_t> waypoint_stopline_indexes;
+
+  size_t wp_index = 0;
+
+  // TODO:
+  // perhaps need to test boundary condition when waypoint is last in lanelet?
+  for (auto wp_i = waypoints.begin(); wp_i < waypoints.end() - 1; wp_i++)
+  {
+    lanelet::Point3d wp_p0(lanelet::utils::getId(), (*wp_i).x(), (*wp_i).y(), (*wp_i).z());
+    lanelet::Point3d wp_p1(lanelet::utils::getId(),(*(wp_i + 1)).x(), (*(wp_i + 1)).y(), (*(wp_i + 1)).z());
+    lanelet::BasicPoint2d wp_p02((*wp_i).x(), (*wp_i).y());
+    lanelet::ConstLanelets candidate_lanelets;
+
+    // check if waypoint is in current lanelets - if not discard
+    int ll_count = 0;
+
+    // if no current lanelets - search again
+    if (current_lanelets.size() == 0)
+    {
+      current_lanelets.push_back(find_actual_nearest_lanelet(*wp_i));
+    }
+
+    for (auto curr_i = current_lanelets.begin(); curr_i < current_lanelets.end();)
+    {
+      current_lanelet = (*curr_i);
+      ll_count++;
+
+      if (!lanelet::geometry::within(wp_p02, lanelet::utils::toHybrid(current_lanelet.polygon2d())))
+      {
+        current_lanelets.erase(curr_i);
+      }
+      else
+      {
+        std::vector<lanelet::ConstLineString3d> current_lanelet_stoplines =
+            lanelet::utils::query::getTrafficLightStopLines(current_lanelet);
+
+        // check if waypoint segment intersects with stoplines
+        // kinda awkward to force use of boost::geometry intersect.
+        // 3d line segment intersection not implememented
+        lanelet::ConstLineString3d wp_ls(lanelet::utils::getId(), { wp_p0, wp_p1 });
+        auto wp_ls2d = lanelet::utils::to2D(wp_ls);
+
+        for (auto sl_i = current_lanelet_stoplines.begin(); sl_i < current_lanelet_stoplines.end(); sl_i++)
+        {
+          auto sl_ls2d = lanelet::utils::to2D(*sl_i);
+          if (lanelet::geometry::intersects(lanelet::utils::toHybrid(wp_ls2d), lanelet::utils::toHybrid(sl_ls2d)))
+          {
+            waypoint_stopline_indexes.push_back(wp_index);
+          }
+        }
+
+        // build up a list of possible connecting lanelets that might contain current waypoint
+        if (LANE_RULES_USE_ROUTING_GRAPH)
+          add_connecting_lanelets(current_lanelet, &candidate_lanelets);
+
+        // increment iterator here, erase call increments if wp not in current lanelet
+        curr_i++;
+      }
+    }
+
+    // add candidate lanelets to current for next waypoint (in case path moves to next lanelets)
+    if (LANE_RULES_USE_ROUTING_GRAPH)
+    {
+      for (auto cand_i = candidate_lanelets.begin(); cand_i < candidate_lanelets.end(); cand_i++)
+      {
+        insert_unique_lanelet((*cand_i), &current_lanelets);
+      }
+    }
+
+    wp_index++;
+  }  // for each waypoint
+
+  return waypoint_stopline_indexes;
+}
+
+// apply acceleration from given starting point
+// same as lane_rule.cpp
+autoware_msgs::Lane apply_acceleration(const autoware_msgs::Lane& lane, double acceleration, size_t start_index,
+                                       size_t fixed_cnt, double fixed_vel)
+{
+  autoware_msgs::Lane l = lane;
+
+  if (fixed_cnt == 0)
+    return l;
+
+  double square_vel = fixed_vel * fixed_vel;
+  double distance = 0;
+  for (size_t i = start_index; i < l.waypoints.size(); ++i)
+  {
+    if (i - start_index < fixed_cnt)
+    {
+      l.waypoints[i].twist.twist.linear.x = fixed_vel;
+      continue;
+    }
+    geometry_msgs::Point a = l.waypoints[i - 1].pose.pose.position;
+    geometry_msgs::Point b = l.waypoints[i].pose.pose.position;
+    distance += hypot(b.x - a.x, b.y - a.y);
+
+    const int sgn = (l.waypoints[i].twist.twist.linear.x < 0.0) ? -1 : 1;
+
+    double v = sqrt(square_vel + 2 * acceleration * distance);
+    if (v < l.waypoints[i].twist.twist.linear.x)
+    {
+      l.waypoints[i].twist.twist.linear.x = sgn * v;
+    }
+    else
+      break;
+  }
+
+  return l;
+}
+
+// apply deceleration before stopline and acceleration after stopline
+// // same as lane_rule.cpp
+autoware_msgs::Lane apply_stopline_acceleration(const autoware_msgs::Lane& lane,
+                                                const std::vector<size_t>& stopline_indexes, double acceleration,
+                                                size_t ahead_cnt, size_t behind_cnt)
+{
+  autoware_msgs::Lane l = lane;
+  if (stopline_indexes.empty())
+    return l;
+
+  for (const size_t i : stopline_indexes)
+    l = apply_acceleration(l, acceleration, i, behind_cnt + 1, 0);
+
+  // reverse in order to apply deceleration using apply_acceleration function
+  std::reverse(l.waypoints.begin(), l.waypoints.end());
+
+  std::vector<size_t> reverse_indexes;
+  for (const size_t i : stopline_indexes)
+    reverse_indexes.push_back(l.waypoints.size() - i - 1);
+  std::reverse(reverse_indexes.begin(), reverse_indexes.end());
+
+  for (const size_t i : reverse_indexes)
+    l = apply_acceleration(l, acceleration, i, ahead_cnt + 1, 0);
+
+  // undo reverse
+  std::reverse(l.waypoints.begin(), l.waypoints.end());
+
+  return l;
+}
+
+// create traffic_waypoint, red_waypoint, and green_waypoint
+void create_waypoint(const autoware_msgs::LaneArray& msg)
+{
+  std_msgs::Header header;
+  header.stamp = ros::Time::now();
+  header.frame_id = g_frame_id;
+  g_cached_waypoint.lanes.clear();
+  g_cached_waypoint.lanes.shrink_to_fit();
+  g_cached_waypoint.id = msg.id;
+
+  // update cache
+  for (const autoware_msgs::Lane& l : msg.lanes)
+    g_cached_waypoint.lanes.push_back(create_new_lane(l, header));
+
+  // check validity of lanelet_map
+  if (!g_loaded_lanelet_map || !g_lanelet_map || g_lanelet_map->laneletLayer.empty() ||
+      g_lanelet_map->pointLayer.empty() || g_lanelet_map->regulatoryElementLayer.empty())
+  {
+    g_traffic_pub.publish(g_cached_waypoint);
+    return;
+  }
+
+  autoware_msgs::LaneArray traffic_waypoint;
+  autoware_msgs::LaneArray red_waypoint;
+  autoware_msgs::LaneArray green_waypoint;
+  traffic_waypoint.id = red_waypoint.id = green_waypoint.id = msg.id;
+  // for each lane (lane is a collection of waypoints
+  for (size_t i = 0; i < msg.lanes.size(); ++i)
+  {
+    autoware_msgs::Lane waypoint_lane = create_new_lane(msg.lanes[i], header);
+
+    // choose to use vectors -> perhaps a lanelet data type better?
+    std::vector<Eigen::Vector3d> waypoints;
+    create_waypoint_array(waypoint_lane, &waypoints);
+    if (waypoints.size() < 2)
+    {
+      traffic_waypoint.lanes.push_back(waypoint_lane);
+      continue;
+    }
+
+    lanelet::Lanelets nearest_lanelets = find_nearest_lanelets(waypoints.front(), 1);
+    if (nearest_lanelets.size() <= 0)
+    {
+      traffic_waypoint.lanes.push_back(waypoint_lane);
+      continue;
+    }
+
+    // velocity smoothing
+    for (int k = 0; k < g_config_number_of_smoothing_count; ++k)
+    {
+      autoware_msgs::Lane temp_lane = waypoint_lane;
+      if (waypoint_lane.waypoints.size() >= 3)
+      {
+        for (size_t j = 1; j < waypoint_lane.waypoints.size() - 1; ++j)
+        {
+          if (waypoint_lane.waypoints.at(j).twist.twist.linear.x != 0)
+          {
+            waypoint_lane.waypoints[j].twist.twist.linear.x =
+                (temp_lane.waypoints.at(j - 1).twist.twist.linear.x + temp_lane.waypoints.at(j).twist.twist.linear.x +
+                 temp_lane.waypoints.at(j + 1).twist.twist.linear.x) /
+                3;
+          }
+        }
+      }
+    }
+
+    // check if any waypoint segments are intersected by stoplines
+    std::vector<size_t> waypoint_stopline_indexes = check_waypoints_for_stoplines(waypoints);
+
+    traffic_waypoint.lanes.push_back(waypoint_lane);
+    green_waypoint.lanes.push_back(waypoint_lane);
+
+    // apply acceleration to waypoint velocities to consider stoplines
+    waypoint_lane = apply_stopline_acceleration(waypoint_lane, waypoint_stopline_indexes, g_config_acceleration,
+                                                g_config_number_of_zeros_ahead, g_config_number_of_zeros_behind);
+
+    red_waypoint.lanes.push_back(waypoint_lane);
+  }
+
+  // publish traffic waypoint
+  g_traffic_pub.publish(traffic_waypoint);
+  g_red_pub.publish(red_waypoint);
+  g_green_pub.publish(green_waypoint);
+}
+
+// callback for lanelet2 map.
+void binMapCallback(const autoware_lanelet2_msgs::MapBin& msg)
+{
+  g_lanelet_map = std::make_shared<lanelet::LaneletMap>();
+
+  lanelet::utils::conversion::fromBinMsg(msg, g_lanelet_map);
+  g_loaded_lanelet_map = true;
+  ROS_INFO("loaded lanelet map\n");
+
+  if (!g_cached_waypoint.lanes.empty())
+  {
+    autoware_msgs::LaneArray update_waypoint = g_cached_waypoint;
+    create_waypoint(update_waypoint);
+  }
+}
+
+// callback for config topic
+void config_parameter(const autoware_config_msgs::ConfigLaneRule& msg)
+{
+  g_config_acceleration = msg.acceleration;
+  g_config_stopline_search_radius = msg.stopline_search_radius;
+  g_config_number_of_zeros_ahead = msg.number_of_zeros_ahead;
+  g_config_number_of_zeros_behind = msg.number_of_zeros_behind;
+  g_config_number_of_smoothing_count = msg.number_of_smoothing_count;
+
+  if (!g_cached_waypoint.lanes.empty())
+  {
+    autoware_msgs::LaneArray update_waypoint = g_cached_waypoint;
+    create_waypoint(update_waypoint);
+  }
+}
+
+//-------------------------------------------------------------------------
+// follows logic of vector map based implmenentation
+// but ignores acceleration adjustment for cross roads
+// and does not reduce velocity for curvature
+// assumed that this is implemented in local planner before
+// path is published
+// -> do only stopline acceleration control
+//-------------------------------------------------------------------------
+int main(int argc, char** argv)
+{
+  g_loaded_lanelet_map = false;
+
+  ros::init(argc, argv, "lane_rule");
+  ros::NodeHandle rosnode;
+  ros::NodeHandle pnh("~");
+
+  int sub_waypoint_queue_size;
+  pnh.param<int>("sub_waypoint_queue_size", sub_waypoint_queue_size, 1);
+  int sub_config_queue_size;
+  pnh.param<int>("sub_config_queue_size", sub_config_queue_size, 1);
+  int pub_waypoint_queue_size;
+  pnh.param<int>("pub_waypoint_queue_size", pub_waypoint_queue_size, 1);
+  bool pub_waypoint_latch;
+  pnh.param<bool>("pub_waypoint_latch", pub_waypoint_latch, true);
+
+  pnh.param<std::string>("frame_id", g_frame_id, "map");
+  pnh.param<double>("acceleration", g_config_acceleration, 1);
+  pnh.param<int>("number_of_smoothing_count", g_config_number_of_smoothing_count, 0);
+  pnh.param<int>("number_of_zeros_ahead", g_config_number_of_zeros_ahead, 0);
+  pnh.param<int>("number_of_zeros_behind", g_config_number_of_zeros_behind, 0);
+
+  g_traffic_pub = rosnode.advertise<autoware_msgs::LaneArray>("traffic_waypoints_array", pub_waypoint_queue_size,
+                                                              pub_waypoint_latch);
+  g_red_pub =
+      rosnode.advertise<autoware_msgs::LaneArray>("red_waypoints_array", pub_waypoint_queue_size, pub_waypoint_latch);
+  g_green_pub = rosnode.advertise<autoware_msgs::LaneArray>("green_waypoints_array", pub_waypoint_queue_size,
+                                                            pub_waypoint_latch);
+
+  ros::Subscriber bin_map_sub = rosnode.subscribe("lanelet_map_bin", 10, binMapCallback);
+  ros::Subscriber waypoint_sub = rosnode.subscribe("lane_waypoints_array", sub_waypoint_queue_size, create_waypoint);
+  ros::Subscriber config_sub = rosnode.subscribe("config/lane_rule", sub_config_queue_size, config_parameter);
+
+  if (LANE_RULES_USE_ROUTING_GRAPH)
+  {
+    lanelet::traffic_rules::TrafficRulesPtr traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
+        lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+    g_routing_graph = lanelet::routing::RoutingGraph::build(*g_lanelet_map, *traffic_rules);
+  }
+
+  ros::spin();
+
+  return 0;
+}

src/ros/catkin/src/lane_planner/nodes/lane_select/hermite_curve.cpp

@@ -0,0 +1,119 @@
+/*
+ * 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 "hermite_curve.h"
+
+namespace lane_planner
+{
+void createVectorFromPose(const geometry_msgs::Pose &p, tf::Vector3 *v)
+{
+  tf::Transform pose;
+  tf::poseMsgToTF(p, pose);
+  tf::Vector3 x_axis(1, 0, 0);
+  *v = pose.getBasis() * x_axis;
+}
+
+void getPointAndVectorFromPose(const geometry_msgs::Pose &pose, Element2D *point, Element2D *vector)
+{
+  point->set(pose.position.x, pose.position.y);
+
+  tf::Vector3 tmp_tf_vevtor;
+  createVectorFromPose(pose, &tmp_tf_vevtor);
+  vector->set(tmp_tf_vevtor.getX(), tmp_tf_vevtor.getY());
+}
+
+std::vector<autoware_msgs::Waypoint> generateHermiteCurveForROS(const geometry_msgs::Pose &start,
+                                                                const geometry_msgs::Pose &end,
+                                                                const double velocity_mps, const double vlength)
+{
+  std::vector<autoware_msgs::Waypoint> wps;
+  Element2D p0(0, 0), v0(0, 0), p1(0, 0), v1(0, 0);
+  getPointAndVectorFromPose(start, &p0, &v0);
+  getPointAndVectorFromPose(end, &p1, &v1);
+
+  std::vector<Element2D> result = generateHermiteCurve(p0, v0, p1, v1, vlength);
+
+  double height_d = fabs(start.position.z - end.position.z);
+  for (uint32_t i = 0; i < result.size(); i++)
+  {
+    autoware_msgs::Waypoint wp;
+    wp.pose.pose.position.x = result.at(i).x;
+    wp.pose.pose.position.y = result.at(i).y;
+    wp.twist.twist.linear.x = velocity_mps;
+
+    // height
+    wp.pose.pose.position.z =
+        (i == 0) ? start.position.z : (i == result.size() - 1) ? end.position.z : start.position.z < end.position.z ?
+                                                                 start.position.z + height_d * i / result.size() :
+                                                                 start.position.z - height_d * i / result.size();
+
+    // orientation
+    if (i != result.size() - 1)
+    {
+      double radian = atan2(result.at(i + 1).y - result.at(i).y, result.at(i + 1).x - result.at(i).x);
+      wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(radian);
+    }
+    else
+    {
+      wp.pose.pose.orientation = wps.at(wps.size() - 1).pose.pose.orientation;
+    }
+
+    wps.push_back(wp);
+  }
+  return wps;
+}
+
+std::vector<Element2D> generateHermiteCurve(const Element2D &p0, const Element2D &v0, const Element2D &p1,
+                                            const Element2D &v1, const double vlength)
+{
+  std::vector<Element2D> result;
+  const double interval = 1.0;
+  int32_t divide = 2;
+  const int32_t loop = 100;
+  while (divide < loop)
+  {
+    result.reserve(divide);
+    for (int32_t i = 0; i < divide; i++)
+    {
+      double u = i * 1.0 / (divide - 1);
+      double u_square = pow(u, 2);
+      double u_cube = pow(u, 3);
+      double coeff_p0 = 2 * u_cube - 3 * u_square + 1;
+      double coeff_v0 = u_cube - 2 * u_square + u;
+      double coeff_p1 = (-1) * 2 * u_cube + 3 * u_square;
+      double coeff_v1 = u_cube - u_square;
+      // printf("u: %lf, u^2: %lf, u^3: %lf, coeff_p0: %lf, coeff_v0: %lf, coeff_p1: %lf, coeff_v1: %lf\n", u, u_square,
+      // u_cube, coeff_p0, coeff_p1, coeff_v0, coeff_v1);
+      result.push_back(
+          Element2D((p0.x * coeff_p0 + vlength * v0.x * coeff_v0 + p1.x * coeff_p1 + vlength * v1.x * coeff_v1),
+                    (p0.y * coeff_p0 + vlength * v0.y * coeff_v0 + p1.y * coeff_p1 + vlength * v1.y * coeff_v1)));
+    }
+
+    double dt = sqrt(pow((result.at(divide / 2 - 1).x - result.at(divide / 2).x), 2) +
+                     pow((result.at(divide / 2 - 1).y - result.at(divide / 2).y), 2));
+    std::cout << "interval : " << dt << std::endl;
+    if (interval > dt || divide == loop - 1)
+      return result;
+    else
+    {
+      result.clear();
+      result.shrink_to_fit();
+      divide++;
+    }
+  }
+  return result;
+}
+}  // namespace

src/ros/catkin/src/lane_planner/nodes/lane_select/lane_select_core.cpp

@@ -0,0 +1,865 @@
+/*
+ * 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 "lane_select_core.h"
+
+#include <algorithm>
+
+namespace lane_planner
+{
+// Constructor
+LaneSelectNode::LaneSelectNode()
+  : private_nh_("~")
+  , lane_array_id_(-1)
+  , current_lane_idx_(-1)
+  , right_lane_idx_(-1)
+  , left_lane_idx_(-1)
+  , is_lane_array_subscribed_(false)
+  , is_current_pose_subscribed_(false)
+  , is_current_velocity_subscribed_(false)
+  , is_current_state_subscribed_(false)
+  , is_config_subscribed_(false)
+  , distance_threshold_(3.0)
+  , lane_change_interval_(10.0)
+  , lane_change_target_ratio_(2.0)
+  , lane_change_target_minimum_(5.0)
+  , vlength_hermite_curve_(10)
+  , search_closest_waypoint_minimum_dt_(5)
+  , current_state_("UNKNOWN")
+{
+  initForROS();
+}
+
+// Destructor
+LaneSelectNode::~LaneSelectNode()
+{
+}
+
+void LaneSelectNode::initForROS()
+{
+  // setup subscriber
+  sub1_ = nh_.subscribe("traffic_waypoints_array", 1, &LaneSelectNode::callbackFromLaneArray, this);
+  sub2_ = nh_.subscribe("current_pose", 1, &LaneSelectNode::callbackFromPoseStamped, this);
+  sub3_ = nh_.subscribe("current_velocity", 1, &LaneSelectNode::callbackFromTwistStamped, this);
+  sub4_ = nh_.subscribe("state", 1, &LaneSelectNode::callbackFromState, this);
+  sub5_ = nh_.subscribe("/config/lane_select", 1, &LaneSelectNode::callbackFromConfig, this);
+  sub6_ = nh_.subscribe("/decision_maker/state", 1, &LaneSelectNode::callbackFromDecisionMakerState, this);
+
+  // setup publisher
+
+  pub1_ = nh_.advertise<autoware_msgs::Lane>("base_waypoints", 1);
+  pub2_ = nh_.advertise<std_msgs::Int32>("closest_waypoint", 1);
+  pub3_ = nh_.advertise<std_msgs::Int32>("change_flag", 1);
+  pub4_ = nh_.advertise<std_msgs::Int32>("/current_lane_id", 1);
+  pub5_ = nh_.advertise<autoware_msgs::VehicleLocation>("vehicle_location", 1);
+
+  vis_pub1_ = nh_.advertise<visualization_msgs::MarkerArray>("lane_select_marker", 1);
+
+  // get from rosparam
+  private_nh_.param<double>("lane_change_interval", lane_change_interval_, double(2));
+  private_nh_.param<double>("distance_threshold", distance_threshold_, double(3.0));
+  private_nh_.param<int>("search_closest_waypoint_minimum_dt", search_closest_waypoint_minimum_dt_, int(5));
+  private_nh_.param<double>("lane_change_target_ratio", lane_change_target_ratio_, double(2.0));
+  private_nh_.param<double>("lane_change_target_minimum", lane_change_target_minimum_, double(5.0));
+  private_nh_.param<double>("vector_length_hermite_curve", vlength_hermite_curve_, double(10.0));
+}
+
+bool LaneSelectNode::isAllTopicsSubscribed()
+{
+  if (!is_current_pose_subscribed_ || !is_lane_array_subscribed_ || !is_current_velocity_subscribed_)
+  {
+    ROS_WARN("Necessary topics are not subscribed yet. Waiting...");
+    return false;
+  }
+  return true;
+}
+
+void LaneSelectNode::initForLaneSelect()
+{
+  if (!isAllTopicsSubscribed())
+    return;
+
+  // search closest waypoint number for each lanes
+  if (!getClosestWaypointNumberForEachLanes())
+  {
+    publishClosestWaypoint(-1);
+    publishVehicleLocation(-1, lane_array_id_);
+    resetLaneIdx();
+    return;
+  }
+
+  findCurrentLane();
+  findNeighborLanes();
+  updateChangeFlag();
+  createLaneForChange();
+
+  publishLane(std::get<0>(tuple_vec_.at(current_lane_idx_)));
+  publishClosestWaypoint(std::get<1>(tuple_vec_.at(current_lane_idx_)));
+  publishChangeFlag(std::get<2>(tuple_vec_.at(current_lane_idx_)));
+  publishVehicleLocation(std::get<1>(tuple_vec_.at(current_lane_idx_)), lane_array_id_);
+  publishVisualizer();
+
+  resetSubscriptionFlag();
+  return;
+}
+
+void LaneSelectNode::resetLaneIdx()
+{
+  current_lane_idx_ = -1;
+  right_lane_idx_ = -1;
+  left_lane_idx_ = -1;
+  publishVisualizer();
+}
+
+void LaneSelectNode::resetSubscriptionFlag()
+{
+  is_current_pose_subscribed_ = false;
+  is_current_velocity_subscribed_ = false;
+  is_current_state_subscribed_ = false;
+}
+
+void LaneSelectNode::processing()
+{
+  if (!isAllTopicsSubscribed())
+    return;
+
+  // search closest waypoint number for each lanes
+  if (!getClosestWaypointNumberForEachLanes())
+  {
+    publishClosestWaypoint(-1);
+    publishVehicleLocation(-1, lane_array_id_);
+    resetLaneIdx();
+    return;
+  }
+
+  // if closest waypoint on current lane is -1,
+  if (std::get<1>(tuple_vec_.at(current_lane_idx_)) == -1)
+  {
+    publishClosestWaypoint(-1);
+    publishVehicleLocation(-1, lane_array_id_);
+    resetLaneIdx();
+    return;
+  }
+
+  findNeighborLanes();
+  ROS_INFO("current_lane_idx: %d", current_lane_idx_);
+  ROS_INFO("right_lane_idx: %d", right_lane_idx_);
+  ROS_INFO("left_lane_idx: %d", left_lane_idx_);
+
+  if (current_state_ == "LANE_CHANGE")
+  {
+    try
+    {
+      changeLane();
+      std::get<1>(lane_for_change_) =
+          getClosestWaypointNumber(std::get<0>(lane_for_change_), current_pose_.pose, current_velocity_.twist,
+                                   std::get<1>(lane_for_change_), distance_threshold_, search_closest_waypoint_minimum_dt_);
+      std::get<2>(lane_for_change_) = static_cast<ChangeFlag>(
+          std::get<0>(lane_for_change_).waypoints.at(std::get<1>(lane_for_change_)).change_flag);
+      ROS_INFO("closest: %d", std::get<1>(lane_for_change_));
+      publishLane(std::get<0>(lane_for_change_));
+      publishLaneID(std::get<0>(lane_for_change_));
+      publishClosestWaypoint(std::get<1>(lane_for_change_));
+      publishChangeFlag(std::get<2>(lane_for_change_));
+      publishVehicleLocation(std::get<1>(lane_for_change_), lane_array_id_);
+    }
+    catch (std::out_of_range)
+    {
+      ROS_WARN("Failed to get closest waypoint num\n");
+    }
+  }
+  else
+  {
+    updateChangeFlag();
+    createLaneForChange();
+
+    publishLane(std::get<0>(tuple_vec_.at(current_lane_idx_)));
+    publishClosestWaypoint(std::get<1>(tuple_vec_.at(current_lane_idx_)));
+    publishChangeFlag(std::get<2>(tuple_vec_.at(current_lane_idx_)));
+    publishVehicleLocation(std::get<1>(tuple_vec_.at(current_lane_idx_)), lane_array_id_);
+  }
+  publishVisualizer();
+  resetSubscriptionFlag();
+}
+
+int32_t LaneSelectNode::getClosestLaneChangeWaypointNumber(const std::vector<autoware_msgs::Waypoint> &wps,
+                                                           int32_t cl_wp)
+{
+  for (uint32_t i = cl_wp; i < wps.size(); i++)
+  {
+    if (static_cast<ChangeFlag>(wps.at(i).change_flag) == ChangeFlag::right ||
+        static_cast<ChangeFlag>(wps.at(i).change_flag) == ChangeFlag::left)
+    {
+      return i;
+    }
+  }
+  return -1;
+}
+
+void LaneSelectNode::createLaneForChange()
+{
+  std::get<0>(lane_for_change_).waypoints.clear();
+  std::get<0>(lane_for_change_).waypoints.shrink_to_fit();
+  std::get<1>(lane_for_change_) = -1;
+
+  const autoware_msgs::Lane &cur_lane = std::get<0>(tuple_vec_.at(current_lane_idx_));
+  const int32_t &clst_wp = std::get<1>(tuple_vec_.at(current_lane_idx_));
+
+  int32_t num_lane_change = getClosestLaneChangeWaypointNumber(cur_lane.waypoints, clst_wp);
+  ROS_INFO("num_lane_change: %d", num_lane_change);
+  if (num_lane_change < 0 || num_lane_change >= static_cast<int32_t>(cur_lane.waypoints.size()))
+  {
+    ROS_WARN("current lane doesn't have change flag");
+    return;
+  }
+
+  if ((static_cast<ChangeFlag>(cur_lane.waypoints.at(num_lane_change).change_flag) == ChangeFlag::right &&
+       right_lane_idx_ < 0) ||
+      (static_cast<ChangeFlag>(cur_lane.waypoints.at(num_lane_change).change_flag) == ChangeFlag::left &&
+       left_lane_idx_ < 0))
+  {
+    ROS_WARN("current lane doesn't have the lane for lane change");
+    return;
+  }
+
+  double dt = getTwoDimensionalDistance(cur_lane.waypoints.at(num_lane_change).pose.pose.position,
+                                        cur_lane.waypoints.at(clst_wp).pose.pose.position);
+  double dt_by_vel = std::max(fabs(current_velocity_.twist.linear.x * lane_change_target_ratio_), lane_change_target_minimum_);
+  ROS_INFO("dt : %lf, dt_by_vel : %lf", dt, dt_by_vel);
+  autoware_msgs::Lane &nghbr_lane =
+      static_cast<ChangeFlag>(cur_lane.waypoints.at(num_lane_change).change_flag) == ChangeFlag::right ?
+          std::get<0>(tuple_vec_.at(right_lane_idx_)) :
+          std::get<0>(tuple_vec_.at(left_lane_idx_));
+  const int32_t &nghbr_clst_wp =
+      static_cast<ChangeFlag>(cur_lane.waypoints.at(num_lane_change).change_flag) == ChangeFlag::right ?
+          std::get<1>(tuple_vec_.at(right_lane_idx_)) :
+          std::get<1>(tuple_vec_.at(left_lane_idx_));
+
+  int32_t target_num = -1;
+  for (uint32_t i = nghbr_clst_wp; i < nghbr_lane.waypoints.size(); i++)
+  {
+    if (i == nghbr_lane.waypoints.size() - 1 ||
+        dt + dt_by_vel < getTwoDimensionalDistance(nghbr_lane.waypoints.at(nghbr_clst_wp).pose.pose.position,
+                                                   nghbr_lane.waypoints.at(i).pose.pose.position))
+    {
+      target_num = i;
+      break;
+    }
+  }
+
+  ROS_INFO("target_num : %d", target_num);
+  if (target_num < 0)
+    return;
+
+  std::get<0>(lane_for_change_).header.stamp = nghbr_lane.header.stamp;
+  std::vector<autoware_msgs::Waypoint> hermite_wps = generateHermiteCurveForROS(
+      cur_lane.waypoints.at(num_lane_change).pose.pose, nghbr_lane.waypoints.at(target_num).pose.pose,
+      cur_lane.waypoints.at(num_lane_change).twist.twist.linear.x, vlength_hermite_curve_);
+
+  for (auto &&el : hermite_wps)
+    el.change_flag = cur_lane.waypoints.at(num_lane_change).change_flag;
+
+  std::get<0>(lane_for_change_).waypoints.reserve(nghbr_lane.waypoints.size() + hermite_wps.size());
+  std::move(hermite_wps.begin(), hermite_wps.end(), std::back_inserter(std::get<0>(lane_for_change_).waypoints));
+  auto itr = nghbr_lane.waypoints.begin();
+  std::advance(itr, target_num);
+  for (auto i = itr; i != nghbr_lane.waypoints.end(); i++)
+  {
+    if (getTwoDimensionalDistance(itr->pose.pose.position, i->pose.pose.position) < lane_change_interval_)
+      i->change_flag = enumToInteger(ChangeFlag::straight);
+    else
+      break;
+  }
+  std::copy(itr, nghbr_lane.waypoints.end(), std::back_inserter(std::get<0>(lane_for_change_).waypoints));
+}
+
+void LaneSelectNode::updateChangeFlag()
+{
+  for (auto &el : tuple_vec_)
+  {
+    std::get<2>(el) = (std::get<1>(el) != -1) ?
+                          static_cast<ChangeFlag>(std::get<0>(el).waypoints.at(std::get<1>(el)).change_flag) :
+                          ChangeFlag::unknown;
+
+    if (std::get<2>(el) == ChangeFlag::right && right_lane_idx_ == -1)
+      std::get<2>(el) = ChangeFlag::unknown;
+    else if (std::get<2>(el) == ChangeFlag::left && left_lane_idx_ == -1)
+      std::get<2>(el) = ChangeFlag::unknown;
+
+    ROS_INFO("change_flag: %d", enumToInteger(std::get<2>(el)));
+  }
+}
+
+void LaneSelectNode::changeLane()
+{
+  if (std::get<2>(tuple_vec_.at(current_lane_idx_)) == ChangeFlag::right && right_lane_idx_ != -1 &&
+      std::get<1>(tuple_vec_.at(right_lane_idx_)) != -1)
+  {
+    current_lane_idx_ = right_lane_idx_;
+  }
+  else if (std::get<2>(tuple_vec_.at(current_lane_idx_)) == ChangeFlag::left && left_lane_idx_ != -1 &&
+           std::get<1>(tuple_vec_.at(left_lane_idx_)) != -1)
+  {
+    current_lane_idx_ = left_lane_idx_;
+  }
+
+  findNeighborLanes();
+  return;
+}
+
+bool LaneSelectNode::getClosestWaypointNumberForEachLanes()
+{
+  for (auto &el : tuple_vec_)
+  {
+    std::get<1>(el) = getClosestWaypointNumber(std::get<0>(el), current_pose_.pose, current_velocity_.twist,
+                                               std::get<1>(el), distance_threshold_, search_closest_waypoint_minimum_dt_);
+    ROS_INFO("closest: %d", std::get<1>(el));
+  }
+
+  // confirm if all closest waypoint numbers are -1. If so, output warning
+  int32_t accum = 0;
+  for (const auto &el : tuple_vec_)
+  {
+    accum += std::get<1>(el);
+  }
+  if (accum == (-1) * static_cast<int32_t>(tuple_vec_.size()))
+  {
+    ROS_WARN("Cannot get closest waypoints. All closest waypoints are changed to -1...");
+    return false;
+  }
+
+  return true;
+}
+
+void LaneSelectNode::findCurrentLane()
+{
+  std::vector<uint32_t> idx_vec;
+  idx_vec.reserve(tuple_vec_.size());
+  for (uint32_t i = 0; i < tuple_vec_.size(); i++)
+  {
+    if (std::get<1>(tuple_vec_.at(i)) == -1)
+      continue;
+    idx_vec.push_back(i);
+  }
+  current_lane_idx_ = findMostClosestLane(idx_vec, current_pose_.pose.position);
+}
+
+int32_t LaneSelectNode::findMostClosestLane(const std::vector<uint32_t> idx_vec, const geometry_msgs::Point p)
+{
+  std::vector<double> dist_vec;
+  dist_vec.reserve(idx_vec.size());
+  for (const auto &el : idx_vec)
+  {
+    int32_t closest_number = std::get<1>(tuple_vec_.at(el));
+    dist_vec.push_back(
+        getTwoDimensionalDistance(p, std::get<0>(tuple_vec_.at(el)).waypoints.at(closest_number).pose.pose.position));
+  }
+  std::vector<double>::iterator itr = std::min_element(dist_vec.begin(), dist_vec.end());
+  return idx_vec.at(std::distance(dist_vec.begin(), itr));
+}
+
+void LaneSelectNode::findNeighborLanes()
+{
+  int32_t current_closest_num = std::get<1>(tuple_vec_.at(current_lane_idx_));
+  const geometry_msgs::Pose &current_closest_pose =
+      std::get<0>(tuple_vec_.at(current_lane_idx_)).waypoints.at(current_closest_num).pose.pose;
+
+  std::vector<uint32_t> left_lane_idx_vec;
+  left_lane_idx_vec.reserve(tuple_vec_.size());
+  std::vector<uint32_t> right_lane_idx_vec;
+  right_lane_idx_vec.reserve(tuple_vec_.size());
+  for (uint32_t i = 0; i < tuple_vec_.size(); i++)
+  {
+    if (i == static_cast<uint32_t>(current_lane_idx_) || std::get<1>(tuple_vec_.at(i)) == -1)
+      continue;
+
+    int32_t target_num = std::get<1>(tuple_vec_.at(i));
+    const geometry_msgs::Point &target_p = std::get<0>(tuple_vec_.at(i)).waypoints.at(target_num).pose.pose.position;
+
+    geometry_msgs::Point converted_p = convertPointIntoRelativeCoordinate(target_p, current_closest_pose);
+
+    ROS_INFO("distance: %lf", converted_p.y);
+    if (fabs(converted_p.y) > distance_threshold_)
+    {
+      ROS_INFO("%d lane is far from current lane...", i);
+      continue;
+    }
+
+    if (converted_p.y > 0)
+      left_lane_idx_vec.push_back(i);
+    else
+      right_lane_idx_vec.push_back(i);
+  }
+
+  if (!left_lane_idx_vec.empty())
+    left_lane_idx_ = findMostClosestLane(left_lane_idx_vec, current_closest_pose.position);
+  else
+    left_lane_idx_ = -1;
+
+  if (!right_lane_idx_vec.empty())
+    right_lane_idx_ = findMostClosestLane(right_lane_idx_vec, current_closest_pose.position);
+  else
+    right_lane_idx_ = -1;
+}
+visualization_msgs::Marker LaneSelectNode::createCurrentLaneMarker()
+{
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time();
+  marker.ns = "current_lane_marker";
+
+  if (current_lane_idx_ == -1 || std::get<0>(tuple_vec_.at(current_lane_idx_)).waypoints.empty())
+  {
+    marker.action = visualization_msgs::Marker::DELETE;
+    return marker;
+  }
+
+  marker.type = visualization_msgs::Marker::LINE_STRIP;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.scale.x = 0.05;
+
+  std_msgs::ColorRGBA color_current;
+  color_current.b = 1.0;
+  color_current.g = 0.7;
+  color_current.a = 1.0;
+  marker.color = color_current;
+
+  for (const auto &em : std::get<0>(tuple_vec_.at(current_lane_idx_)).waypoints)
+    marker.points.push_back(em.pose.pose.position);
+
+  return marker;
+}
+
+visualization_msgs::Marker LaneSelectNode::createRightLaneMarker()
+{
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time();
+  marker.ns = "right_lane_marker";
+
+  if (right_lane_idx_ == -1 || std::get<0>(tuple_vec_.at(current_lane_idx_)).waypoints.empty())
+  {
+    marker.action = visualization_msgs::Marker::DELETE;
+    return marker;
+  }
+
+  marker.type = visualization_msgs::Marker::LINE_STRIP;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.scale.x = 0.05;
+
+  std_msgs::ColorRGBA color_neighbor;
+  color_neighbor.r = 0.5;
+  color_neighbor.b = 0.5;
+  color_neighbor.g = 0.5;
+  color_neighbor.a = 1.0;
+
+  std_msgs::ColorRGBA color_neighbor_change;
+  color_neighbor_change.b = 0.7;
+  color_neighbor_change.g = 1.0;
+  color_neighbor_change.a = 1.0;
+
+  const ChangeFlag &change_flag = std::get<2>(tuple_vec_.at(current_lane_idx_));
+  marker.color = change_flag == ChangeFlag::right ? color_neighbor_change : color_neighbor;
+
+  for (const auto &em : std::get<0>(tuple_vec_.at(right_lane_idx_)).waypoints)
+    marker.points.push_back(em.pose.pose.position);
+
+  return marker;
+}
+
+visualization_msgs::Marker LaneSelectNode::createLeftLaneMarker()
+{
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time();
+  marker.ns = "left_lane_marker";
+
+  if (left_lane_idx_ == -1 || std::get<0>(tuple_vec_.at(current_lane_idx_)).waypoints.empty())
+  {
+    marker.action = visualization_msgs::Marker::DELETE;
+    return marker;
+  }
+
+  marker.type = visualization_msgs::Marker::LINE_STRIP;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.scale.x = 0.05;
+
+  std_msgs::ColorRGBA color_neighbor;
+  color_neighbor.r = 0.5;
+  color_neighbor.b = 0.5;
+  color_neighbor.g = 0.5;
+  color_neighbor.a = 1.0;
+
+  std_msgs::ColorRGBA color_neighbor_change;
+  color_neighbor_change.b = 0.7;
+  color_neighbor_change.g = 1.0;
+  color_neighbor_change.a = 1.0;
+
+  const ChangeFlag &change_flag = std::get<2>(tuple_vec_.at(current_lane_idx_));
+  marker.color = change_flag == ChangeFlag::left ? color_neighbor_change : color_neighbor;
+
+  for (const auto &em : std::get<0>(tuple_vec_.at((left_lane_idx_))).waypoints)
+    marker.points.push_back(em.pose.pose.position);
+
+  return marker;
+}
+
+visualization_msgs::Marker LaneSelectNode::createChangeLaneMarker()
+{
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time();
+  marker.ns = "change_lane_marker";
+
+  if (std::get<0>(lane_for_change_).waypoints.empty())
+  {
+    marker.action = visualization_msgs::Marker::DELETE;
+    return marker;
+  }
+
+  marker.type = visualization_msgs::Marker::LINE_STRIP;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.scale.x = 0.05;
+
+  std_msgs::ColorRGBA color;
+  color.r = 1.0;
+  color.a = 1.0;
+
+  std_msgs::ColorRGBA color_current;
+  color_current.b = 1.0;
+  color_current.g = 0.7;
+  color_current.a = 1.0;
+
+  marker.color = current_state_ == "LANE_CHANGE" ? color_current : color;
+  for (const auto &em : std::get<0>(lane_for_change_).waypoints)
+    marker.points.push_back(em.pose.pose.position);
+
+  return marker;
+}
+
+visualization_msgs::Marker LaneSelectNode::createClosestWaypointsMarker()
+{
+  visualization_msgs::Marker marker;
+  std_msgs::ColorRGBA color_closest_wp;
+  color_closest_wp.r = 1.0;
+  color_closest_wp.b = 1.0;
+  color_closest_wp.g = 1.0;
+  color_closest_wp.a = 1.0;
+
+  marker.header.frame_id = "map";
+  marker.header.stamp = ros::Time();
+  marker.ns = "closest_waypoints_marker";
+  marker.type = visualization_msgs::Marker::POINTS;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.scale.x = 0.5;
+  marker.color = color_closest_wp;
+
+  marker.points.reserve(tuple_vec_.size());
+  for (uint32_t i = 0; i < tuple_vec_.size(); i++)
+  {
+    if (std::get<1>(tuple_vec_.at(i)) == -1)
+      continue;
+
+    marker.points.push_back(
+        std::get<0>(tuple_vec_.at(i)).waypoints.at(std::get<1>(tuple_vec_.at(i))).pose.pose.position);
+  }
+
+  return marker;
+}
+
+void LaneSelectNode::publishVisualizer()
+{
+  visualization_msgs::MarkerArray marker_array;
+  marker_array.markers.push_back(createChangeLaneMarker());
+  marker_array.markers.push_back(createCurrentLaneMarker());
+  marker_array.markers.push_back(createRightLaneMarker());
+  marker_array.markers.push_back(createLeftLaneMarker());
+  marker_array.markers.push_back(createClosestWaypointsMarker());
+
+  vis_pub1_.publish(marker_array);
+}
+
+void LaneSelectNode::publishLane(const autoware_msgs::Lane &lane)
+{
+  // publish global lane
+  pub1_.publish(lane);
+}
+
+void LaneSelectNode::publishLaneID(const autoware_msgs::Lane &lane)
+{
+  std_msgs::Int32 msg;
+  msg.data = lane.lane_id;
+  pub4_.publish(msg);
+}
+
+void LaneSelectNode::publishClosestWaypoint(const int32_t clst_wp)
+{
+  // publish closest waypoint
+  std_msgs::Int32 closest_waypoint;
+  closest_waypoint.data = clst_wp;
+  pub2_.publish(closest_waypoint);
+}
+
+void LaneSelectNode::publishChangeFlag(const ChangeFlag flag)
+{
+  std_msgs::Int32 change_flag;
+  change_flag.data = enumToInteger(flag);
+  pub3_.publish(change_flag);
+}
+
+void LaneSelectNode::publishVehicleLocation(const int32_t clst_wp, const int32_t larray_id)
+{
+  autoware_msgs::VehicleLocation vehicle_location;
+  vehicle_location.header.stamp = ros::Time::now();
+  vehicle_location.waypoint_index = clst_wp;
+  vehicle_location.lane_array_id = larray_id;
+  pub5_.publish(vehicle_location);
+}
+
+void LaneSelectNode::callbackFromLaneArray(const autoware_msgs::LaneArrayConstPtr &msg)
+{
+  tuple_vec_.clear();
+  tuple_vec_.shrink_to_fit();
+  tuple_vec_.reserve(msg->lanes.size());
+  for (const auto &el : msg->lanes)
+  {
+    auto t = std::make_tuple(el, -1, ChangeFlag::unknown);
+    tuple_vec_.push_back(t);
+  }
+
+  lane_array_id_ = msg->id;
+  current_lane_idx_ = -1;
+  right_lane_idx_ = -1;
+  left_lane_idx_ = -1;
+  is_lane_array_subscribed_ = true;
+
+  if (current_lane_idx_ == -1)
+    initForLaneSelect();
+  else
+    processing();
+}
+
+void LaneSelectNode::callbackFromPoseStamped(const geometry_msgs::PoseStampedConstPtr &msg)
+{
+  current_pose_ = *msg;
+  is_current_pose_subscribed_ = true;
+
+  if (current_lane_idx_ == -1)
+    initForLaneSelect();
+  else
+    processing();
+}
+
+void LaneSelectNode::callbackFromTwistStamped(const geometry_msgs::TwistStampedConstPtr &msg)
+{
+  current_velocity_ = *msg;
+  is_current_velocity_subscribed_ = true;
+
+  if (current_lane_idx_ == -1)
+    initForLaneSelect();
+  else
+    processing();
+}
+
+void LaneSelectNode::callbackFromState(const std_msgs::StringConstPtr &msg)
+{
+  current_state_ = msg->data;
+  is_current_state_subscribed_ = true;
+
+  if (current_lane_idx_ == -1)
+    initForLaneSelect();
+  else
+    processing();
+}
+void LaneSelectNode::callbackFromDecisionMakerState(const std_msgs::StringConstPtr &msg)
+{
+  is_current_state_subscribed_ = true;
+
+  if (msg->data.find("ChangeTo") != std::string::npos)
+  {
+    current_state_ = std::string("LANE_CHANGE");
+    ;
+  }
+  else
+  {
+    current_state_ = msg->data;
+  }
+
+  if (current_lane_idx_ == -1)
+    initForLaneSelect();
+  else
+    processing();
+}
+
+void LaneSelectNode::callbackFromConfig(const autoware_config_msgs::ConfigLaneSelectConstPtr &msg)
+{
+  distance_threshold_ = msg->distance_threshold_neighbor_lanes;
+  lane_change_interval_ = msg->lane_change_interval;
+  lane_change_target_ratio_ = msg->lane_change_target_ratio;
+  lane_change_target_minimum_ = msg->lane_change_target_minimum;
+  vlength_hermite_curve_ = msg->vector_length_hermite_curve;
+  is_config_subscribed_ = true;
+
+  if (current_lane_idx_ == -1)
+    initForLaneSelect();
+  else
+    processing();
+}
+
+void LaneSelectNode::run()
+{
+  ros::spin();
+}
+
+// distance between target 1 and target2 in 2-D
+double getTwoDimensionalDistance(const geometry_msgs::Point &target1, const geometry_msgs::Point &target2)
+{
+  double distance = sqrt(pow(target1.x - target2.x, 2) + pow(target1.y - target2.y, 2));
+  return distance;
+}
+
+geometry_msgs::Point convertPointIntoRelativeCoordinate(const geometry_msgs::Point &input_point,
+                                                        const geometry_msgs::Pose &pose)
+{
+  tf::Transform inverse;
+  tf::poseMsgToTF(pose, inverse);
+  tf::Transform transform = inverse.inverse();
+
+  tf::Point p;
+  pointMsgToTF(input_point, p);
+  tf::Point tf_p = transform * p;
+  geometry_msgs::Point tf_point_msg;
+  pointTFToMsg(tf_p, tf_point_msg);
+  return tf_point_msg;
+}
+
+geometry_msgs::Point convertPointIntoWorldCoordinate(const geometry_msgs::Point &input_point,
+                                                     const geometry_msgs::Pose &pose)
+{
+  tf::Transform inverse;
+  tf::poseMsgToTF(pose, inverse);
+
+  tf::Point p;
+  pointMsgToTF(input_point, p);
+  tf::Point tf_p = inverse * p;
+
+  geometry_msgs::Point tf_point_msg;
+  pointTFToMsg(tf_p, tf_point_msg);
+  return tf_point_msg;
+}
+
+double getRelativeAngle(const geometry_msgs::Pose &waypoint_pose, const geometry_msgs::Pose &current_pose)
+{
+  tf::Vector3 x_axis(1, 0, 0);
+  tf::Transform waypoint_tfpose;
+  tf::poseMsgToTF(waypoint_pose, waypoint_tfpose);
+  tf::Vector3 waypoint_v = waypoint_tfpose.getBasis() * x_axis;
+  tf::Transform current_tfpose;
+  tf::poseMsgToTF(current_pose, current_tfpose);
+  tf::Vector3 current_v = current_tfpose.getBasis() * x_axis;
+
+  return current_v.angle(waypoint_v) * 180 / M_PI;
+}
+
+// get closest waypoint from current pose
+int32_t getClosestWaypointNumber(const autoware_msgs::Lane &current_lane, const geometry_msgs::Pose &current_pose,
+                                 const geometry_msgs::Twist &current_velocity, const int32_t previous_number,
+                                 const double distance_threshold, const int search_closest_waypoint_minimum_dt)
+{
+  if (current_lane.waypoints.size() < 2)
+    return -1;
+
+  std::vector<uint32_t> idx_vec;
+  // if previous number is -1, search closest waypoint from waypoints in front of current pose
+  uint32_t range_min = 0;
+  uint32_t range_max = current_lane.waypoints.size();
+  if (previous_number == -1)
+  {
+    idx_vec.reserve(current_lane.waypoints.size());
+  }
+  else
+  {
+    if (distance_threshold <
+        getTwoDimensionalDistance(current_lane.waypoints.at(previous_number).pose.pose.position, current_pose.position))
+    {
+      ROS_WARN("Current_pose is far away from previous closest waypoint. Initilized...");
+      return -1;
+    }
+    range_min = static_cast<uint32_t>(previous_number);
+    double ratio = 3;
+    double dt = std::max(current_velocity.linear.x * ratio, static_cast<double>(search_closest_waypoint_minimum_dt));
+    if (static_cast<uint32_t>(previous_number + dt) < current_lane.waypoints.size())
+    {
+      range_max = static_cast<uint32_t>(previous_number + dt);
+    }
+  }
+  const LaneDirection dir = getLaneDirection(current_lane);
+  const int sgn = (dir == LaneDirection::Forward) ? 1 : (dir == LaneDirection::Backward) ? -1 : 0;
+  for (uint32_t i = range_min; i < range_max; i++)
+  {
+    geometry_msgs::Point converted_p =
+      convertPointIntoRelativeCoordinate(current_lane.waypoints.at(i).pose.pose.position, current_pose);
+    double angle = getRelativeAngle(current_lane.waypoints.at(i).pose.pose, current_pose);
+    if (converted_p.x * sgn > 0 && angle < 90)
+    {
+      idx_vec.push_back(i);
+    }
+  }
+
+  if (idx_vec.empty())
+    return -1;
+
+  std::vector<double> dist_vec;
+  dist_vec.reserve(idx_vec.size());
+  for (const auto &el : idx_vec)
+  {
+    double dt = getTwoDimensionalDistance(current_pose.position, current_lane.waypoints.at(el).pose.pose.position);
+    dist_vec.push_back(dt);
+  }
+  std::vector<double>::iterator itr = std::min_element(dist_vec.begin(), dist_vec.end());
+  int32_t found_number = idx_vec.at(static_cast<uint32_t>(std::distance(dist_vec.begin(), itr)));
+  return found_number;
+}
+
+// let the linear equation be "ax + by + c = 0"
+// if there are two points (x1,y1) , (x2,y2), a = "y2-y1, b = "(-1) * x2 - x1" ,c = "(-1) * (y2-y1)x1 + (x2-x1)y1"
+bool getLinearEquation(geometry_msgs::Point start, geometry_msgs::Point end, double *a, double *b, double *c)
+{
+  //(x1, y1) = (start.x, star.y), (x2, y2) = (end.x, end.y)
+  double sub_x = fabs(start.x - end.x);
+  double sub_y = fabs(start.y - end.y);
+  double error = pow(10, -5);  // 0.00001
+
+  if (sub_x < error && sub_y < error)
+  {
+    ROS_INFO("two points are the same point!!");
+    return false;
+  }
+
+  *a = end.y - start.y;
+  *b = (-1) * (end.x - start.x);
+  *c = (-1) * (end.y - start.y) * start.x + (end.x - start.x) * start.y;
+
+  return true;
+}
+double getDistanceBetweenLineAndPoint(geometry_msgs::Point point, double a, double b, double c)
+{
+  double d = fabs(a * point.x + b * point.y + c) / sqrt(pow(a, 2) + pow(b, 2));
+
+  return d;
+}
+
+}  // lane_planner

src/ros/catkin/src/lane_planner/nodes/lane_select/lane_select_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 "lane_select_core.h"
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "lane_select");
+  lane_planner::LaneSelectNode lsn;
+  lsn.run();
+
+  return 0;
+}

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

@@ -0,0 +1,124 @@
+/*
+ * 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/console.h>
+
+#include "autoware_config_msgs/ConfigLaneStop.h"
+#include "autoware_msgs/TrafficLight.h"
+#include "autoware_msgs/LaneArray.h"
+
+#include <lane_planner/lane_planner_vmap.hpp>
+
+namespace {
+
+bool config_manual_detection = true;
+
+ros::Publisher traffic_pub;
+
+autoware_msgs::LaneArray current_red_lane;
+autoware_msgs::LaneArray current_green_lane;
+
+const autoware_msgs::LaneArray *previous_lane = &current_red_lane;
+
+void select_current_lane(const autoware_msgs::TrafficLight& msg)
+{
+  const autoware_msgs::LaneArray *current;
+  switch (msg.traffic_light) {
+  case lane_planner::vmap::TRAFFIC_LIGHT_RED:
+    current = &current_red_lane;
+    break;
+  case lane_planner::vmap::TRAFFIC_LIGHT_GREEN:
+    current = &current_green_lane;
+    break;
+  case lane_planner::vmap::TRAFFIC_LIGHT_UNKNOWN:
+    current = previous_lane; // if traffic light state is unknown, keep previous state
+    break;
+  default:
+    ROS_ERROR_STREAM("undefined traffic light");
+    return;
+  }
+
+  if (current->lanes.empty()) {
+    ROS_ERROR_STREAM("empty lanes");
+    return;
+  }
+
+  traffic_pub.publish(*current);
+
+  previous_lane = current;
+}
+
+void receive_auto_detection(const autoware_msgs::TrafficLight& msg)
+{
+  if (!config_manual_detection)
+    select_current_lane(msg);
+}
+
+void receive_manual_detection(const autoware_msgs::TrafficLight& msg)
+{
+  if (config_manual_detection)
+    select_current_lane(msg);
+}
+
+void cache_red_lane(const autoware_msgs::LaneArray& msg)
+{
+  current_red_lane = msg;
+}
+
+void cache_green_lane(const autoware_msgs::LaneArray& msg)
+{
+  current_green_lane = msg;
+}
+
+void config_parameter(const autoware_config_msgs::ConfigLaneStop& msg)
+{
+  config_manual_detection = msg.manual_detection;
+}
+
+} // namespace
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "lane_stop");
+
+  ros::NodeHandle n;
+
+  int sub_light_queue_size;
+  n.param<int>("/lane_stop/sub_light_queue_size", sub_light_queue_size, 1);
+  int sub_waypoint_queue_size;
+  n.param<int>("/lane_stop/sub_waypoint_queue_size", sub_waypoint_queue_size, 1);
+  int sub_config_queue_size;
+  n.param<int>("/lane_rule/sub_config_queue_size", sub_config_queue_size, 1);
+  int pub_waypoint_queue_size;
+  n.param<int>("/lane_stop/pub_waypoint_queue_size", pub_waypoint_queue_size, 1);
+  bool pub_waypoint_latch;
+  n.param<bool>("/lane_stop/pub_waypoint_latch", pub_waypoint_latch, true);
+  n.param<bool>("/lane_stop/manual_detection", config_manual_detection, true);
+
+  traffic_pub = n.advertise<autoware_msgs::LaneArray>("/traffic_waypoints_array", pub_waypoint_queue_size,
+                pub_waypoint_latch);
+
+  ros::Subscriber light_sub = n.subscribe("/light_color", sub_light_queue_size, receive_auto_detection);
+  ros::Subscriber light_managed_sub = n.subscribe("/light_color_managed", sub_light_queue_size,
+              receive_manual_detection);
+  ros::Subscriber red_sub = n.subscribe("/red_waypoints_array", sub_waypoint_queue_size, cache_red_lane);
+  ros::Subscriber green_sub = n.subscribe("/green_waypoints_array", sub_waypoint_queue_size, cache_green_lane);
+  ros::Subscriber config_sub = n.subscribe("/config/lane_stop", sub_config_queue_size, config_parameter);
+
+  ros::spin();
+
+  return EXIT_SUCCESS;
+}

src/ros/catkin/src/lane_planner/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lane_planner</name>
+  <version>1.12.0</version>
+  <description>The lane_planner package</description>
+  <maintainer email="tomo@axe-inc.co.jp">Tomomi HORII</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>autoware_config_msgs</depend>
+  <depend>autoware_msgs</depend>
+  <depend>gnss</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>libwaypoint_follower</depend>
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>tablet_socket_msgs</depend>
+  <depend>vector_map</depend>
+
+  <test_depend>rostest</test_depend>
+  <test_depend>rosunit</test_depend>
+</package>

src/ros/catkin/src/lane_planner/test/src/test_lane_select.cpp

@@ -0,0 +1,85 @@
+/*
+ * 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 <gtest/gtest.h>
+#include <ros/ros.h>
+
+#include "test_lane_select.hpp"
+
+namespace lane_planner {
+
+class LaneSelectTestSuite : public ::testing::Test {
+public:
+  LaneSelectTestSuite() {}
+  ~LaneSelectTestSuite() {}
+
+  LaneSelectTestClass test_obj_;
+
+protected:
+  virtual void SetUp() { test_obj_.lsn = new LaneSelectNode(); };
+  virtual void TearDown() { delete test_obj_.lsn; };
+};
+
+TEST_F(LaneSelectTestSuite, publishVehicleLocation) {
+  ASSERT_EQ(test_obj_.vehicle_location_sub.getNumPublishers(), 1U)
+      << "No publisher exist!";
+
+  std::array<std::pair<std::string, int>, 2> driving_direction =
+  {
+    std::make_pair("Forward", 1),
+    std::make_pair("Backward", -1)
+  };
+  for (const auto& dir : driving_direction)
+  {
+    test_obj_.publishTrafficWaypointsArray(dir.second);
+    test_obj_.publishCurrentPose(-0.5 * dir.second, 0.0, 0.0);
+    test_obj_.publishCurrentVelocity(0);
+    ros::spinOnce();
+    ros::WallDuration(0.1).sleep();
+    test_obj_.lsnSpinOnce();
+    ros::WallDuration(0.1).sleep();
+    ros::spinOnce();
+
+    ASSERT_EQ(0, test_obj_.cb_vehicle_location.waypoint_index)
+        << dir.first << "Waypoint index does not match."
+        << "It should be 0";
+    ASSERT_EQ(test_obj_.lane_array_id_,
+              test_obj_.cb_vehicle_location.lane_array_id)
+        << dir.first << "LaneArray id does not match."
+        << "It should be " << test_obj_.lane_array_id_;
+
+    test_obj_.publishCurrentPose(0.5 * dir.second, 0.0, 0.0);
+    test_obj_.publishCurrentVelocity(0);
+    ros::spinOnce();
+    ros::WallDuration(0.1).sleep();
+    test_obj_.lsnSpinOnce();
+    ros::WallDuration(0.1).sleep();
+    ros::spinOnce();
+
+    ASSERT_EQ(1, test_obj_.cb_vehicle_location.waypoint_index)
+        << dir.first << "Waypoint index does not match."
+        << "It should be 1";
+  }
+}
+
+} // namespace lane_planner
+
+int main(int argc, char **argv) {
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "LaneSelectTestNode");
+  ros::NodeHandle nh;
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/lane_planner/test/src/test_lane_select.hpp

@@ -0,0 +1,99 @@
+/*
+ * 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 <gtest/gtest.h>
+#include <ros/connection_manager.h>
+#include <ros/ros.h>
+
+#include "lane_select_core.h"
+
+namespace lane_planner {
+
+class LaneSelectTestClass {
+public:
+  LaneSelectTestClass() {}
+
+  LaneSelectNode *lsn;
+  autoware_msgs::VehicleLocation cb_vehicle_location;
+
+  ros::NodeHandle nh;
+  ros::Publisher traffic_waypoints_array_pub =
+      nh.advertise<autoware_msgs::LaneArray>("/traffic_waypoints_array", 0);
+  ros::Publisher current_pose_pub =
+      nh.advertise<geometry_msgs::PoseStamped>("/current_pose", 0);
+  ros::Publisher current_velocity_pub =
+      nh.advertise<geometry_msgs::TwistStamped>("/current_velocity", 0);
+  ros::Subscriber vehicle_location_sub =
+      nh.subscribe("/vehicle_location", 1,
+                   &LaneSelectTestClass::vehicleLocationCallback, this);
+
+  int32_t lane_array_id_ = 6587651;
+
+  void accessPublishVehicleLocation(int32_t clst_wp, int32_t larray_id) {
+    lsn->publishVehicleLocation(clst_wp, larray_id);
+  }
+
+  void publishTrafficWaypointsArray(int driving_direction) {
+    autoware_msgs::LaneArray pub_msg;
+    autoware_msgs::Lane pub_lane;
+    for (int idx = 0; idx < 100; idx++) {
+      static autoware_msgs::Waypoint wp;
+      wp.gid = idx;
+      wp.lid = idx;
+      wp.pose.pose.position.x = 0.0 + driving_direction * (double)idx;
+      wp.pose.pose.position.y = 0.0;
+      wp.pose.pose.position.z = 0.0;
+      wp.twist.twist.linear.x = 5.0 * driving_direction;
+      wp.twist.twist.angular.z = 0.0;
+
+      tf::Quaternion quaternion = tf::createQuaternionFromRPY(0, 0, 0);
+      quaternionTFToMsg(quaternion, wp.pose.pose.orientation);
+
+      pub_lane.waypoints.push_back(wp);
+    }
+    pub_msg.lanes.push_back(pub_lane);
+    pub_msg.id = lane_array_id_;
+
+    traffic_waypoints_array_pub.publish(pub_msg);
+  }
+
+  void publishCurrentPose(double x, double y, double yaw) {
+    geometry_msgs::PoseStamped pose;
+    pose.pose.position.x = x;
+    pose.pose.position.y = y;
+    tf::Quaternion quaternion = tf::createQuaternionFromRPY(0, 0, yaw);
+    quaternionTFToMsg(quaternion, pose.pose.orientation);
+    current_pose_pub.publish(pose);
+  }
+
+  void publishCurrentVelocity(double vel) {
+    geometry_msgs::TwistStamped twist;
+    twist.twist.linear.x = vel;
+    current_velocity_pub.publish(twist);
+  }
+
+  void
+  vehicleLocationCallback(const autoware_msgs::VehicleLocationConstPtr &msg) {
+    cb_vehicle_location = *msg;
+    std::cout << "vehicleLocationCallback: "
+              << cb_vehicle_location.waypoint_index << ", "
+              << cb_vehicle_location.lane_array_id << std::endl;
+  }
+
+  void lsnSpinOnce() { lsn->spinOnce(); }
+};
+
+} // namespace lane_planner

src/ros/catkin/src/lane_planner/test/test_lane_select.test

@@ -0,0 +1,5 @@
+<launch>
+
+  <test test-name="test-lane_select" pkg="lane_planner" type="test-lane_select" name="test"/>
+
+</launch>

src/ros/catkin/src/libvectormap/CHANGELOG.rst

@@ -0,0 +1,269 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package libvectormap
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* [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
+* Fix license notice in corresponding package.xml
+* Contributors: Abraham Monrroy Cano, amc-nu
+
+1.10.0 (2019-01-17)
+-------------------
+* 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
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+
+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>`_)
+* Contributors: Esteve Fernandez
+
+1.7.0 (2018-05-18)
+------------------
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* [Fix] rename packages (`#1269 <https://github.com/CPFL/Autoware/pull/1269>`_)
+  * rename lidar_tracker
+  * Modify pf_lidar_track's cmake file
+  * Refactor code
+  * Rename from euclidean_lidar_tracker to lidar_euclidean_track
+  * Rename from kf_contour_track to lidar_kf_contour_track
+  * Rename from kf_lidar_track to lidar_kf_track, but need some modification in euclidean cluster(Cluster.h)
+  * Rename from pf_lidar_tarck to lidar_pf_track
+  * Rename range_fusion
+  * Rename obj_reproj
+  * Rename euclidean_cluster to lidar_euclidean_cluster_detect
+  * Rename svm_lidar_detect to lidar_svm_detect
+  * Rename kf_lidar_track to lidar_kf_track
+  * Change version 1.6.3 to 1.7.0 in pacakge.xml
+  * Modify CMake so that extrenal header would be loaded
+  * Remove obj_reproj from cv_tracker
+  * Add interface.yaml
+  * Rename road_wizard to trafficlight_recognizer
+  * create common directory
+  * Add lidar_imm_ukf_pda_track
+  * create vision_detector and moved cv
+  * Modify interface.yaml and package.xml
+  * remove dpm_ocv
+  * moved directory
+  * Delete unnecessary launch file
+  * Delete rcnn related file and code
+  * separated dummy_track from cv_tracker
+  * separated klt_track from cv_tracker
+  * Fix a cmake
+  * Remove unnecessary dependency of lidar_euclidean_cluster_detect package
+  * Rename image_segmenter to vision_segment_enet_detect
+  * Remove unnecessary dependency of lidar_svm_detect package
+  * separated kf_track and fix a some compiling issue
+  * move viewers
+  * merge ndt_localizer and icp_localizer, and rename to lidar_localizer
+  * Remove unnecessary dependency of lidar_euclidean_track
+  * moved image lib
+  * add launch
+  * lib move under lidar_tracker
+  * Rename dpm_ttic to vision_dpm_ttic_detect
+  * rename yolo3detector to vision_yolo3_detect
+  * Modify cmake and package.xml in vision_dpm_ttic_detect
+  * moved sourcefiles into nodes dir
+  * moved sourcefiles into nodes dir
+  * Move cv_tracker/data folder and delete cv_tracker/model folder
+  * fix a package file and cmake
+  * Rename yolo2 -> vision_yolo2_detect
+  * fix a package file and cmake
+  * Fix package name of launch file
+  * Rename ssd to vision_ssd_detect
+  * fixed cmake and package for decerese dependencies
+  * remove top packages dir for detection
+  * fixed cmake for cuda
+  * Rename lane_detector to vision_lane_detect
+  * Modify package.xml in lidar-related packages
+  * Remove unnecessary dependencies in lidar_detector and lidar_tracker
+  * Modify computing.yaml for dpm_ttic
+  * Modify dpm_ttic launch file
+  * Remove/Add dependencies to trafficlight_recognizer
+  * Update data folder in dpm_ttic
+  * Modified CMake and package file in dpm_ttic.
+  * Remove src dir in imm_ukf_pda_track
+  * removed unnecessary comments
+  * rename lidar_tracker
+  * Modify pf_lidar_track's cmake file
+  * Refactor code
+  * Rename from euclidean_lidar_tracker to lidar_euclidean_track
+  * Rename from kf_contour_track to lidar_kf_contour_track
+  * Rename from kf_lidar_track to lidar_kf_track, but need some modification in euclidean cluster(Cluster.h)
+  * Rename from pf_lidar_tarck to lidar_pf_track
+  * Rename range_fusion
+  * Rename obj_reproj
+  * Rename road_wizard to trafficlight_recognizer
+  * Rename euclidean_cluster to lidar_euclidean_cluster_detect
+  * Rename svm_lidar_detect to lidar_svm_detect
+  * Rename kf_lidar_track to lidar_kf_track
+  * Change version 1.6.3 to 1.7.0 in pacakge.xml
+  * Modify CMake so that extrenal header would be loaded
+  * Remove obj_reproj from cv_tracker
+  * Add interface.yaml
+  * create common directory
+  * Add lidar_imm_ukf_pda_track
+  * create vision_detector and moved cv
+  * Modify interface.yaml and package.xml
+  * remove dpm_ocv
+  * moved directory
+  * Delete unnecessary launch file
+  * Delete rcnn related file and code
+  * separated dummy_track from cv_tracker
+  * separated klt_track from cv_tracker
+  * Fix a cmake
+  * Remove unnecessary dependency of lidar_euclidean_cluster_detect package
+  * Rename image_segmenter to vision_segment_enet_detect
+  * Remove unnecessary dependency of lidar_svm_detect package
+  * separated kf_track and fix a some compiling issue
+  * move viewers
+  * merge ndt_localizer and icp_localizer, and rename to lidar_localizer
+  * Remove unnecessary dependency of lidar_euclidean_track
+  * moved image lib
+  * add launch
+  * lib move under lidar_tracker
+  * Rename dpm_ttic to vision_dpm_ttic_detect
+  * rename yolo3detector to vision_yolo3_detect
+  * Modify cmake and package.xml in vision_dpm_ttic_detect
+  * moved sourcefiles into nodes dir
+  * moved sourcefiles into nodes dir
+  * Move cv_tracker/data folder and delete cv_tracker/model folder
+  * fix a package file and cmake
+  * Rename yolo2 -> vision_yolo2_detect
+  * fix a package file and cmake
+  * Fix package name of launch file
+  * Rename ssd to vision_ssd_detect
+  * fixed cmake and package for decerese dependencies
+  * remove top packages dir for detection
+  * fixed cmake for cuda
+  * Rename lane_detector to vision_lane_detect
+  * Modify package.xml in lidar-related packages
+  * Remove unnecessary dependencies in lidar_detector and lidar_tracker
+  * Modify computing.yaml for dpm_ttic
+  * Modify dpm_ttic launch file
+  * Remove/Add dependencies to trafficlight_recognizer
+  * Update data folder in dpm_ttic
+  * Modified CMake and package file in dpm_ttic.
+  * Remove src dir in imm_ukf_pda_track
+  * Fix bug for not starting run time manager
+  * Remove invalid dependency
+* Contributors: Kosuke Murakami
+
+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
+* Contributors: Yamato ANDO
+
+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
+* fixed cmake and package.xml for libvectormap
+  moved headers into include/libvectormap since otherwise this otherwise can conflict with other files elsewhere.
+* Contributors: Dejan Pangercic, 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)
+------------------
+
+1.2.0 (2017-06-07)
+------------------
+
+1.1.2 (2017-02-27 23:10)
+------------------------
+
+1.1.1 (2017-02-27 22:25)
+------------------------
+
+1.1.0 (2017-02-24)
+------------------
+
+1.0.1 (2017-01-14)
+------------------
+
+1.0.0 (2016-12-22)
+------------------
+* Fix codes to use map_file messages and old vector_map_info topics
+* Fix for rosjava installed platform
+  Some packages don't declare package dependencies correctly.
+  This makes message jar files built failure.
+* Eigen3 with Fallback on ros-catkin-modules
+* Use c++11 option instead of c++0x
+  We can use newer compilers which support 'c++11' option
+* Initial commit for public release
+* Contributors: Jit Ray Chowdhury, Shinpei Kato, Syohei YOSHIDA, syouji

src/ros/catkin/src/libvectormap/CMakeLists.txt

@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 2.8.12)
+
+project(libvectormap)
+
+find_package(autoware_build_flags REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  roslint
+  vector_map
+  vector_map_msgs
+)
+
+find_package(Eigen3 QUIET)
+
+if (NOT EIGEN3_FOUND)
+  # Fallback to cmake_modules
+  find_package(cmake_modules REQUIRED)
+  find_package(Eigen REQUIRED)
+  set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+  set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})  # Not strictly necessary as Eigen is head only
+  # Possibly map additional variables to the EIGEN3_ prefix.
+else ()
+  set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+endif ()
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS}
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS vector_map vector_map_msgs
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME} STATIC
+  src/vector_map.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${EIGEN3_LIBRARIES}
+)
+
+add_dependencies(${PROJECT_NAME}
+  ${catkin_EXPORTED_TARGETS}
+  ${vector_map_msgs_EXPORTED_TARGETS}
+)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
+
+set(ROSLINT_CPP_OPTS "--filter=-build/c++14,-runtime/references")
+roslint_cpp()
+
+if(CATKIN_ENABLE_TESTING)
+  roslint_add_test()
+endif()

src/ros/catkin/src/libvectormap/include/libvectormap/Math.h

@@ -0,0 +1,89 @@
+/*
+ * 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.
+ *
+ *  Created on: Apr 8, 2015
+ *      Author: sujiwo
+ */
+
+#ifndef LIBVECTORMAP_MATH_H
+#define LIBVECTORMAP_MATH_H
+
+#include <Eigen/Eigen>
+#include <cmath>
+
+typedef Eigen::Vector2f Point2;
+typedef Eigen::Vector3f Point3;
+typedef Eigen::Vector3f Vector3;
+typedef Eigen::Vector4f Point4;
+typedef Eigen::Vector4f Vector4;
+typedef Eigen::Vector2f Vector2;
+typedef Eigen::Quaternionf Quaternion;
+typedef Eigen::Matrix4f Matrix4;
+typedef Eigen::Matrix2f Matrix2;
+
+template<typename F>
+static F degreeToRadian(const F degree)
+{
+  return degree * M_PI / 180;
+}
+
+template<typename F>
+static F radianToDegree(const F radian)
+{
+  return radian * 180 / M_PI;
+}
+
+inline double distance(const Point2 &p1, const Point2 &p2)
+{
+  Vector2 p3(p2.x()-p1.x(), p2.y()-p1.y());
+  return sqrt (p3.squaredNorm());
+}
+
+inline double distance(const Point3 &p1, const Point3 &p2)
+{
+  Vector3 p3(p2.x()-p1.x(), p2.y()-p1.y(), p2.z()-p1.z());
+  return sqrt(p3.squaredNorm());
+}
+
+inline double distance(const Point4 &p1, const Point4 &p2)
+{
+  Vector4 p3(p2.x() - p1.x(), p2.y() - p1.y(), p2.z() - p1.z(), p2.w() - p1.w());
+  return std::sqrt(p3.squaredNorm());
+}
+
+inline double distance(const double &x1, const double &y1, const double &x2, const double &y2)
+{
+  Point2 p(x1, y1), q(x2, y2);
+  return distance (p, q);
+}
+
+inline Vector2 perpendicular(const Vector2 &v)
+{
+  return Vector2(-v.y(), v.x());
+}
+
+inline void rotate(Vector2 &v, const float angleInDegree)
+{
+  Matrix2 rot;
+  double c = std::cos(degreeToRadian(angleInDegree)),
+    s = std::sin(degreeToRadian(angleInDegree));
+  rot(0, 0) = c;
+  rot(0, 1) = -s;
+  rot(1, 0) = s;
+  rot(1, 1) = c;
+  v = rot * v;
+}
+
+#endif  // LIBVECTORMAP_MATH_H

src/ros/catkin/src/libvectormap/include/libvectormap/vector_map.h

@@ -0,0 +1,163 @@
+/*
+ * 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 LIBVECTORMAP_VECTOR_MAP_H
+#define LIBVECTORMAP_VECTOR_MAP_H
+
+#include <cstdio>
+#include <cstdlib>
+#include <cmath>
+#include <vector>
+#include <map>
+#include <string>
+
+#include "Math.h"
+
+#include <vector_map/vector_map.h>
+
+typedef struct
+{
+  int pid;
+  double bx;
+  double ly;
+  double h;
+  double b;
+  double l;
+  int ref;
+  int mcode1;
+  int mcode2;
+  int mcode3;
+}
+Point;
+
+typedef struct
+{
+  int lid;
+  int bpid;
+  int fpid;
+  int blid;
+  int flid;
+}
+Line;
+
+typedef struct
+{
+  int vid;
+  int pid;
+  double hang;
+  double vang;
+}
+Vector;
+
+typedef struct
+{
+  int id;
+  int vid;
+  int plid;
+  int type;
+  int linkid;
+}
+Signal;
+
+typedef struct
+{
+  int id;
+  int lid;
+  double width;
+  char color;
+  int type;
+  int linkid;
+}
+WhiteLine;
+
+typedef struct
+{
+  int lid;
+}
+Mark;
+
+typedef struct
+{
+  int did;
+  double dist;
+  int pid;
+  double dir;
+  double apara;
+  double r;
+  double slope;
+  double cant;
+  double lw;
+  double rw;
+}
+DTLane;
+
+typedef struct
+{
+  int lnid;
+  int did;
+  int blid;
+  int flid;
+  int bnid;
+  int fnid;
+  int jct;
+  int blid2;
+  int blid3;
+  int blid4;
+  int flid2;
+  int flid3;
+  int flid4;
+  int clossid;
+  double span;
+  int lcnt;
+  int lno;
+}
+Lane;
+
+class VectorMap
+{
+public:
+  bool loaded;
+  std::map<int, Point> points;
+  std::map<int, Line> lines;
+  std::map<int, WhiteLine> whitelines;
+  std::map<int, Lane> lanes;
+  std::map<int, DTLane> dtlanes;
+  std::map<int, Vector> vectors;
+  std::map<int, Signal> signals;
+
+  void load_points(const vector_map::PointArray& msg);
+  void load_lines(const vector_map::LineArray& msg);
+  void load_lanes(const vector_map::LaneArray& msg);
+  void load_vectors(const vector_map::VectorArray& msg);
+  void load_signals(const vector_map::SignalArray& msg);
+  void load_whitelines(const vector_map::WhiteLineArray& msg);
+  void load_dtlanes(const vector_map::DTLaneArray& msg);
+
+  VectorMap() :
+    loaded(false) {}
+
+  inline Point3 getPoint(const int idx)
+  {
+    Point3 p;
+    Point psrc = points[idx];
+    p.x() = psrc.bx;
+    p.y() = psrc.ly;
+    p.z() = psrc.h;
+    return p;
+  }
+};
+
+#endif  // LIBVECTORMAP_VECTOR_MAP_H

src/ros/catkin/src/libvectormap/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>libvectormap</name>
+  <version>1.12.0</version>
+  <description>Common Library to Read Vector Map</description>
+  <maintainer email="sujiwo@ertl.jp">sujiwo</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>cmake_modules</depend>
+  <depend>eigen</depend>
+  <depend>roslint</depend>
+  <depend>vector_map</depend>
+  <depend>vector_map_msgs</depend>
+</package>

src/ros/catkin/src/libvectormap/src/vector_map.cpp

@@ -0,0 +1,163 @@
+/*
+ * 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 "libvectormap/vector_map.h"
+
+#include <cstdio>
+#include <cstdlib>
+#include <cmath>
+#include <vector>
+#include <map>
+
+void VectorMap::load_points(const vector_map::PointArray& msg)
+{
+  for (const auto& point : msg.data)
+  {
+    Point tmp;
+    tmp.pid   = point.pid;
+    tmp.b      = point.b;
+    tmp.l      = point.l;
+    tmp.h      = point.h;
+    tmp.bx   = point.ly;
+    tmp.ly   = point.bx;
+    tmp.ref   = point.ref;
+    tmp.mcode1 = point.mcode1;
+    tmp.mcode2 = point.mcode2;
+    tmp.mcode3 = point.mcode3;
+    points.insert(std::map<int, Point>::value_type(tmp.pid, tmp));
+  }
+
+  std::cout << "load points complete. element num: " << points.size() << std::endl;
+} /* void VectorMap::load_points() */
+
+void VectorMap::load_lines(const vector_map::LineArray& msg)
+{
+  for (const auto& line : msg.data)
+  {
+    Line tmp;
+    tmp.lid  = line.lid;
+    tmp.bpid = line.bpid;
+    tmp.fpid = line.fpid;
+    tmp.blid = line.blid;
+    tmp.flid = line.flid;
+
+    lines.insert(std::map<int, Line>::value_type(tmp.lid, tmp));
+  }
+
+  std::cout << "load lines complete." << std::endl;
+} /* void VectorMap::load_lines() */
+
+void VectorMap::load_lanes(const vector_map::LaneArray& msg)
+{
+  for (const auto& lane : msg.data)
+  {
+    Lane tmp;
+    tmp.lnid    = lane.lnid;
+    tmp.did     = lane.did;
+    tmp.blid    = lane.blid;
+    tmp.flid    = lane.flid;
+    tmp.bnid    = lane.bnid;
+    tmp.fnid    = lane.fnid;
+    tmp.jct     = lane.jct;
+    tmp.blid2   = lane.blid2;
+    tmp.blid3   = lane.blid3;
+    tmp.blid4   = lane.blid4;
+    tmp.flid2   = lane.flid2;
+    tmp.flid3   = lane.flid3;
+    tmp.flid4   = lane.flid4;
+    tmp.clossid = lane.clossid;
+    tmp.span    = lane.span;
+    tmp.lcnt    = lane.lcnt;
+    tmp.lno     = lane.lno;
+
+    lanes.insert(std::map<int, Lane>::value_type(tmp.lnid, tmp));
+  }
+
+  std::cout << "load lanes complete." << std::endl;
+} /* void VectorMap::load_lanes() */
+
+void VectorMap::load_vectors(const vector_map::VectorArray& msg)
+{
+  for (const auto& vector : msg.data)
+  {
+    Vector tmp;
+    tmp.vid  = vector.vid;
+    tmp.pid  = vector.pid;
+    tmp.hang = vector.hang;
+    tmp.vang = vector.vang;
+
+    vectors.insert(std::map<int, Vector>::value_type(tmp.vid, tmp));
+  }
+
+  std::cout << "load vectors complete. element num: " << vectors.size() << std::endl;
+} /* void VectorMap::load_vectors() */
+
+void VectorMap::load_signals(const vector_map::SignalArray& msg)
+{
+  for (const auto& signal : msg.data)
+  {
+    Signal tmp;
+    tmp.id     = signal.id;
+    tmp.vid    = signal.vid;
+    tmp.plid   = signal.plid;
+    tmp.type   = signal.type;
+    tmp.linkid = signal.linkid;
+
+    signals.insert(std::map<int, Signal>::value_type(tmp.id, tmp));
+  }
+
+  std::cout << "load signals complete. element num: " << signals.size() << std::endl;
+} /* void VectorMap::load_signals() */
+
+void VectorMap::load_whitelines(const vector_map::WhiteLineArray& msg)
+{
+  for (const auto& white_line : msg.data)
+  {
+    WhiteLine tmp;
+    tmp.id     = white_line.id;
+    tmp.lid    = white_line.lid;
+    tmp.width  = white_line.width;
+    tmp.color  = white_line.color;
+    tmp.type   = white_line.type;
+    tmp.linkid = white_line.linkid;
+
+    whitelines.insert(std::map<int, WhiteLine>::value_type(tmp.id, tmp));
+  }
+
+  std::cout << "load whitelines complete." << std::endl;
+} /* void VectorMap::load_whitelines() */
+
+void VectorMap::load_dtlanes(const vector_map::DTLaneArray& msg)
+{
+  for (const auto& dtlane : msg.data)
+  {
+    DTLane tmp;
+    tmp.did   = dtlane.did;
+    tmp.dist  = dtlane.dist;
+    tmp.pid   = dtlane.pid;
+    tmp.dir   = dtlane.dir;
+    tmp.apara = dtlane.apara;
+    tmp.r     = dtlane.r;
+    tmp.slope = dtlane.slope;
+    tmp.cant  = dtlane.cant;
+    tmp.lw    = dtlane.lw;
+    tmp.rw    = dtlane.rw;
+
+    dtlanes.insert(std::map<int, DTLane>::value_type(tmp.did, tmp));
+  }
+
+  std::cout << "load dtlanes complete." << std::endl;
+} /* void VectorMap::load_dtlanes() */

src/ros/catkin/src/map_file/CHANGELOG.rst

@@ -0,0 +1,215 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package map_file
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* [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
+* Fix license notice in corresponding package.xml
+* Contributors: Abraham Monrroy Cano, 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
+* Feature/points map filter (`#1658 <https://github.com/CPFL/Autoware/issues/1658>`_)
+  * add points_map_filter node
+  * add passthrough filter
+  * fix filter function
+  * apply clang-format
+  * add README.md
+* Contributors: Esteve Fernandez, Masaya Kataoka, amc-nu
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+* [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)
+------------------
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* -Added support fot VMap colouring to Left Traffic signals (`#988 <https://github.com/CPFL/Autoware/pull/988>`_)
+  -Added Lane number on tlr_superimpose
+* Contributors: Abraham Monrroy, Kosuke Murakami
+
+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
+* use header.frame_id included in initialpose topic
+* Initial modifications to feat_proj, tlr, context and vector_map loader, server and client to support different types of traffic signals
+* Contributors: AMC, Yamato ANDO
+
+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
+* compilation issues
+* added install targets
+  changed finding pcl
+  removed unneeded dependencies
+* Contributors: Dejan Pangercic, 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)
+------------------
+* convert to autoware_msgs
+* Contributors: 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)
+------------------
+
+1.0.1 (2017-01-14)
+------------------
+
+1.0.0 (2016-12-22)
+------------------
+* Remove unnecessary error checks
+* Delete map_file messages
+* Refactoring
+* Add error check for vector_map_loader
+* Add download mode for vector_map_loader
+* Have to be under_scored filename
+* Rename wrong directory and filename
+* Fix typos around map_db
+* Add vector_map_loader corresponding to new road objects
+* Add draft proposal of vector_map_loader
+* Runtime Manager, update points_map_loader for SIGINT termination
+* add const to errp read only parameter
+* Runtime Manager Quick Start tab, fix Map load OK label
+* Add module graph tool
+* Fix for rosjava installed platform
+  Some packages don't declare package dependencies correctly.
+  This makes message jar files built failure.
+* Rewrite points_map_loader
+  Rewrite the entire main program.
+  Delete a noisy debug message in library.
+* Use pcd download thread too
+  Existing callbacks use pcd download thread too.
+* Add look ahead downloader
+* Implement request queue to download pcd
+* Are not DEBUG_PRINT
+  These outputs are used by Runtime Manager.
+* Move output of load message
+* Fix handling of /pmap_stat
+  Needn't buffer messages, should be lached.
+  Add initialization code.
+* Default variable is 1000 msec
+* Fix update_rate
+* Redesign map_downloader dialog
+* Add ROS parameters for HTTP server
+* Don't require initial position
+* Delete file by the failure of download
+  If libcurl fails to download, obtained file is deleted.
+* Check HTTP response code
+* Move std:ofstream::close
+* Add digest access authentication
+* Stop publishing messages of lane namespace
+* Refactoring CMakeLists.txt
+  Remove absolute paths by using cmake features and pkg-config.
+* Use c++11 option instead of c++0x
+  We can use newer compilers which support 'c++11' option
+* Merge map_db with map_file.
+* Fix road sign warning on Rviz
+* Initial commit for public release
+* Contributors: Shinpei Kato, Syohei YOSHIDA, USUDA Hisashi, kondoh, syouji

src/ros/catkin/src/map_file/CMakeLists.txt

@@ -0,0 +1,93 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(map_file)
+
+find_package(autoware_build_flags REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  autoware_msgs
+  geometry_msgs
+  pcl_ros
+  roscpp
+  std_msgs
+  tf
+  tf2_geometry_msgs
+  tf2_ros
+  vector_map
+  visualization_msgs
+)
+
+find_package(PCL REQUIRED COMPONENTS io)
+find_package(Boost REQUIRED COMPONENTS filesystem)
+
+# See: https://github.com/ros-perception/perception_pcl/blob/lunar-devel/pcl_ros/CMakeLists.txt#L10-L22
+if(NOT "${PCL_LIBRARIES}" STREQUAL "")
+  # FIXME: this causes duplicates and not found error in ubuntu:zesty
+  list(REMOVE_ITEM PCL_LIBRARIES "/usr/lib/libmpi.so")
+
+  # For debian: https://github.com/ros-perception/perception_pcl/issues/139
+  list(REMOVE_ITEM PCL_IO_LIBRARIES
+    "vtkGUISupportQt"
+    "vtkGUISupportQtOpenGL"
+    "vtkGUISupportQtSQL"
+    "vtkGUISupportQtWebkit"
+    "vtkViewsQt"
+    "vtkRenderingQt"
+  )
+endif()
+
+find_package(CURL REQUIRED)
+
+set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}")
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES get_file
+  CATKIN_DEPENDS
+    autoware_msgs
+    geometry_msgs
+    std_msgs
+    tf2_geometry_msgs
+    visualization_msgs
+  DEPENDS Boost
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${PCL_IO_INCLUDE_DIRS}
+  ${CURL_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIR}
+)
+
+add_library(get_file
+  lib/map_file/get_file.cpp
+)
+target_link_libraries(get_file ${CURL_LIBRARIES})
+
+
+
+add_executable(vector_map_loader nodes/vector_map_loader/vector_map_loader.cpp)
+target_link_libraries(vector_map_loader ${catkin_LIBRARIES} ${vector_map_LIBRARIES} get_file ${CURL_LIBRARIES})
+add_dependencies(vector_map_loader ${catkin_EXPORTED_TARGETS})
+
+
+## Install executables and/or libraries
+install(
+  TARGETS
+    get_file
+    vector_map_loader
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Install project namespaced headers
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)

src/ros/catkin/src/map_file/README.md

@@ -0,0 +1,83 @@
+# map_file package
+
+## points_map_filter
+
+### feature
+points_map_filter_node subscribe pointcloud maps and current pose, the node extract pointcloud near to the current pose.
+
+#### subscribed topics
+/points_map (sensor_msgs/PointCloud2)  : Raw pointcloud map. This topic usually comes from points_map_loader.  
+/current_pose (geometry_msgs/PoseStamped) : Current pose of the car. This topic usually comes from pose_relay node.  
+
+#### published topics
+/points_map/filtered (sensor_msgs/PointCloud2) : Filtered pointcloud submap.  
+
+#### parameters
+
+For points_map_filter
+
+| Param | Type | Default value | Options |
+|-------|------|---------------|-----------|
+| load_grid_size | double | 100.0 | grid size of submap. |
+| load_trigger_distance | double | 20.0 | if the car moves load_trigger_distance(m), the map filter publish filtered submap. |
+
+For points_map_loader
+
+| Param | Type | Default value | Options |
+|-------|------|---------------|-----------|
+| points_map_loader/area | String | "noupdate" | "noupdate", "1x1", "3x3", "5x5", "7x7", "9x9" |
+| points_map_loader/mode | String | "" | "", "download" |
+| points_map_loader/pcd_paths | String array | [] | - |
+| points_map_loader/arealist_path | String array | [] | - |
+
+.pcd file search function is also implemented. Now you can specify multiple files or directories for pcd_paths.
+If directories are specified, it automatically search files in it.
+
+### how it works
+map_filter_node relay /points_map topic until it recieves /current_pose topic.  
+Then, the /current_pose topic recieved, the map_filter_node publish submap.
+
+## demonstration
+[![IMAGE ALT TEXT HERE](http://img.youtube.com/vi/LpKIuI5b4DU/0.jpg)](http://www.youtube.com/watch?v=LpKIuI5b4DU)
+
+## vector_map_loader
+
+This node loads a vector map from csv files.
+
+#### parameters
+
+- `load_mode` - Set the mode of operation for the node. Possible values are:
+    - "file" - Default operation mode, requires each csv file to be specified in args.
+    - "directory" - Loads all csv files with vector map names in the directory specified by the `map_dir` parameter.
+    - "download" - Downloads the vector map csvs from a webhost, use the args to specify
+- `map_dir` - Specify the path to the directory containing vector map csv files. Only used in "directory" mode.
+- `host` - Hostname of the webserver. Only used in "download" mode.
+- `port` - Port of the webserver. Only used in "download" mode.
+- `username` - Username. Only used in "download" mode.
+- `password` - Password. Only used in "download" mode.
+
+## lanelet2_map_loader
+### Feature
+lanelet2_map_loader loads Lanelet2 file and publish the map data as autoware_lanelet2_msgs/MapBin message.
+The node projects lan/lon coordinates into MGRS coordinates.
+
+### How to run
+Run from CLI:
+`rosrun map_file lanelet2_map_loader path/to/map.osm`
+
+### Published Topic
+/lanelet_map_bin (autoware_lanelet2_msgs/MapBin) : Binary data of loaded Lanelet2 Map.
+
+## lanelet2_map_visualization
+### Feature
+lanelet2_map_visualization visualizes autoware_lanelet2_msgs/MapBin messages into visualization_msgs/MarkerArray.
+
+### How to run
+Run from CLI:
+`rosrun map_file lanelet2_map_visualization`
+
+### Subscribed Topics
+/lanelet_map_bin (autoware_lanelet2_msgs/MapBin) : binary data of Lanelet2 Map
+
+### Published Topics
+/lanelet2_map_viz (visualization_msgs/MarkerArray) : visualization messages for RVIZ

src/ros/catkin/src/map_file/include/map_file/get_file.h

@@ -0,0 +1,46 @@
+/*
+ * 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 _GET_FILE_H_
+#define _GET_FILE_H_
+
+#include <cstdint>
+#include <string>
+#include <netinet/in.h>
+
+#define HTTP_HOSTNAME     "133.6.148.90"
+#define HTTP_PORT         (80)
+#define HTTP_USER         ""
+#define HTTP_PASSWORD     ""
+
+class GetFile {
+private:
+  std::string host_name_;
+  int port_;
+  std::string user_;
+  std::string password_;
+  int sock;
+  struct sockaddr_in server;
+
+public:
+  GetFile();
+  explicit GetFile(const std::string& host_name, int port, const std::string& user, const std::string& password);
+
+  int GetHTTPFile(const std::string& value);
+};
+
+
+#endif /* _GET_FILE_H_ */

src/ros/catkin/src/map_file/include/map_file/points_map_filter.h

@@ -0,0 +1,82 @@
+/*
+ *  Copyright (c) 2018, TierIV, Inc
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice,
+ * this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  * Neither the name of Autoware nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE
+ *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY,
+ *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
+ * USE
+ *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef POINTS_MAP_FILTER_H_INCLUDED
+#define POINTS_MAP_FILTER_H_INCLUDED
+
+// headers in ROS
+#include <geometry_msgs/PoseStamped.h>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+
+// headers in PCL
+#include <pcl/filters/passthrough.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/point_cloud.h>
+
+// headers in boost
+#include <boost/optional.hpp>
+
+// headers in STL
+#include <mutex>
+#include <regex>
+
+class points_map_filter {
+public:
+  points_map_filter(ros::NodeHandle nh, ros::NodeHandle pnh);
+  ~points_map_filter();
+  void init();
+  void run();
+
+private:
+  pcl::PassThrough<pcl::PointXYZ> pass_;
+  ros::Subscriber map_sub_;
+  ros::Subscriber pose_sub_;
+  ros::Publisher map_pub_;
+  std::mutex mtx_;
+  ros::NodeHandle nh_, pnh_;
+  void map_callback_(const sensor_msgs::PointCloud2::ConstPtr msg);
+  void current_pose_callback_(const geometry_msgs::PoseStamped::ConstPtr msg);
+  double load_grid_size_;
+  double load_trigger_distance_;
+  std::string map_frame_;
+  boost::optional<geometry_msgs::PoseStamped> last_load_pose_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr map_cloud_;
+  volatile bool map_recieved_;
+};
+
+#endif // POINTS_MAP_FILTER_H_INCLUDED

src/ros/catkin/src/map_file/interface.yaml

@@ -0,0 +1,5 @@
+- name: /points_map_loader
+  publish: [/points_map, /pmap_stat]
+  subscribe: [/gnss_pose, /current_pose, /initialpose, /traffic_waypoints_array]
+- name: /vector_map_loader
+  publish: [/vector_map, /vmap_stat, /vector_map_info/*]

src/ros/catkin/src/map_file/launch/vector_map_loader.launch

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+  <node pkg="map_file" type="vector_map_loader" name="vector_map_loader" output="screen">
+    <param name="load_mode" value="directory" />
+    <param name="map_dir" value="$(env HOME)/map/vector_map" />
+    <param name="host_name" value="133.6.148.90" />
+    <param name="port" value="80" />
+    <param name="user" value="" />
+    <param name="password" value="" />
+  </node>
+</launch>

src/ros/catkin/src/map_file/nodes/vector_map_loader/vector_map_loader.cpp

@@ -0,0 +1,1312 @@
+/*
+ * 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/console.h>
+#include <std_msgs/Bool.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <vector_map/vector_map.h>
+#include <map_file/get_file.h>
+#include <sys/stat.h>
+
+using vector_map::VectorMap;
+using vector_map::Category;
+using vector_map::Color;
+using vector_map::Key;
+
+using vector_map::Point;
+using vector_map::Vector;
+using vector_map::Line;
+using vector_map::Area;
+using vector_map::Pole;
+using vector_map::Box;
+using vector_map::DTLane;
+using vector_map::Node;
+using vector_map::Lane;
+using vector_map::WayArea;
+using vector_map::RoadEdge;
+using vector_map::Gutter;
+using vector_map::Curb;
+using vector_map::WhiteLine;
+using vector_map::StopLine;
+using vector_map::ZebraZone;
+using vector_map::CrossWalk;
+using vector_map::RoadMark;
+using vector_map::RoadPole;
+using vector_map::RoadSign;
+using vector_map::Signal;
+using vector_map::StreetLight;
+using vector_map::UtilityPole;
+using vector_map::GuardRail;
+using vector_map::SideWalk;
+using vector_map::DriveOnPortion;
+using vector_map::CrossRoad;
+using vector_map::SideStrip;
+using vector_map::CurveMirror;
+using vector_map::Wall;
+using vector_map::Fence;
+using vector_map::RailCrossing;
+
+using vector_map::PointArray;
+using vector_map::VectorArray;
+using vector_map::LineArray;
+using vector_map::AreaArray;
+using vector_map::PoleArray;
+using vector_map::BoxArray;
+using vector_map::DTLaneArray;
+using vector_map::NodeArray;
+using vector_map::LaneArray;
+using vector_map::WayAreaArray;
+using vector_map::RoadEdgeArray;
+using vector_map::GutterArray;
+using vector_map::CurbArray;
+using vector_map::WhiteLineArray;
+using vector_map::StopLineArray;
+using vector_map::ZebraZoneArray;
+using vector_map::CrossWalkArray;
+using vector_map::RoadMarkArray;
+using vector_map::RoadPoleArray;
+using vector_map::RoadSignArray;
+using vector_map::SignalArray;
+using vector_map::StreetLightArray;
+using vector_map::UtilityPoleArray;
+using vector_map::GuardRailArray;
+using vector_map::SideWalkArray;
+using vector_map::DriveOnPortionArray;
+using vector_map::CrossRoadArray;
+using vector_map::SideStripArray;
+using vector_map::CurveMirrorArray;
+using vector_map::WallArray;
+using vector_map::FenceArray;
+using vector_map::RailCrossingArray;
+
+using vector_map::isValidMarker;
+using vector_map::createVectorMarker;
+using vector_map::createAreaMarker;
+using vector_map::createPoleMarker;
+
+namespace
+{
+
+enum class LoadMode {
+  FILE,
+  DIRECTORY,
+  DOWNLOAD
+};
+
+void printUsage()
+{
+  ROS_ERROR_STREAM("Usage:");
+  ROS_ERROR_STREAM("rosrun map_file vector_map_loader [CSV]...");
+  ROS_ERROR_STREAM("rosrun map_file vector_map_loader download [X] [Y]");
+}
+
+bool isDownloaded(const std::string& local_path)
+{
+  struct stat st;
+  return stat(local_path.c_str(), &st) == 0;
+}
+
+template <class T, class U>
+vector_map::category_t registerVectormapPortion(
+  const std::string& file_path, ros::Publisher *publisher,
+  const std::string topic_name, const vector_map::category_t category,
+  ros::NodeHandle *nh)
+{
+  U obj_array;
+  obj_array.header.frame_id = "map";
+  obj_array.data = vector_map::parse<T>(file_path);
+  if (!obj_array.data.empty())
+  {
+    *publisher = nh->advertise<U>(topic_name, 1, true);
+    publisher->publish(obj_array);
+    return category;
+  }
+  else
+  {
+    return Category::NONE;
+  }
+}
+
+visualization_msgs::Marker createLinkedLineMarker(const std::string& ns, int id, Color color, const VectorMap& vmap,
+                                                  const Line& line)
+{
+  Area area;
+  area.aid = 1; // must set valid aid
+  area.slid = line.lid;
+  return createAreaMarker(ns, id, color, vmap, area);
+}
+
+visualization_msgs::MarkerArray createRoadEdgeMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& road_edge : vmap.findByFilter([](const RoadEdge& road_edge){return true;}))
+  {
+    if (road_edge.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createRoadEdgeMarkerArray] invalid road_edge: " << road_edge);
+      continue;
+    }
+
+    Line line = vmap.findByKey(Key<Line>(road_edge.lid));
+    if (line.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createRoadEdgeMarkerArray] invalid line: " << line);
+      continue;
+    }
+
+    if (line.blid == 0) // if beginning line
+    {
+      visualization_msgs::Marker marker = createLinkedLineMarker("road_edge", id++, color, vmap, line);
+      if (isValidMarker(marker))
+        marker_array.markers.push_back(marker);
+      else
+        ROS_ERROR_STREAM("[createRoadEdgeMarkerArray] failed createLinkedLineMarker: " << line);
+    }
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createGutterMarkerArray(const VectorMap& vmap, Color no_cover_color,
+                                                        Color cover_color, Color grating_color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& gutter : vmap.findByFilter([](const Gutter& gutter){return true;}))
+  {
+    if (gutter.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createGutterMarkerArray] invalid gutter: " << gutter);
+      continue;
+    }
+
+    Area area = vmap.findByKey(Key<Area>(gutter.aid));
+    if (area.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createGutterMarkerArray] invalid area: " << area);
+      continue;
+    }
+
+    visualization_msgs::Marker marker;
+    switch (gutter.type)
+    {
+    case Gutter::NO_COVER:
+      marker = createAreaMarker("gutter", id++, no_cover_color, vmap, area);
+      break;
+    case Gutter::COVER:
+      marker = createAreaMarker("gutter", id++, cover_color, vmap, area);
+      break;
+    case Gutter::GRATING:
+      marker = createAreaMarker("gutter", id++, grating_color, vmap, area);
+      break;
+    default:
+      ROS_ERROR_STREAM("[createGutterMarkerArray] unknown gutter.type: " << gutter);
+      continue;
+    }
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createGutterMarkerArray] failed createAreaMarker: " << area);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createCurbMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& curb : vmap.findByFilter([](const Curb& curb){return true;}))
+  {
+    if (curb.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createCurbMarkerArray] invalid curb: " << curb);
+      continue;
+    }
+
+    Line line = vmap.findByKey(Key<Line>(curb.lid));
+    if (line.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createCurbMarkerArray] invalid line: " << line);
+      continue;
+    }
+
+    if (line.blid == 0) // if beginning line
+    {
+      visualization_msgs::Marker marker = createLinkedLineMarker("curb", id++, color, vmap, line);
+      // XXX: The visualization_msgs::Marker::LINE_STRIP is difficult to deal with curb.width and curb.height.
+      if (isValidMarker(marker))
+        marker_array.markers.push_back(marker);
+      else
+        ROS_ERROR_STREAM("[createCurbMarkerArray] failed createLinkedLineMarker: " << line);
+    }
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createWhiteLineMarkerArray(const VectorMap& vmap, Color white_color,
+                                                           Color yellow_color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& white_line : vmap.findByFilter([](const WhiteLine& white_line){return true;}))
+  {
+    if (white_line.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createWhiteLineMarkerArray] invalid white_line: " << white_line);
+      continue;
+    }
+    if (white_line.type == WhiteLine::DASHED_LINE_BLANK) // if invisible line
+      continue;
+
+    Line line = vmap.findByKey(Key<Line>(white_line.lid));
+    if (line.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createWhiteLineMarkerArray] invalid line: " << line);
+      continue;
+    }
+
+    if (line.blid == 0) // if beginning line
+    {
+      visualization_msgs::Marker marker;
+      switch (white_line.color)
+      {
+      case 'W':
+        marker = createLinkedLineMarker("white_line", id++, white_color, vmap, line);
+        break;
+      case 'Y':
+        marker = createLinkedLineMarker("white_line", id++, yellow_color, vmap, line);
+        break;
+      default:
+        ROS_ERROR_STREAM("[createWhiteLineMarkerArray] unknown white_line.color: " << white_line);
+        continue;
+      }
+      // XXX: The visualization_msgs::Marker::LINE_STRIP is difficult to deal with white_line.width.
+      if (isValidMarker(marker))
+        marker_array.markers.push_back(marker);
+      else
+        ROS_ERROR_STREAM("[createWhiteLineMarkerArray] failed createLinkedLineMarker: " << line);
+    }
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createStopLineMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& stop_line : vmap.findByFilter([](const StopLine& stop_line){return true;}))
+  {
+    if (stop_line.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createStopLineMarkerArray] invalid stop_line: " << stop_line);
+      continue;
+    }
+
+    Line line = vmap.findByKey(Key<Line>(stop_line.lid));
+    if (line.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createStopLineMarkerArray] invalid line: " << line);
+      continue;
+    }
+
+    if (line.blid == 0) // if beginning line
+    {
+      visualization_msgs::Marker marker = createLinkedLineMarker("stop_line", id++, color, vmap, line);
+      if (isValidMarker(marker))
+        marker_array.markers.push_back(marker);
+      else
+        ROS_ERROR_STREAM("[createStopLineMarkerArray] failed createLinkedLineMarker: " << line);
+    }
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createZebraZoneMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& zebra_zone : vmap.findByFilter([](const ZebraZone& zebra_zone){return true;}))
+  {
+    if (zebra_zone.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createZebraZoneMarkerArray] invalid zebra_zone: " << zebra_zone);
+      continue;
+    }
+
+    Area area = vmap.findByKey(Key<Area>(zebra_zone.aid));
+    if (area.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createZebraZoneMarkerArray] invalid area: " << area);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createAreaMarker("zebra_zone", id++, color, vmap, area);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createZebraZoneMarkerArray] failed createAreaMarker: " << area);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createCrossWalkMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& cross_walk : vmap.findByFilter([](const CrossWalk& cross_walk){return true;}))
+  {
+    if (cross_walk.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createCrossWalkMarkerArray] invalid cross_walk: " << cross_walk);
+      continue;
+    }
+
+    Area area = vmap.findByKey(Key<Area>(cross_walk.aid));
+    if (area.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createCrossWalkMarkerArray] invalid area: " << area);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createAreaMarker("cross_walk", id++, color, vmap, area);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createCrossWalkMarkerArray] failed createAreaMarker: " << area);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createRoadMarkMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& road_mark : vmap.findByFilter([](const RoadMark& road_mark){return true;}))
+  {
+    if (road_mark.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createRoadMarkMarkerArray] invalid road_mark: " << road_mark);
+      continue;
+    }
+
+    Area area = vmap.findByKey(Key<Area>(road_mark.aid));
+    if (area.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createRoadMarkMarkerArray] invalid area: " << area);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createAreaMarker("road_mark", id++, color, vmap, area);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createRoadMarkMarkerArray] failed createAreaMarker: " << area);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createRoadPoleMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& road_pole : vmap.findByFilter([](const RoadPole& road_pole){return true;}))
+  {
+    if (road_pole.plid == 0)
+    {
+      ROS_ERROR_STREAM("[createRoadPoleMarkerArray] invalid road_pole: " << road_pole);
+      continue;
+    }
+
+    Pole pole = vmap.findByKey(Key<Pole>(road_pole.plid));
+    if (pole.plid == 0)
+    {
+      ROS_ERROR_STREAM("[createRoadPoleMarkerArray] invalid pole: " << pole);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createPoleMarker("road_pole", id++, color, vmap, pole);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createRoadPoleMarkerArray] failed createPoleMarker: " << pole);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createRoadSignMarkerArray(const VectorMap& vmap, Color sign_color, Color pole_color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& road_sign : vmap.findByFilter([](const RoadSign& road_sign){return true;}))
+  {
+    if (road_sign.vid == 0)
+    {
+      ROS_ERROR_STREAM("[createRoadSignMarkerArray] invalid road_sign: " << road_sign);
+      continue;
+    }
+
+    Vector vector = vmap.findByKey(Key<Vector>(road_sign.vid));
+    if (vector.vid == 0)
+    {
+      ROS_ERROR_STREAM("[createRoadSignMarkerArray] invalid vector: " << vector);
+      continue;
+    }
+
+    Pole pole;
+    if (road_sign.plid != 0)
+    {
+      pole = vmap.findByKey(Key<Pole>(road_sign.plid));
+      if (pole.plid == 0)
+      {
+        ROS_ERROR_STREAM("[createRoadSignMarkerArray] invalid pole: " << pole);
+        continue;
+      }
+    }
+
+    visualization_msgs::Marker vector_marker = createVectorMarker("road_sign", id++, sign_color, vmap, vector);
+    if (isValidMarker(vector_marker))
+      marker_array.markers.push_back(vector_marker);
+    else
+      ROS_ERROR_STREAM("[createRoadSignMarkerArray] failed createVectorMarker: " << vector);
+
+    if (road_sign.plid != 0)
+    {
+      visualization_msgs::Marker pole_marker = createPoleMarker("road_sign", id++, pole_color, vmap, pole);
+      if (isValidMarker(pole_marker))
+        marker_array.markers.push_back(pole_marker);
+      else
+        ROS_ERROR_STREAM("[createRoadSignMarkerArray] failed createPoleMarker: " << pole);
+    }
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createSignalMarkerArray(const VectorMap& vmap, Color red_color, Color blue_color,
+                                                        Color yellow_color, Color other_color, Color pole_color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& signal : vmap.findByFilter([](const Signal& signal){return true;}))
+  {
+    if (signal.vid == 0)
+    {
+      ROS_ERROR_STREAM("[createSignalMarkerArray] invalid signal: " << signal);
+      continue;
+    }
+
+    Vector vector = vmap.findByKey(Key<Vector>(signal.vid));
+    if (vector.vid == 0)
+    {
+      ROS_ERROR_STREAM("[createSignalMarkerArray] invalid vector: " << vector);
+      continue;
+    }
+
+    Pole pole;
+    if (signal.plid != 0)
+    {
+      pole = vmap.findByKey(Key<Pole>(signal.plid));
+      if (pole.plid == 0)
+      {
+        ROS_ERROR_STREAM("[createSignalMarkerArray] invalid pole: " << pole);
+        continue;
+      }
+    }
+
+    visualization_msgs::Marker vector_marker;
+    switch (signal.type)
+    {
+    case Signal::RED:
+    case Signal::PEDESTRIAN_RED:
+      vector_marker = createVectorMarker("signal", id++, red_color, vmap, vector);
+      break;
+    case Signal::BLUE:
+    case Signal::PEDESTRIAN_BLUE:
+      vector_marker = createVectorMarker("signal", id++, blue_color, vmap, vector);
+      break;
+    case Signal::YELLOW:
+      vector_marker = createVectorMarker("signal", id++, yellow_color, vmap, vector);
+      break;
+    case Signal::RED_LEFT:
+      vector_marker = createVectorMarker("signal", id++, Color::LIGHT_RED, vmap, vector);
+          break;
+    case Signal::BLUE_LEFT:
+      vector_marker = createVectorMarker("signal", id++, Color::LIGHT_GREEN, vmap, vector);
+          break;
+    case Signal::YELLOW_LEFT:
+      vector_marker = createVectorMarker("signal", id++, Color::LIGHT_YELLOW, vmap, vector);
+          break;
+    case Signal::OTHER:
+      vector_marker = createVectorMarker("signal", id++, other_color, vmap, vector);
+      break;
+    default:
+      ROS_WARN_STREAM("[createSignalMarkerArray] unknown signal.type: " << signal.type << " Creating Marker as OTHER.");
+      vector_marker = createVectorMarker("signal", id++, Color::GRAY, vmap, vector);
+      break;
+    }
+    if (isValidMarker(vector_marker))
+      marker_array.markers.push_back(vector_marker);
+    else
+      ROS_ERROR_STREAM("[createSignalMarkerArray] failed createVectorMarker: " << vector);
+
+    if (signal.plid != 0)
+    {
+      visualization_msgs::Marker pole_marker = createPoleMarker("signal", id++, pole_color, vmap, pole);
+      if (isValidMarker(pole_marker))
+        marker_array.markers.push_back(pole_marker);
+      else
+        ROS_ERROR_STREAM("[createSignalMarkerArray] failed createPoleMarker: " << pole);
+    }
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createStreetLightMarkerArray(const VectorMap& vmap, Color light_color,
+                                                             Color pole_color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& street_light : vmap.findByFilter([](const StreetLight& street_light){return true;}))
+  {
+    if (street_light.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createStreetLightMarkerArray] invalid street_light: " << street_light);
+      continue;
+    }
+
+    Line line = vmap.findByKey(Key<Line>(street_light.lid));
+    if (line.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createStreetLightMarkerArray] invalid line: " << line);
+      continue;
+    }
+
+    Pole pole;
+    if (street_light.plid != 0)
+    {
+      pole = vmap.findByKey(Key<Pole>(street_light.plid));
+      if (pole.plid == 0)
+      {
+        ROS_ERROR_STREAM("[createStreetLightMarkerArray] invalid pole: " << pole);
+        continue;
+      }
+    }
+
+    if (line.blid == 0) // if beginning line
+    {
+      visualization_msgs::Marker line_marker = createLinkedLineMarker("street_light", id++, light_color, vmap, line);
+      if (isValidMarker(line_marker))
+        marker_array.markers.push_back(line_marker);
+      else
+        ROS_ERROR_STREAM("[createStreetLightMarkerArray] failed createLinkedLineMarker: " << line);
+    }
+
+    if (street_light.plid != 0)
+    {
+      visualization_msgs::Marker pole_marker = createPoleMarker("street_light", id++, pole_color, vmap, pole);
+      if (isValidMarker(pole_marker))
+        marker_array.markers.push_back(pole_marker);
+      else
+        ROS_ERROR_STREAM("[createStreetLightMarkerArray] failed createPoleMarker: " << pole);
+    }
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createUtilityPoleMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& utility_pole : vmap.findByFilter([](const UtilityPole& utility_pole){return true;}))
+  {
+    if (utility_pole.plid == 0)
+    {
+      ROS_ERROR_STREAM("[createUtilityPoleMarkerArray] invalid utility_pole: " << utility_pole);
+      continue;
+    }
+
+    Pole pole = vmap.findByKey(Key<Pole>(utility_pole.plid));
+    if (pole.plid == 0)
+    {
+      ROS_ERROR_STREAM("[createUtilityPoleMarkerArray] invalid pole: " << pole);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createPoleMarker("utility_pole", id++, color, vmap, pole);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createUtilityPoleMarkerArray] failed createPoleMarker: " << pole);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createGuardRailMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& guard_rail : vmap.findByFilter([](const GuardRail& guard_rail){return true;}))
+  {
+    if (guard_rail.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createGuardRailMarkerArray] invalid guard_rail: " << guard_rail);
+      continue;
+    }
+
+    Area area = vmap.findByKey(Key<Area>(guard_rail.aid));
+    if (area.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createGuardRailMarkerArray] invalid area: " << area);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createAreaMarker("guard_rail", id++, color, vmap, area);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createGuardRailMarkerArray] failed createAreaMarker: " << area);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createSideWalkMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& side_walk : vmap.findByFilter([](const SideWalk& side_walk){return true;}))
+  {
+    if (side_walk.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createSideWalkMarkerArray] invalid side_walk: " << side_walk);
+      continue;
+    }
+
+    Area area = vmap.findByKey(Key<Area>(side_walk.aid));
+    if (area.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createSideWalkMarkerArray] invalid area: " << area);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createAreaMarker("side_walk", id++, color, vmap, area);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createSideWalkMarkerArray] failed createAreaMarker: " << area);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createDriveOnPortionMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& drive_on_portion : vmap.findByFilter([](const DriveOnPortion& drive_on_portion){return true;}))
+  {
+    if (drive_on_portion.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createDriveOnPortionMarkerArray] invalid drive_on_portion: " << drive_on_portion);
+      continue;
+    }
+
+    Area area = vmap.findByKey(Key<Area>(drive_on_portion.aid));
+    if (area.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createDriveOnPortionMarkerArray] invalid area: " << area);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createAreaMarker("drive_on_portion", id++, color, vmap, area);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createDriveOnPortionMarkerArray] failed createAreaMarker: " << area);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createCrossRoadMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& cross_road : vmap.findByFilter([](const CrossRoad& cross_road){return true;}))
+  {
+    if (cross_road.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createCrossRoadMarkerArray] invalid cross_road: " << cross_road);
+      continue;
+    }
+
+    Area area = vmap.findByKey(Key<Area>(cross_road.aid));
+    if (area.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createCrossRoadMarkerArray] invalid area: " << area);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createAreaMarker("cross_road", id++, color, vmap, area);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createCrossRoadMarkerArray] failed createAreaMarker: " << area);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createSideStripMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& side_strip : vmap.findByFilter([](const SideStrip& side_strip){return true;}))
+  {
+    if (side_strip.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createSideStripMarkerArray] invalid side_strip: " << side_strip);
+      continue;
+    }
+
+    Line line = vmap.findByKey(Key<Line>(side_strip.lid));
+    if (line.lid == 0)
+    {
+      ROS_ERROR_STREAM("[createSideStripMarkerArray] invalid line: " << line);
+      continue;
+    }
+
+    if (line.blid == 0) // if beginning line
+    {
+      visualization_msgs::Marker marker = createLinkedLineMarker("side_strip", id++, color, vmap, line);
+      if (isValidMarker(marker))
+        marker_array.markers.push_back(marker);
+      else
+        ROS_ERROR_STREAM("[createSideStripMarkerArray] failed createLinkedLineMarker: " << line);
+    }
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createCurveMirrorMarkerArray(const VectorMap& vmap, Color mirror_color,
+                                                             Color pole_color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& curve_mirror : vmap.findByFilter([](const CurveMirror& curve_mirror){return true;}))
+  {
+    if (curve_mirror.vid == 0 || curve_mirror.plid == 0)
+    {
+      ROS_ERROR_STREAM("[createCurveMirrorMarkerArray] invalid curve_mirror: " << curve_mirror);
+      continue;
+    }
+
+    Vector vector = vmap.findByKey(Key<Vector>(curve_mirror.vid));
+    if (vector.vid == 0)
+    {
+      ROS_ERROR_STREAM("[createCurveMirrorMarkerArray] invalid vector: " << vector);
+      continue;
+    }
+
+    Pole pole = vmap.findByKey(Key<Pole>(curve_mirror.plid));
+    if (pole.plid == 0)
+    {
+      ROS_ERROR_STREAM("[createCurveMirrorMarkerArray] invalid pole: " << pole);
+      continue;
+    }
+
+    visualization_msgs::Marker vector_marker = createVectorMarker("curve_mirror", id++, mirror_color, vmap, vector);
+    if (isValidMarker(vector_marker))
+      marker_array.markers.push_back(vector_marker);
+    else
+      ROS_ERROR_STREAM("[createCurveMirrorMarkerArray] failed createVectorMarker: " << vector);
+
+    visualization_msgs::Marker pole_marker = createPoleMarker("curve_mirror", id++, pole_color, vmap, pole);
+    if (isValidMarker(pole_marker))
+      marker_array.markers.push_back(pole_marker);
+    else
+      ROS_ERROR_STREAM("[createCurveMirrorMarkerArray] failed createPoleMarker: " << pole);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createWallMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& wall : vmap.findByFilter([](const Wall& wall){return true;}))
+  {
+    if (wall.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createWallMarkerArray] invalid wall: " << wall);
+      continue;
+    }
+
+    Area area = vmap.findByKey(Key<Area>(wall.aid));
+    if (area.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createWallMarkerArray] invalid area: " << area);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createAreaMarker("wall", id++, color, vmap, area);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createWallMarkerArray] failed createAreaMarker: " << area);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createFenceMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& fence : vmap.findByFilter([](const Fence& fence){return true;}))
+  {
+    if (fence.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createFenceMarkerArray] invalid fence: " << fence);
+      continue;
+    }
+
+    Area area = vmap.findByKey(Key<Area>(fence.aid));
+    if (area.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createFenceMarkerArray] invalid area: " << area);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createAreaMarker("fence", id++, color, vmap, area);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createFenceMarkerArray] failed createAreaMarker: " << area);
+  }
+  return marker_array;
+}
+
+visualization_msgs::MarkerArray createRailCrossingMarkerArray(const VectorMap& vmap, Color color)
+{
+  visualization_msgs::MarkerArray marker_array;
+  int id = 0;
+  for (const auto& rail_crossing : vmap.findByFilter([](const RailCrossing& rail_crossing){return true;}))
+  {
+    if (rail_crossing.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createRailCrossingMarkerArray] invalid rail_crossing: " << rail_crossing);
+      continue;
+    }
+
+    Area area = vmap.findByKey(Key<Area>(rail_crossing.aid));
+    if (area.aid == 0)
+    {
+      ROS_ERROR_STREAM("[createRailCrossingMarkerArray] invalid area: " << area);
+      continue;
+    }
+
+    visualization_msgs::Marker marker = createAreaMarker("rail_crossing", id++, color, vmap, area);
+    if (isValidMarker(marker))
+      marker_array.markers.push_back(marker);
+    else
+      ROS_ERROR_STREAM("[createRailCrossingMarkerArray] failed createAreaMarker: " << area);
+  }
+  return marker_array;
+}
+
+void insertMarkerArray(visualization_msgs::MarkerArray& a1, const visualization_msgs::MarkerArray& a2)
+{
+  a1.markers.insert(a1.markers.end(), a2.markers.begin(), a2.markers.end());
+}
+} // namespace
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "vector_map_loader");
+  ros::NodeHandle nh;
+  ros::NodeHandle pnh("~");
+
+  LoadMode load_mode = LoadMode::FILE;
+  if (pnh.hasParam("load_mode"))
+  {
+    // Set mode based on rosparam
+    std::string load_mode_param;
+    pnh.getParam("load_mode", load_mode_param);
+    if (load_mode_param == "file")
+    {
+      load_mode = LoadMode::FILE;
+    }
+    else if (load_mode_param == "directory")
+    {
+      load_mode = LoadMode::DIRECTORY;
+    }
+    else if (load_mode_param == "download")
+    {
+      load_mode = LoadMode::DOWNLOAD;
+    }
+    else
+    {
+      printUsage();
+    }
+  }
+  else
+  {
+    // Set mode based on args
+    if (argc < 2)
+    {
+      printUsage();
+      ros::shutdown();
+    }
+
+    if (strcmp(argv[1], "download") == 0)
+    {
+      load_mode = LoadMode::DOWNLOAD;
+      if (argc < 4)
+      {
+        printUsage();
+        ros::shutdown();
+      }
+    }
+  }
+
+  // Directory containing vector map csvs
+  std::string map_dir;
+  pnh.param<std::string>("map_dir", map_dir, "");
+
+  // Vector map publishers will be initialized later as data is loaded.
+  ros::Publisher area_pub;
+  ros::Publisher box_pub;
+  ros::Publisher cross_road_pub;
+  ros::Publisher cross_walk_pub;
+  ros::Publisher curb_pub;
+  ros::Publisher curve_mirror_pub;
+  ros::Publisher drive_on_portion_pub;
+  ros::Publisher dtlane_pub;
+  ros::Publisher fence_pub;
+  ros::Publisher guard_rail_pub;
+  ros::Publisher gutter_pub;
+  ros::Publisher lane_pub;
+  ros::Publisher line_pub;
+  ros::Publisher node_pub;
+  ros::Publisher point_pub;
+  ros::Publisher pole_pub;
+  ros::Publisher rail_crossing_pub;
+  ros::Publisher road_edge_pub;
+  ros::Publisher road_mark_pub;
+  ros::Publisher road_pole_pub;
+  ros::Publisher road_sign_pub;
+  ros::Publisher side_strip_pub;
+  ros::Publisher side_walk_pub;
+  ros::Publisher signal_pub;
+  ros::Publisher stop_line_pub;
+  ros::Publisher street_light_pub;
+  ros::Publisher utility_pole_pub;
+  ros::Publisher vector_pub;
+  ros::Publisher wall_pub;
+  ros::Publisher way_area_pub;
+  ros::Publisher white_line_pub;
+  ros::Publisher zebra_zone_pub;
+
+  ros::Publisher marker_array_pub = nh.advertise<visualization_msgs::MarkerArray>("vector_map", 1, true);
+  ros::Publisher stat_pub = nh.advertise<std_msgs::Bool>("vmap_stat", 1, true);
+
+  std_msgs::Bool stat;
+  stat.data = false;
+  stat_pub.publish(stat);
+
+  std::vector<std::string> file_paths;
+  std::vector<std::string> file_names
+  {
+    "area.csv",
+    "box.csv",
+    "crosswalk.csv",
+    "curb.csv",
+    "curvemirror.csv",
+    "driveon_portion.csv",
+    "dtlane.csv",
+    "fence.csv",
+    "guardrail.csv",
+    "gutter.csv",
+    "idx.csv",
+    "intersection.csv",
+    "lane.csv",
+    "line.csv",
+    "node.csv",
+    "point.csv",
+    "pole.csv",
+    "poledata.csv",
+    "railroad_crossing.csv",
+    "road_surface_mark.csv",
+    "roadedge.csv",
+    "roadsign.csv",
+    "sidestrip.csv",
+    "sidewalk.csv",
+    "signaldata.csv",
+    "stopline.csv",
+    "streetlight.csv",
+    "utilitypole.csv",
+    "vector.csv",
+    "wall.csv",
+    "wayarea.csv",
+    "whiteline.csv",
+    "zebrazone.csv"
+  };
+
+  if (load_mode == LoadMode::DOWNLOAD)
+  {
+    ROS_INFO("Load Mode: Download");
+    std::string host_name;
+    pnh.param<std::string>("host_name", host_name, HTTP_HOSTNAME);
+    int port;
+    pnh.param<int>("port", port, HTTP_PORT);
+    std::string user;
+    pnh.param<std::string>("user", user, HTTP_USER);
+    std::string password;
+    pnh.param<std::string>("password", password, HTTP_PASSWORD);
+    GetFile gf = GetFile(host_name, port, user, password);
+
+    std::string remote_path = "/data/map";
+    int x = std::atoi(argv[2]);
+    int y = std::atoi(argv[3]);
+    x -= x % 1000 + 1000;
+    y -= y % 1000 + 1000;
+    remote_path += std::to_string(x) + "/" + std::to_string(y) + "/vector";
+
+    std::string local_path = "/tmp" + remote_path;
+    if (!isDownloaded(local_path))
+    {
+      std::istringstream iss(remote_path);
+      std::string column;
+      std::getline(iss, column, '/');
+      std::string mkdir_path = "/tmp";
+      while (std::getline(iss, column, '/'))
+      {
+        mkdir_path += "/" + column;
+        mkdir(mkdir_path.c_str(), 0755);
+      }
+    }
+
+    for (const auto& file_name : file_names)
+    {
+      if (gf.GetHTTPFile(remote_path + "/" + file_name) == 0)
+        file_paths.push_back("/tmp" + remote_path + "/" + file_name);
+      else
+        ROS_ERROR_STREAM("download failure: " << remote_path + "/" + file_name);
+    }
+  }
+  else if (load_mode == LoadMode::DIRECTORY)
+  {
+    ROS_INFO("Load Mode: Directory");
+    // add slash if it doesn't exist
+    if (map_dir.back() != '/')
+    {
+      map_dir.append("/");
+    }
+
+    // Add all possible file paths for csv files.
+    for (auto file_name : file_names)
+    {
+      std::string file_path = map_dir;
+      file_path.append(file_name);
+      file_paths.push_back(file_path);
+    }
+  }
+  else if (load_mode == LoadMode::FILE)
+  {
+    ROS_INFO("Load Mode: File");
+    for (int i = 1; i < argc; ++i)
+    {
+      std::string file_path(argv[i]);
+      file_paths.push_back(file_path);
+    }
+  }
+
+  vector_map::category_t category = Category::NONE;
+  for (const auto& file_path : file_paths)
+  {
+    std::string file_name(basename(file_path.c_str()));
+    if (file_name == "idx.csv")
+    {
+      ; // XXX: This version of Autoware don't support index csv file now.
+    }
+    else if (file_name == "point.csv")
+    {
+      category |= registerVectormapPortion<Point, PointArray>(file_path, &point_pub, "vector_map_info/point", Category::POINT, &nh);
+    }
+    else if (file_name == "vector.csv")
+    {
+      category |= registerVectormapPortion<Vector, VectorArray>(file_path, &vector_pub, "vector_map_info/vector", Category::VECTOR, &nh);
+    }
+    else if (file_name == "line.csv")
+    {
+      category |= registerVectormapPortion<Line, LineArray>(file_path, &line_pub, "vector_map_info/line", Category::LINE, &nh);
+    }
+    else if (file_name == "area.csv")
+    {
+      category |= registerVectormapPortion<Area, AreaArray>(file_path, &area_pub, "vector_map_info/area", Category::AREA, &nh);
+    }
+    else if (file_name == "pole.csv")
+    {
+      category |= registerVectormapPortion<Pole, PoleArray>(file_path, &pole_pub, "vector_map_info/pole", Category::POLE, &nh);
+    }
+    else if (file_name == "box.csv")
+    {
+      category |= registerVectormapPortion<Box, BoxArray>(file_path, &box_pub, "vector_map_info/box", Category::BOX, &nh);
+    }
+    else if (file_name == "dtlane.csv")
+    {
+      category |= registerVectormapPortion<DTLane, DTLaneArray>(file_path, &dtlane_pub, "vector_map_info/dtlane", Category::DTLANE, &nh);
+    }
+    else if (file_name == "node.csv")
+    {
+      category |= registerVectormapPortion<Node, NodeArray>(file_path, &node_pub, "vector_map_info/node", Category::NODE, &nh);
+    }
+    else if (file_name == "lane.csv")
+    {
+      category |= registerVectormapPortion<Lane, LaneArray>(file_path, &lane_pub, "vector_map_info/lane", Category::LANE, &nh);
+    }
+    else if (file_name == "wayarea.csv")
+    {
+      category |= registerVectormapPortion<WayArea, WayAreaArray>(file_path, &way_area_pub, "vector_map_info/way_area", Category::WAY_AREA, &nh);
+    }
+    else if (file_name == "roadedge.csv")
+    {
+      category |= registerVectormapPortion<RoadEdge, RoadEdgeArray>(file_path, &road_edge_pub, "vector_map_info/road_edge", Category::ROAD_EDGE, &nh);
+    }
+    else if (file_name == "gutter.csv")
+    {
+      category |= registerVectormapPortion<Gutter, GutterArray>(file_path, &gutter_pub, "vector_map_info/gutter", Category::GUTTER, &nh);
+    }
+    else if (file_name == "curb.csv")
+    {
+      category |= registerVectormapPortion<Curb, CurbArray>(file_path, &curb_pub, "vector_map_info/curb", Category::CURB, &nh);
+    }
+    else if (file_name == "whiteline.csv")
+    {
+      category |= registerVectormapPortion<WhiteLine, WhiteLineArray>(file_path, &white_line_pub, "vector_map_info/white_line", Category::WHITE_LINE, &nh);
+    }
+    else if (file_name == "stopline.csv")
+    {
+      category |= registerVectormapPortion<StopLine, StopLineArray>(file_path, &stop_line_pub, "vector_map_info/stop_line", Category::STOP_LINE, &nh);
+    }
+    else if (file_name == "zebrazone.csv")
+    {
+      category |= registerVectormapPortion<ZebraZone, ZebraZoneArray>(file_path, &zebra_zone_pub, "vector_map_info/zebra_zone", Category::ZEBRA_ZONE, &nh);
+    }
+    else if (file_name == "crosswalk.csv")
+    {
+      category |= registerVectormapPortion<CrossWalk, CrossWalkArray>(file_path, &cross_walk_pub, "vector_map_info/cross_walk", Category::CROSS_WALK, &nh);
+    }
+    else if (file_name == "road_surface_mark.csv")
+    {
+      category |= registerVectormapPortion<RoadMark, RoadMarkArray>(file_path, &road_mark_pub, "vector_map_info/road_mark", Category::ROAD_MARK, &nh);
+    }
+    else if (file_name == "poledata.csv")
+    {
+      category |= registerVectormapPortion<RoadPole, RoadPoleArray>(file_path, &road_pole_pub, "vector_map_info/road_pole", Category::ROAD_POLE, &nh);
+    }
+    else if (file_name == "roadsign.csv")
+    {
+      category |= registerVectormapPortion<RoadSign, RoadSignArray>(file_path, &road_sign_pub, "vector_map_info/road_sign", Category::ROAD_SIGN, &nh);
+    }
+    else if (file_name == "signaldata.csv")
+    {
+      category |= registerVectormapPortion<Signal, SignalArray>(file_path, &signal_pub, "vector_map_info/signal", Category::SIGNAL, &nh);
+    }
+    else if (file_name == "streetlight.csv")
+    {
+      category |= registerVectormapPortion<StreetLight, StreetLightArray>(file_path, &street_light_pub, "vector_map_info/street_light", Category::STREET_LIGHT, &nh);
+    }
+    else if (file_name == "utilitypole.csv")
+    {
+      category |= registerVectormapPortion<UtilityPole, UtilityPoleArray>(file_path, &utility_pole_pub, "vector_map_info/utility_pole", Category::UTILITY_POLE, &nh);
+    }
+    else if (file_name == "guardrail.csv")
+    {
+      category |= registerVectormapPortion<GuardRail, GuardRailArray>(file_path, &guard_rail_pub, "vector_map_info/guard_rail", Category::GUARD_RAIL, &nh);
+    }
+    else if (file_name == "sidewalk.csv")
+    {
+      category |= registerVectormapPortion<SideWalk, SideWalkArray>(file_path, &side_walk_pub, "vector_map_info/side_walk", Category::SIDE_WALK, &nh);
+    }
+    else if (file_name == "driveon_portion.csv")
+    {
+      category |= registerVectormapPortion<DriveOnPortion, DriveOnPortionArray>(file_path, &drive_on_portion_pub, "vector_map_info/drive_on_portion", Category::DRIVE_ON_PORTION, &nh);
+    }
+    else if (file_name == "intersection.csv")
+    {
+      category |= registerVectormapPortion<CrossRoad, CrossRoadArray>(file_path, &cross_road_pub, "vector_map_info/cross_road", Category::CROSS_ROAD, &nh);
+    }
+    else if (file_name == "sidestrip.csv")
+    {
+      category |= registerVectormapPortion<SideStrip, SideStripArray>(file_path, &side_strip_pub, "vector_map_info/side_strip", Category::SIDE_STRIP, &nh);
+    }
+    else if (file_name == "curvemirror.csv")
+    {
+      category |= registerVectormapPortion<CurveMirror, CurveMirrorArray>(file_path, &curve_mirror_pub, "vector_map_info/curve_mirror", Category::CURVE_MIRROR, &nh);
+    }
+    else if (file_name == "wall.csv")
+    {
+      category |= registerVectormapPortion<Wall, WallArray>(file_path, &wall_pub, "vector_map_info/wall", Category::WALL, &nh);
+    }
+    else if (file_name == "fence.csv")
+    {
+      category |= registerVectormapPortion<Fence, FenceArray>(file_path, &fence_pub, "vector_map_info/fence", Category::FENCE, &nh);
+    }
+    else if (file_name == "railroad_crossing.csv")
+    {
+      category |= registerVectormapPortion<RailCrossing, RailCrossingArray>(file_path, &rail_crossing_pub, "vector_map_info/rail_crossing", Category::RAIL_CROSSING, &nh);
+    }
+    else
+    {
+      ROS_ERROR_STREAM("unknown csv file: " << file_path);
+    }
+  }
+
+  ROS_INFO("Published vector_map_info topics");
+
+  VectorMap vmap;
+  vmap.subscribe(nh, category);
+
+  visualization_msgs::MarkerArray marker_array;
+  insertMarkerArray(marker_array, createRoadEdgeMarkerArray(vmap, Color::GRAY));
+  insertMarkerArray(marker_array, createGutterMarkerArray(vmap, Color::GRAY, Color::GRAY, Color::GRAY));
+  insertMarkerArray(marker_array, createCurbMarkerArray(vmap, Color::GRAY));
+  insertMarkerArray(marker_array, createWhiteLineMarkerArray(vmap, Color::WHITE, Color::YELLOW));
+  insertMarkerArray(marker_array, createStopLineMarkerArray(vmap, Color::WHITE));
+  insertMarkerArray(marker_array, createZebraZoneMarkerArray(vmap, Color::WHITE));
+  insertMarkerArray(marker_array, createCrossWalkMarkerArray(vmap, Color::WHITE));
+  insertMarkerArray(marker_array, createRoadMarkMarkerArray(vmap, Color::WHITE));
+  insertMarkerArray(marker_array, createRoadPoleMarkerArray(vmap, Color::GRAY));
+  insertMarkerArray(marker_array, createRoadSignMarkerArray(vmap, Color::GREEN, Color::GRAY));
+  insertMarkerArray(marker_array, createSignalMarkerArray(vmap, Color::RED, Color::BLUE, Color::YELLOW, Color::CYAN,
+                                                          Color::GRAY));
+  insertMarkerArray(marker_array, createStreetLightMarkerArray(vmap, Color::YELLOW, Color::GRAY));
+  insertMarkerArray(marker_array, createUtilityPoleMarkerArray(vmap, Color::GRAY));
+  insertMarkerArray(marker_array, createGuardRailMarkerArray(vmap, Color::LIGHT_BLUE));
+  insertMarkerArray(marker_array, createSideWalkMarkerArray(vmap, Color::GRAY));
+  insertMarkerArray(marker_array, createDriveOnPortionMarkerArray(vmap, Color::LIGHT_CYAN));
+  insertMarkerArray(marker_array, createCrossRoadMarkerArray(vmap, Color::LIGHT_GREEN));
+  insertMarkerArray(marker_array, createSideStripMarkerArray(vmap, Color::GRAY));
+  insertMarkerArray(marker_array, createCurveMirrorMarkerArray(vmap, Color::MAGENTA, Color::GRAY));
+  insertMarkerArray(marker_array, createWallMarkerArray(vmap, Color::LIGHT_YELLOW));
+  insertMarkerArray(marker_array, createFenceMarkerArray(vmap, Color::LIGHT_RED));
+  insertMarkerArray(marker_array, createRailCrossingMarkerArray(vmap, Color::LIGHT_MAGENTA));
+  marker_array_pub.publish(marker_array);
+  ROS_INFO("Published vector_map visualization");
+
+  stat.data = true;
+  stat_pub.publish(stat);
+
+  ros::spin();
+
+  return EXIT_SUCCESS;
+}

src/ros/catkin/src/map_file/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>map_file</name>
+  <version>1.12.0</version>
+  <description>The map_file package</description>
+  <maintainer email="kondoh@axe-inc.co.jp">Masao KONDOH</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>autoware_msgs</depend>
+  <depend>curl</depend>
+  <depend>geometry_msgs</depend>
+  <depend>libpcl-all-dev</depend>
+  <depend>pcl_ros</depend>
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf</depend>
+  <depend>vector_map</depend>
+  <depend>visualization_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>libboost-filesystem-dev</depend>
+  
+</package>

src/ros/catkin/src/tablet_socket_msgs/CHANGELOG.rst

@@ -0,0 +1,195 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package tablet_socket_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.13.0 (2019-12-03)
+-------------------
+* Update package.xml files to Format 2.
+* Contributors: Joshua Whitley
+
+1.12.0 (2019-07-12)
+-------------------
+
+1.11.0 (2019-03-21)
+-------------------
+* Fix license notice in corresponding package.xml
+* Contributors: amc-nu
+
+1.10.0 (2019-01-17)
+-------------------
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+
+1.8.0 (2018-08-31)
+------------------
+
+1.7.0 (2018-05-18)
+------------------
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* [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
+* Contributors: Abraham Monrroy, Kosuke Murakami
+
+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
+* Contributors: Yamato ANDO
+
+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)
+------------------
+
+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)
+------------------
+
+1.0.1 (2017-01-14)
+------------------
+
+1.0.0 (2016-12-22)
+------------------

src/ros/catkin/src/tablet_socket_msgs/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(tablet_socket_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  std_msgs
+)
+
+add_message_files(
+  FILES
+    Waypoint.msg
+    error_info.msg
+    gear_cmd.msg
+    mode_cmd.msg
+    mode_info.msg
+    route_cmd.msg
+)
+
+generate_messages(
+  DEPENDENCIES std_msgs 
+)
+
+catkin_package(
+  CATKIN_DEPENDS message_runtime std_msgs
+)

src/ros/catkin/src/tablet_socket_msgs/msg/Waypoint.msg

@@ -0,0 +1,2 @@
+float64 lat
+float64 lon

src/ros/catkin/src/tablet_socket_msgs/msg/error_info.msg

@@ -0,0 +1,2 @@
+Header header
+int32 error

src/ros/catkin/src/tablet_socket_msgs/msg/gear_cmd.msg

@@ -0,0 +1,2 @@
+Header header
+int32 gear

src/ros/catkin/src/tablet_socket_msgs/msg/mode_cmd.msg

@@ -0,0 +1,2 @@
+Header header
+int32 mode

src/ros/catkin/src/tablet_socket_msgs/msg/mode_info.msg

@@ -0,0 +1,2 @@
+Header header
+int32 mode

src/ros/catkin/src/tablet_socket_msgs/msg/route_cmd.msg

@@ -0,0 +1,2 @@
+Header header
+Waypoint[] point

src/ros/catkin/src/tablet_socket_msgs/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>tablet_socket_msgs</name>
+  <version>1.13.0</version>
+  <description>The tablet_socket_msgs package</description>
+  <maintainer email="fujii.shohei@gmail.com">Shohei Fujii</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <depend>std_msgs</depend>
+
+  <exec_depend>message_runtime</exec_depend>
+</package>

src/ros/catkin/src/vector_map/CHANGELOG.rst

@@ -0,0 +1,123 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package vector_map
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* Fix license notice in corresponding package.xml
+* Make VectorMap::hasSubscribed public (`#1821 <https://github.com/CPFL/Autoware/issues/1821>`_)
+* Contributors: Kenji Funaoka, amc-nu
+
+1.10.0 (2019-01-17)
+-------------------
+* 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
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+
+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>`_)
+* Contributors: Esteve Fernandez
+
+1.7.0 (2018-05-18)
+------------------
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* Fix Clustering performance when wayarea is not available or no vector map is available.
+* Contributors: AMC, Kosuke Murakami
+
+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
+* Contributors: Yamato ANDO
+
+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
+* static functions were never defined, so I removed them
+* install target
+* Contributors: Dejan Pangercic, 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
+* Add axis and adjust the direction.
+* Contributors: Dejan Pangercic, USUDA Hisashi
+
+1.3.1 (2017-07-16)
+------------------
+
+1.3.0 (2017-07-14)
+------------------
+
+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)
+------------------
+
+1.0.1 (2017-01-14)
+------------------
+
+1.0.0 (2016-12-22)
+------------------
+* Remove magic number
+* Only cache valid record
+* Add error check for vector_map_loader
+* Remove samples of using vector_map library
+* Add new subscribe() with timeout
+* Add vector_map library
+* Contributors: syouji

src/ros/catkin/src/vector_map/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(vector_map)
+
+find_package(autoware_build_flags REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  tf
+  geometry_msgs
+  roslint
+  vector_map_msgs
+  visualization_msgs
+)
+
+set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}")
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS tf geometry_msgs visualization_msgs vector_map_msgs
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME}
+  lib/vector_map/vector_map.cpp
+)
+add_dependencies(${PROJECT_NAME}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+set(ROSLINT_CPP_OPTS "--filter=-build/c++14,-runtime/references")
+roslint_cpp()
+
+## Install project namespaced headers
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+## Install executables and/or libraries
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+if(CATKIN_ENABLE_TESTING)
+  roslint_add_test()
+endif()

src/ros/catkin/src/vector_map/include/vector_map/vector_map.h

@@ -0,0 +1,569 @@
+/*
+ * 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 VECTOR_MAP_VECTOR_MAP_H
+#define VECTOR_MAP_VECTOR_MAP_H
+
+#include <ros/ros.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Quaternion.h>
+#include <visualization_msgs/Marker.h>
+
+#include <vector_map_msgs/PointArray.h>
+#include <vector_map_msgs/VectorArray.h>
+#include <vector_map_msgs/LineArray.h>
+#include <vector_map_msgs/AreaArray.h>
+#include <vector_map_msgs/PoleArray.h>
+#include <vector_map_msgs/BoxArray.h>
+#include <vector_map_msgs/DTLaneArray.h>
+#include <vector_map_msgs/NodeArray.h>
+#include <vector_map_msgs/LaneArray.h>
+#include <vector_map_msgs/WayAreaArray.h>
+#include <vector_map_msgs/RoadEdgeArray.h>
+#include <vector_map_msgs/GutterArray.h>
+#include <vector_map_msgs/CurbArray.h>
+#include <vector_map_msgs/WhiteLineArray.h>
+#include <vector_map_msgs/StopLineArray.h>
+#include <vector_map_msgs/ZebraZoneArray.h>
+#include <vector_map_msgs/CrossWalkArray.h>
+#include <vector_map_msgs/RoadMarkArray.h>
+#include <vector_map_msgs/RoadPoleArray.h>
+#include <vector_map_msgs/RoadSignArray.h>
+#include <vector_map_msgs/SignalArray.h>
+#include <vector_map_msgs/StreetLightArray.h>
+#include <vector_map_msgs/UtilityPoleArray.h>
+#include <vector_map_msgs/GuardRailArray.h>
+#include <vector_map_msgs/SideWalkArray.h>
+#include <vector_map_msgs/DriveOnPortionArray.h>
+#include <vector_map_msgs/CrossRoadArray.h>
+#include <vector_map_msgs/SideStripArray.h>
+#include <vector_map_msgs/CurveMirrorArray.h>
+#include <vector_map_msgs/WallArray.h>
+#include <vector_map_msgs/FenceArray.h>
+#include <vector_map_msgs/RailCrossingArray.h>
+
+#include <fstream>
+#include <map>
+#include <string>
+#include <vector>
+
+namespace vector_map
+{
+using vector_map_msgs::Point;
+using vector_map_msgs::Vector;
+using vector_map_msgs::Line;
+using vector_map_msgs::Area;
+using vector_map_msgs::Pole;
+using vector_map_msgs::Box;
+using vector_map_msgs::DTLane;
+using vector_map_msgs::Node;
+using vector_map_msgs::Lane;
+using vector_map_msgs::WayArea;
+using vector_map_msgs::RoadEdge;
+using vector_map_msgs::Gutter;
+using vector_map_msgs::Curb;
+using vector_map_msgs::WhiteLine;
+using vector_map_msgs::StopLine;
+using vector_map_msgs::ZebraZone;
+using vector_map_msgs::CrossWalk;
+using vector_map_msgs::RoadMark;
+using vector_map_msgs::RoadPole;
+using vector_map_msgs::RoadSign;
+using vector_map_msgs::Signal;
+using vector_map_msgs::StreetLight;
+using vector_map_msgs::UtilityPole;
+using vector_map_msgs::GuardRail;
+using vector_map_msgs::SideWalk;
+using vector_map_msgs::DriveOnPortion;
+using vector_map_msgs::CrossRoad;
+using vector_map_msgs::SideStrip;
+using vector_map_msgs::CurveMirror;
+using vector_map_msgs::Wall;
+using vector_map_msgs::Fence;
+using vector_map_msgs::RailCrossing;
+
+using vector_map_msgs::PointArray;
+using vector_map_msgs::VectorArray;
+using vector_map_msgs::LineArray;
+using vector_map_msgs::AreaArray;
+using vector_map_msgs::PoleArray;
+using vector_map_msgs::BoxArray;
+using vector_map_msgs::DTLaneArray;
+using vector_map_msgs::NodeArray;
+using vector_map_msgs::LaneArray;
+using vector_map_msgs::WayAreaArray;
+using vector_map_msgs::RoadEdgeArray;
+using vector_map_msgs::GutterArray;
+using vector_map_msgs::CurbArray;
+using vector_map_msgs::WhiteLineArray;
+using vector_map_msgs::StopLineArray;
+using vector_map_msgs::ZebraZoneArray;
+using vector_map_msgs::CrossWalkArray;
+using vector_map_msgs::RoadMarkArray;
+using vector_map_msgs::RoadPoleArray;
+using vector_map_msgs::RoadSignArray;
+using vector_map_msgs::SignalArray;
+using vector_map_msgs::StreetLightArray;
+using vector_map_msgs::UtilityPoleArray;
+using vector_map_msgs::GuardRailArray;
+using vector_map_msgs::SideWalkArray;
+using vector_map_msgs::DriveOnPortionArray;
+using vector_map_msgs::CrossRoadArray;
+using vector_map_msgs::SideStripArray;
+using vector_map_msgs::CurveMirrorArray;
+using vector_map_msgs::WallArray;
+using vector_map_msgs::FenceArray;
+using vector_map_msgs::RailCrossingArray;
+
+using category_t = uint32_t;
+
+enum Category : category_t
+{
+  NONE = 0LLU,
+
+  // Graphical Primitive Class
+  POINT = 1LLU << 0,
+  VECTOR = 1LLU << 1,
+  LINE = 1LLU << 2,
+  AREA = 1LLU << 3,
+  POLE = 1LLU << 4,
+  BOX = 1LLU << 5,
+
+  // Road Data
+  DTLANE = 1LLU << 6,
+  NODE = 1LLU << 7,
+  LANE = 1LLU << 8,
+  WAY_AREA = 1LLU << 9,
+
+  // Object Data
+  ROAD_EDGE = 1LLU << 10,
+  GUTTER = 1LLU << 11,
+  CURB = 1LLU << 12,
+  WHITE_LINE = 1LLU << 13,
+  STOP_LINE = 1LLU << 14,
+  ZEBRA_ZONE = 1LLU << 15,
+  CROSS_WALK = 1LLU << 16,
+  ROAD_MARK = 1LLU << 17,
+  ROAD_POLE = 1LLU << 18,
+  ROAD_SIGN = 1LLU << 19,
+  SIGNAL = 1LLU << 20,
+  STREET_LIGHT = 1LLU << 21,
+  UTILITY_POLE = 1LLU << 22,
+  GUARD_RAIL = 1LLU << 23,
+  SIDE_WALK = 1LLU << 24,
+  DRIVE_ON_PORTION = 1LLU << 25,
+  CROSS_ROAD = 1LLU << 26,
+  SIDE_STRIP = 1LLU << 27,
+  CURVE_MIRROR = 1LLU << 28,
+  WALL = 1LLU << 29,
+  FENCE = 1LLU << 30,
+  RAIL_CROSSING = 1LLU << 31,
+
+  ALL = (1LLU << 32) - 1
+};
+
+enum Color : int
+{
+  BLACK,
+  GRAY,
+  LIGHT_RED,
+  LIGHT_GREEN,
+  LIGHT_BLUE,
+  LIGHT_YELLOW,
+  LIGHT_CYAN,
+  LIGHT_MAGENTA,
+  RED,
+  GREEN,
+  BLUE,
+  YELLOW,
+  CYAN,
+  MAGENTA,
+  WHITE
+};
+
+template <class T>
+class Key
+{
+private:
+  int id_;
+
+public:
+  Key()
+  {
+  }
+
+  explicit Key(int id)
+    : id_(id)
+  {
+  }
+
+  void setId(int id)
+  {
+    id_ = id;
+  }
+
+  int getId() const
+  {
+    return id_;
+  }
+
+  bool operator<(const Key<T>& right) const
+  {
+    return id_ < right.getId();
+  }
+};
+
+template <class T, class U>
+using Updater = std::function<void(std::map<Key<T>, T>&, const U&)>;
+
+template <class T>
+using Callback = std::function<void(const T&)>;
+
+template <class T>
+using Filter = std::function<bool(const T&)>;
+
+template <class T, class U>
+class Handle
+{
+private:
+  ros::Subscriber sub_;
+  Updater<T, U> update_;
+  std::vector<Callback<U>> cbs_;
+  std::map<Key<T>, T> map_;
+
+  void subscribe(const U& msg)
+  {
+    update_(map_, msg);
+    for (const auto& cb : cbs_)
+      cb(msg);
+  }
+
+public:
+  Handle()
+  {
+  }
+
+  void registerSubscriber(ros::NodeHandle& nh, const std::string& topic_name)
+  {
+    sub_ = nh.subscribe(topic_name, 1, &Handle<T, U>::subscribe, this);
+  }
+
+  void registerUpdater(const Updater<T, U>& update)
+  {
+    update_ = update;
+  }
+
+  void registerCallback(const Callback<U>& cb)
+  {
+    cbs_.push_back(cb);
+  }
+
+  T findByKey(const Key<T>& key) const
+  {
+    auto it = map_.find(key);
+    if (it == map_.end())
+      return T();
+    return it->second;
+  }
+
+  std::vector<T> findByFilter(const Filter<T>& filter) const
+  {
+    std::vector<T> vector;
+    for (const auto& pair : map_)
+    {
+      if (filter(pair.second))
+        vector.push_back(pair.second);
+    }
+    return vector;
+  }
+
+  bool empty() const
+  {
+    return map_.empty();
+  }
+};
+
+template <class T>
+std::vector<T> parse(const std::string& csv_file)
+{
+  std::ifstream ifs(csv_file.c_str());
+  std::string line;
+  std::getline(ifs, line);  // remove first line
+  std::vector<T> objs;
+  while (std::getline(ifs, line))
+  {
+    T obj;
+    std::istringstream iss(line);
+    iss >> obj;
+    objs.push_back(obj);
+  }
+  return objs;
+}
+
+class VectorMap
+{
+private:
+  Handle<Point, PointArray> point_;
+  Handle<Vector, VectorArray> vector_;
+  Handle<Line, LineArray> line_;
+  Handle<Area, AreaArray> area_;
+  Handle<Pole, PoleArray> pole_;
+  Handle<Box, BoxArray> box_;
+  Handle<DTLane, DTLaneArray> dtlane_;
+  Handle<Node, NodeArray> node_;
+  Handle<Lane, LaneArray> lane_;
+  Handle<WayArea, WayAreaArray> way_area_;
+  Handle<RoadEdge, RoadEdgeArray> road_edge_;
+  Handle<Gutter, GutterArray> gutter_;
+  Handle<Curb, CurbArray> curb_;
+  Handle<WhiteLine, WhiteLineArray> white_line_;
+  Handle<StopLine, StopLineArray> stop_line_;
+  Handle<ZebraZone, ZebraZoneArray> zebra_zone_;
+  Handle<CrossWalk, CrossWalkArray> cross_walk_;
+  Handle<RoadMark, RoadMarkArray> road_mark_;
+  Handle<RoadPole, RoadPoleArray> road_pole_;
+  Handle<RoadSign, RoadSignArray> road_sign_;
+  Handle<Signal, SignalArray> signal_;
+  Handle<StreetLight, StreetLightArray> street_light_;
+  Handle<UtilityPole, UtilityPoleArray> utility_pole_;
+  Handle<GuardRail, GuardRailArray> guard_rail_;
+  Handle<SideWalk, SideWalkArray> side_walk_;
+  Handle<DriveOnPortion, DriveOnPortionArray> drive_on_portion_;
+  Handle<CrossRoad, CrossRoadArray> cross_road_;
+  Handle<SideStrip, SideStripArray> side_strip_;
+  Handle<CurveMirror, CurveMirrorArray> curve_mirror_;
+  Handle<Wall, WallArray> wall_;
+  Handle<Fence, FenceArray> fence_;
+  Handle<RailCrossing, RailCrossingArray> rail_crossing_;
+
+  void registerSubscriber(ros::NodeHandle& nh, category_t category);
+
+public:
+  VectorMap();
+
+  void subscribe(ros::NodeHandle& nh, category_t category);
+  void subscribe(ros::NodeHandle& nh, category_t category, const ros::Duration& timeout);
+  void subscribe(ros::NodeHandle& nh, category_t category, const size_t max_retries);
+
+  Point findByKey(const Key<Point>& key) const;
+  Vector findByKey(const Key<Vector>& key) const;
+  Line findByKey(const Key<Line>& key) const;
+  Area findByKey(const Key<Area>& key) const;
+  Pole findByKey(const Key<Pole>& key) const;
+  Box findByKey(const Key<Box>& key) const;
+  DTLane findByKey(const Key<DTLane>& key) const;
+  Node findByKey(const Key<Node>& key) const;
+  Lane findByKey(const Key<Lane>& key) const;
+  WayArea findByKey(const Key<WayArea>& key) const;
+  RoadEdge findByKey(const Key<RoadEdge>& key) const;
+  Gutter findByKey(const Key<Gutter>& key) const;
+  Curb findByKey(const Key<Curb>& key) const;
+  WhiteLine findByKey(const Key<WhiteLine>& key) const;
+  StopLine findByKey(const Key<StopLine>& key) const;
+  ZebraZone findByKey(const Key<ZebraZone>& key) const;
+  CrossWalk findByKey(const Key<CrossWalk>& key) const;
+  RoadMark findByKey(const Key<RoadMark>& key) const;
+  RoadPole findByKey(const Key<RoadPole>& key) const;
+  RoadSign findByKey(const Key<RoadSign>& key) const;
+  Signal findByKey(const Key<Signal>& key) const;
+  StreetLight findByKey(const Key<StreetLight>& key) const;
+  UtilityPole findByKey(const Key<UtilityPole>& key) const;
+  GuardRail findByKey(const Key<GuardRail>& key) const;
+  SideWalk findByKey(const Key<SideWalk>& key) const;
+  DriveOnPortion findByKey(const Key<DriveOnPortion>& key) const;
+  CrossRoad findByKey(const Key<CrossRoad>& key) const;
+  SideStrip findByKey(const Key<SideStrip>& key) const;
+  CurveMirror findByKey(const Key<CurveMirror>& key) const;
+  Wall findByKey(const Key<Wall>& key) const;
+  Fence findByKey(const Key<Fence>& key) const;
+  RailCrossing findByKey(const Key<RailCrossing>& key) const;
+
+  std::vector<Point> findByFilter(const Filter<Point>& filter) const;
+  std::vector<Vector> findByFilter(const Filter<Vector>& filter) const;
+  std::vector<Line> findByFilter(const Filter<Line>& filter) const;
+  std::vector<Area> findByFilter(const Filter<Area>& filter) const;
+  std::vector<Pole> findByFilter(const Filter<Pole>& filter) const;
+  std::vector<Box> findByFilter(const Filter<Box>& filter) const;
+  std::vector<DTLane> findByFilter(const Filter<DTLane>& filter) const;
+  std::vector<Node> findByFilter(const Filter<Node>& filter) const;
+  std::vector<Lane> findByFilter(const Filter<Lane>& filter) const;
+  std::vector<WayArea> findByFilter(const Filter<WayArea>& filter) const;
+  std::vector<RoadEdge> findByFilter(const Filter<RoadEdge>& filter) const;
+  std::vector<Gutter> findByFilter(const Filter<Gutter>& filter) const;
+  std::vector<Curb> findByFilter(const Filter<Curb>& filter) const;
+  std::vector<WhiteLine> findByFilter(const Filter<WhiteLine>& filter) const;
+  std::vector<StopLine> findByFilter(const Filter<StopLine>& filter) const;
+  std::vector<ZebraZone> findByFilter(const Filter<ZebraZone>& filter) const;
+  std::vector<CrossWalk> findByFilter(const Filter<CrossWalk>& filter) const;
+  std::vector<RoadMark> findByFilter(const Filter<RoadMark>& filter) const;
+  std::vector<RoadPole> findByFilter(const Filter<RoadPole>& filter) const;
+  std::vector<RoadSign> findByFilter(const Filter<RoadSign>& filter) const;
+  std::vector<Signal> findByFilter(const Filter<Signal>& filter) const;
+  std::vector<StreetLight> findByFilter(const Filter<StreetLight>& filter) const;
+  std::vector<UtilityPole> findByFilter(const Filter<UtilityPole>& filter) const;
+  std::vector<GuardRail> findByFilter(const Filter<GuardRail>& filter) const;
+  std::vector<SideWalk> findByFilter(const Filter<SideWalk>& filter) const;
+  std::vector<DriveOnPortion> findByFilter(const Filter<DriveOnPortion>& filter) const;
+  std::vector<CrossRoad> findByFilter(const Filter<CrossRoad>& filter) const;
+  std::vector<SideStrip> findByFilter(const Filter<SideStrip>& filter) const;
+  std::vector<CurveMirror> findByFilter(const Filter<CurveMirror>& filter) const;
+  std::vector<Wall> findByFilter(const Filter<Wall>& filter) const;
+  std::vector<Fence> findByFilter(const Filter<Fence>& filter) const;
+  std::vector<RailCrossing> findByFilter(const Filter<RailCrossing>& filter) const;
+
+  bool hasSubscribed(category_t category) const;
+
+  void registerCallback(const Callback<PointArray>& cb);
+  void registerCallback(const Callback<VectorArray>& cb);
+  void registerCallback(const Callback<LineArray>& cb);
+  void registerCallback(const Callback<AreaArray>& cb);
+  void registerCallback(const Callback<PoleArray>& cb);
+  void registerCallback(const Callback<BoxArray>& cb);
+  void registerCallback(const Callback<DTLaneArray>& cb);
+  void registerCallback(const Callback<NodeArray>& cb);
+  void registerCallback(const Callback<LaneArray>& cb);
+  void registerCallback(const Callback<WayAreaArray>& cb);
+  void registerCallback(const Callback<RoadEdgeArray>& cb);
+  void registerCallback(const Callback<GutterArray>& cb);
+  void registerCallback(const Callback<CurbArray>& cb);
+  void registerCallback(const Callback<WhiteLineArray>& cb);
+  void registerCallback(const Callback<StopLineArray>& cb);
+  void registerCallback(const Callback<ZebraZoneArray>& cb);
+  void registerCallback(const Callback<CrossWalkArray>& cb);
+  void registerCallback(const Callback<RoadMarkArray>& cb);
+  void registerCallback(const Callback<RoadPoleArray>& cb);
+  void registerCallback(const Callback<RoadSignArray>& cb);
+  void registerCallback(const Callback<SignalArray>& cb);
+  void registerCallback(const Callback<StreetLightArray>& cb);
+  void registerCallback(const Callback<UtilityPoleArray>& cb);
+  void registerCallback(const Callback<GuardRailArray>& cb);
+  void registerCallback(const Callback<SideWalkArray>& cb);
+  void registerCallback(const Callback<DriveOnPortionArray>& cb);
+  void registerCallback(const Callback<CrossRoadArray>& cb);
+  void registerCallback(const Callback<SideStripArray>& cb);
+  void registerCallback(const Callback<CurveMirrorArray>& cb);
+  void registerCallback(const Callback<WallArray>& cb);
+  void registerCallback(const Callback<FenceArray>& cb);
+  void registerCallback(const Callback<RailCrossingArray>& cb);
+};
+
+extern const double COLOR_VALUE_MIN;
+extern const double COLOR_VALUE_MAX;
+extern const double COLOR_VALUE_MEDIAN;
+extern const double COLOR_VALUE_LIGHT_LOW;
+extern const double COLOR_VALUE_LIGHT_HIGH;
+
+std_msgs::ColorRGBA createColorRGBA(Color color);
+
+void enableMarker(visualization_msgs::Marker& marker);
+void disableMarker(visualization_msgs::Marker& marker);
+bool isValidMarker(const visualization_msgs::Marker& marker);
+
+extern const double MAKER_SCALE_POINT;
+extern const double MAKER_SCALE_VECTOR;
+extern const double MAKER_SCALE_VECTOR_LENGTH;
+extern const double MAKER_SCALE_LINE;
+extern const double MAKER_SCALE_AREA;
+extern const double MAKER_SCALE_BOX;
+
+visualization_msgs::Marker createMarker(const std::string& ns, int id, int type);
+visualization_msgs::Marker createPointMarker(const std::string& ns, int id, Color color, const Point& point);
+visualization_msgs::Marker createVectorMarker(const std::string& ns, int id, Color color, const VectorMap& vmap,
+                                              const Vector& vector);
+visualization_msgs::Marker createLineMarker(const std::string& ns, int id, Color color, const VectorMap& vmap,
+                                            const Line& line);
+visualization_msgs::Marker createAreaMarker(const std::string& ns, int id, Color color, const VectorMap& vmap,
+                                            const Area& area);
+visualization_msgs::Marker createPoleMarker(const std::string& ns, int id, Color color, const VectorMap& vmap,
+                                            const Pole& pole);
+visualization_msgs::Marker createBoxMarker(const std::string& ns, int id, Color color, const VectorMap& vmap,
+                                           const Box& box);
+
+double convertDegreeToRadian(double degree);
+double convertRadianToDegree(double radian);
+geometry_msgs::Point convertPointToGeomPoint(const Point& point);
+Point convertGeomPointToPoint(const geometry_msgs::Point& geom_point);
+geometry_msgs::Quaternion convertVectorToGeomQuaternion(const Vector& vector);
+Vector convertGeomQuaternionToVector(const geometry_msgs::Quaternion& geom_quaternion);
+}  // namespace vector_map
+
+std::ostream& operator<<(std::ostream& os, const vector_map::Point& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Vector& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Line& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Area& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Pole& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Box& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::DTLane& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Node& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Lane& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::WayArea& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::RoadEdge& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Gutter& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Curb& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::WhiteLine& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::StopLine& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::ZebraZone& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::CrossWalk& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::RoadMark& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::RoadPole& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::RoadSign& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Signal& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::StreetLight& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::UtilityPole& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::GuardRail& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::SideWalk& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::DriveOnPortion& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::CrossRoad& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::SideStrip& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::CurveMirror& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Wall& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::Fence& obj);
+std::ostream& operator<<(std::ostream& os, const vector_map::RailCrossing& obj);
+
+std::istream& operator>>(std::istream& is, vector_map::Point& obj);
+std::istream& operator>>(std::istream& is, vector_map::Vector& obj);
+std::istream& operator>>(std::istream& is, vector_map::Line& obj);
+std::istream& operator>>(std::istream& is, vector_map::Area& obj);
+std::istream& operator>>(std::istream& is, vector_map::Pole& obj);
+std::istream& operator>>(std::istream& is, vector_map::Box& obj);
+std::istream& operator>>(std::istream& is, vector_map::DTLane& obj);
+std::istream& operator>>(std::istream& is, vector_map::Node& obj);
+std::istream& operator>>(std::istream& is, vector_map::Lane& obj);
+std::istream& operator>>(std::istream& is, vector_map::WayArea& obj);
+std::istream& operator>>(std::istream& is, vector_map::RoadEdge& obj);
+std::istream& operator>>(std::istream& is, vector_map::Gutter& obj);
+std::istream& operator>>(std::istream& is, vector_map::Curb& obj);
+std::istream& operator>>(std::istream& is, vector_map::WhiteLine& obj);
+std::istream& operator>>(std::istream& is, vector_map::StopLine& obj);
+std::istream& operator>>(std::istream& is, vector_map::ZebraZone& obj);
+std::istream& operator>>(std::istream& is, vector_map::CrossWalk& obj);
+std::istream& operator>>(std::istream& is, vector_map::RoadMark& obj);
+std::istream& operator>>(std::istream& is, vector_map::RoadPole& obj);
+std::istream& operator>>(std::istream& is, vector_map::RoadSign& obj);
+std::istream& operator>>(std::istream& is, vector_map::Signal& obj);
+std::istream& operator>>(std::istream& is, vector_map::StreetLight& obj);
+std::istream& operator>>(std::istream& is, vector_map::UtilityPole& obj);
+std::istream& operator>>(std::istream& is, vector_map::GuardRail& obj);
+std::istream& operator>>(std::istream& is, vector_map::SideWalk& obj);
+std::istream& operator>>(std::istream& is, vector_map::DriveOnPortion& obj);
+std::istream& operator>>(std::istream& is, vector_map::CrossRoad& obj);
+std::istream& operator>>(std::istream& is, vector_map::SideStrip& obj);
+std::istream& operator>>(std::istream& is, vector_map::CurveMirror& obj);
+std::istream& operator>>(std::istream& is, vector_map::Wall& obj);
+std::istream& operator>>(std::istream& is, vector_map::Fence& obj);
+std::istream& operator>>(std::istream& is, vector_map::RailCrossing& obj);
+
+#endif  // VECTOR_MAP_VECTOR_MAP_H

src/ros/catkin/src/vector_map/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>vector_map</name>
+  <version>1.12.0</version>
+  <description>The vector_map package</description>
+  <maintainer email="syouji@axe.bz">syouji</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>tf</depend>
+  <depend>roslint</depend>
+  <depend>vector_map_msgs</depend>
+  <depend>visualization_msgs</depend>
+</package>

src/ros/catkin/src/vector_map_msgs/CHANGELOG.rst

@@ -0,0 +1,117 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package vector_map_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.13.0 (2019-12-03)
+-------------------
+* Update package.xml files to Format 2.
+* Contributors: Joshua Whitley
+
+1.12.0 (2019-07-12)
+-------------------
+
+1.11.0 (2019-03-21)
+-------------------
+* Fix license notice in corresponding package.xml
+* Contributors: amc-nu
+
+1.10.0 (2019-01-17)
+-------------------
+* 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
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+
+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>`_)
+* Contributors: Esteve Fernandez
+
+1.7.0 (2018-05-18)
+------------------
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* -Added support fot VMap colouring to Left Traffic signals (`#988 <https://github.com/CPFL/Autoware/pull/988>`_)
+  -Added Lane number on tlr_superimpose
+* Contributors: Abraham Monrroy, Kosuke Murakami
+
+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 feature of to find stopline. and following minor fixes
+  - to change vectormap operation to vectormap lib.
+  - to change state operation
+* 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)
+------------------
+
+1.2.0 (2017-06-07)
+------------------
+
+1.1.2 (2017-02-27 23:10)
+------------------------
+
+1.1.1 (2017-02-27 22:25)
+------------------------
+
+1.1.0 (2017-02-24)
+------------------
+
+1.0.1 (2017-01-14)
+------------------
+
+1.0.0 (2016-12-22)
+------------------
+* Add message_runtime dependency
+* Add vector_map library
+* Contributors: syouji

src/ros/catkin/src/vector_map_msgs/CMakeLists.txt

@@ -0,0 +1,86 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(vector_map_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  std_msgs
+)
+
+add_message_files(
+  FILES
+    Area.msg
+    AreaArray.msg
+    Box.msg
+    BoxArray.msg
+    CrossRoad.msg
+    CrossRoadArray.msg
+    CrossWalk.msg
+    CrossWalkArray.msg
+    Curb.msg
+    CurbArray.msg
+    CurveMirror.msg
+    CurveMirrorArray.msg
+    DTLane.msg
+    DTLaneArray.msg
+    DriveOnPortion.msg
+    DriveOnPortionArray.msg
+    Fence.msg
+    FenceArray.msg
+    GuardRail.msg
+    GuardRailArray.msg
+    Gutter.msg
+    GutterArray.msg
+    Lane.msg
+    LaneArray.msg
+    Line.msg
+    LineArray.msg
+    Node.msg
+    NodeArray.msg
+    Point.msg
+    PointArray.msg
+    Pole.msg
+    PoleArray.msg
+    RailCrossing.msg
+    RailCrossingArray.msg
+    RoadEdge.msg
+    RoadEdgeArray.msg
+    RoadMark.msg
+    RoadMarkArray.msg
+    RoadPole.msg
+    RoadPoleArray.msg
+    RoadSign.msg
+    RoadSignArray.msg
+    SideStrip.msg
+    SideStripArray.msg
+    SideWalk.msg
+    SideWalkArray.msg
+    Signal.msg
+    SignalArray.msg
+    StopLine.msg
+    StopLineArray.msg
+    StreetLight.msg
+    StreetLightArray.msg
+    UtilityPole.msg
+    UtilityPoleArray.msg
+    Vector.msg
+    VectorArray.msg
+    Wall.msg
+    WallArray.msg
+    WayArea.msg
+    WayAreaArray.msg
+    WhiteLine.msg
+    WhiteLineArray.msg
+    ZebraZone.msg
+    ZebraZoneArray.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+    std_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    message_runtime
+    std_msgs
+)

src/ros/catkin/src/vector_map_msgs/msg/Area.msg

@@ -0,0 +1,4 @@
+# Ver 1.00
+int32 aid
+int32 slid
+int32 elid

src/ros/catkin/src/vector_map_msgs/msg/AreaArray.msg

@@ -0,0 +1,2 @@
+Header header
+Area[] data

src/ros/catkin/src/vector_map_msgs/msg/Box.msg

@@ -0,0 +1,7 @@
+# Ver 1.00
+int32 bid
+int32 pid1
+int32 pid2
+int32 pid3
+int32 pid4
+float64 height

src/ros/catkin/src/vector_map_msgs/msg/BoxArray.msg

@@ -0,0 +1,2 @@
+Header header
+Box[] data

src/ros/catkin/src/vector_map_msgs/msg/CrossRoad.msg

@@ -0,0 +1,4 @@
+# Ver 1.00
+int32 id
+int32 aid
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/CrossRoadArray.msg

@@ -0,0 +1,2 @@
+Header header
+CrossRoad[] data

src/ros/catkin/src/vector_map_msgs/msg/CrossWalk.msg

@@ -0,0 +1,11 @@
+# type
+uint8 CLOSURE_LINE=0
+uint8 STRIPE_PATTERN=1
+uint8 BICYCLE_LANE=2
+
+# Ver 1.00
+int32 id
+int32 aid
+int32 type
+int32 bdid
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/CrossWalkArray.msg

@@ -0,0 +1,2 @@
+Header header
+CrossWalk[] data

src/ros/catkin/src/vector_map_msgs/msg/Curb.msg

@@ -0,0 +1,11 @@
+# dir
+uint8 RIGHT=0
+uint8 LEFT=1
+
+# Ver 1.00
+int32 id
+int32 lid
+float64 height
+float64 width
+int32 dir
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/CurbArray.msg

@@ -0,0 +1,2 @@
+Header header
+Curb[] data

src/ros/catkin/src/vector_map_msgs/msg/CurveMirror.msg

@@ -0,0 +1,6 @@
+# Ver 1.10
+int32 id
+int32 vid
+int32 plid
+int32 type # differ from specification
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/CurveMirrorArray.msg

@@ -0,0 +1,2 @@
+Header header
+CurveMirror[] data

src/ros/catkin/src/vector_map_msgs/msg/DTLane.msg

@@ -0,0 +1,11 @@
+# Ver 1.00
+int32 did
+float64 dist
+int32 pid
+float64 dir
+float64 apara
+float64 r
+float64 slope
+float64 cant
+float64 lw
+float64 rw

src/ros/catkin/src/vector_map_msgs/msg/DTLaneArray.msg

@@ -0,0 +1,2 @@
+Header header
+DTLane[] data

src/ros/catkin/src/vector_map_msgs/msg/DriveOnPortion.msg

@@ -0,0 +1,4 @@
+# Ver 1.10
+int32 id
+int32 aid
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/DriveOnPortionArray.msg

@@ -0,0 +1,2 @@
+Header header
+DriveOnPortion[] data

src/ros/catkin/src/vector_map_msgs/msg/Fence.msg

@@ -0,0 +1,4 @@
+# Ver 1.10
+int32 id
+int32 aid
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/FenceArray.msg

@@ -0,0 +1,2 @@
+Header header
+Fence[] data

src/ros/catkin/src/vector_map_msgs/msg/GuardRail.msg

@@ -0,0 +1,9 @@
+# type
+uint8 PLATE_BLADE=0
+uint8 POLE=1
+
+# Ver 1.00
+int32 id
+int32 aid
+int32 type
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/GuardRailArray.msg

@@ -0,0 +1,2 @@
+Header header
+GuardRail[] data

src/ros/catkin/src/vector_map_msgs/msg/Gutter.msg

@@ -0,0 +1,10 @@
+# type
+uint8 NO_COVER=0
+uint8 COVER=1
+uint8 GRATING=2
+
+# Ver 1.00
+int32 id
+int32 aid
+int32 type
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/GutterArray.msg

@@ -0,0 +1,2 @@
+Header header
+Gutter[] data

src/ros/catkin/src/vector_map_msgs/msg/Lane.msg

@@ -0,0 +1,43 @@
+# jct
+uint8 NORMAL=0
+uint8 LEFT_BRANCHING=1
+uint8 RIGHT_BRANCHING=2
+uint8 LEFT_MERGING=3
+uint8 RIGHT_MERGING=4
+uint8 COMPOSITION=5
+
+# lanetype
+uint8 STRAIGHT=0
+uint8 LEFT_TURN=1
+uint8 RIGHT_TURN=2
+
+# lanecfgfg
+uint8 PASS=0
+uint8 FAIL=1
+
+# Ver 1.00
+int32 lnid
+int32 did
+int32 blid
+int32 flid
+int32 bnid
+int32 fnid
+int32 jct
+int32 blid2
+int32 blid3
+int32 blid4
+int32 flid2
+int32 flid3
+int32 flid4
+int32 clossid
+float64 span
+int32 lcnt
+int32 lno
+
+# Ver 1.20
+int32 lanetype
+int32 limitvel
+int32 refvel
+int32 roadsecid
+int32 lanecfgfg
+int32 linkwaid

src/ros/catkin/src/vector_map_msgs/msg/LaneArray.msg

@@ -0,0 +1,2 @@
+Header header
+Lane[] data

src/ros/catkin/src/vector_map_msgs/msg/Line.msg

@@ -0,0 +1,6 @@
+# Ver 1.00
+int32 lid
+int32 bpid
+int32 fpid
+int32 blid
+int32 flid

src/ros/catkin/src/vector_map_msgs/msg/LineArray.msg

@@ -0,0 +1,2 @@
+Header header
+Line[] data

src/ros/catkin/src/vector_map_msgs/msg/Node.msg

@@ -0,0 +1,3 @@
+# Ver 1.00
+int32 nid
+int32 pid

src/ros/catkin/src/vector_map_msgs/msg/NodeArray.msg

@@ -0,0 +1,2 @@
+Header header
+Node[] data

src/ros/catkin/src/vector_map_msgs/msg/Point.msg

@@ -0,0 +1,11 @@
+# Ver 1.00
+int32 pid
+float64 b
+float64 l
+float64 h
+float64 bx
+float64 ly
+int32 ref
+int32 mcode1
+int32 mcode2
+int32 mcode3

src/ros/catkin/src/vector_map_msgs/msg/PointArray.msg

@@ -0,0 +1,2 @@
+Header header
+Point[] data

src/ros/catkin/src/vector_map_msgs/msg/Pole.msg

@@ -0,0 +1,5 @@
+# Ver 1.00
+int32 plid
+int32 vid
+float64 length
+float64 dim

src/ros/catkin/src/vector_map_msgs/msg/PoleArray.msg

@@ -0,0 +1,2 @@
+Header header
+Pole[] data

src/ros/catkin/src/vector_map_msgs/msg/RailCrossing.msg

@@ -0,0 +1,4 @@
+# Ver 1.00
+int32 id
+int32 aid
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/RailCrossingArray.msg

@@ -0,0 +1,2 @@
+Header header
+RailCrossing[] data

src/ros/catkin/src/vector_map_msgs/msg/RoadEdge.msg

@@ -0,0 +1,4 @@
+# Ver 1.00
+int32 id
+int32 lid
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/RoadEdgeArray.msg

@@ -0,0 +1,2 @@
+Header header
+RoadEdge[] data

src/ros/catkin/src/vector_map_msgs/msg/RoadMark.msg

@@ -0,0 +1,11 @@
+# type
+uint8 MARK=1
+uint8 ARROW=2
+uint8 CHARACTER=3
+uint8 SIGN=4
+
+# Ver 1.00
+int32 id
+int32 aid
+int32 type # differ from specification
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/RoadMarkArray.msg

@@ -0,0 +1,2 @@
+Header header
+RoadMark[] data

src/ros/catkin/src/vector_map_msgs/msg/RoadPole.msg

@@ -0,0 +1,4 @@
+# Ver 1.00
+int32 id
+int32 plid
+int32 linkid

src/ros/catkin/src/vector_map_msgs/msg/RoadPoleArray.msg

@@ -0,0 +1,2 @@
+Header header
+RoadPole[] data

src/ros/catkin/src/vector_map_msgs/msg/RoadSign.msg

@@ -0,0 +1,10 @@
+# Ver 1.00
+int32 id
+int32 vid
+int32 plid
+int32 type # differ from specification
+int32 linkid
+
+## Optional specification
+int32 TYPE_NULL = 0
+int32 TYPE_STOP = 1

src/ros/catkin/src/vector_map_msgs/msg/RoadSignArray.msg

@@ -0,0 +1,2 @@
+Header header
+RoadSign[] data