add some autoware module from autoware.

src/ros/catkin/src/astar_search/CHANGELOG.rst

@@ -0,0 +1,82 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package astar_search
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* [Feature] Improve Hybrid A* planner (`#1594 <https://github.com/CPFL/Autoware/issues/1594>`_)
+  * Delete obstacle_sim from astar_planner package, replaced to lidar_fake_perception
+  * Modify package name, astar_planner -> waypoint_planner, and create astar_planner library package
+  * Delete obstacle_avoid/astar* and modify its dependency to astar_planner library
+  * Fix astar_navi with astar_planner library
+  * Refactor astar_navi by separating HAstar library and fixing coodinate system
+  * Rename obstacle_avoid -> astar_avoid and under refactoring
+  * Fix cost function and configures
+  * Fix backward search and refactor configurations
+  * Apply clang-format
+  * Refactor include
+  * Fix typo and so on
+  * Improve astar_avoid by incremental goal search
+  * Apply clang-format
+  * Revert package names
+  * Fix package/code names
+  * Update runtime_manager
+  * Improve astar_avoid to execute avoidance behavior by state transition (by rebuild decision maker)
+  * Fix PascalCase message names by `#1408 <https://github.com/CPFL/Autoware/issues/1408>`_
+  * Remove obstacle_avoid directory
+  * Fix default parameter for costmap topic
+  * Fix warning and initialize condition
+  * Remove use_avoidance_state mode (TODO: after merging rebuild decision maker)
+  * Improve astar_avoid behavior by simple state transition and multi-threading
+  * Apply clang-format
+  * Fix replan_interval in astar_avoid
+  * Add descriptions for paramters
+  * Rename pkg name, astar_planner -> waypoint_planner
+  * Fix param name
+  * Fix avoid waypoints height
+  * Fix parameter and formalize code
+  * Add README for freespace/waypoint_planner
+  * Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Fix CHANGELOG
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  * Fix astar_navi/README.md
+  * Add License terms
+  * Fix const pointer
+  * Added unit test base
+  * Restructured folders
+  * Fix bug by adding AstarSearch reset
+  * Fix WaveFrontNode initialization
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  * Fix variable name
+  * Refactor threading
+  * Re-adding lidar_fake_perception
+  * Fix the condition to judge reaching goal
+  * Add 'use_decision state' mode to transit avoidance state by decision_maker
+  * Fix calcDiffOfRadian (if diff > 2pi)
+  * Feature/test astar planner (`#1753 <https://github.com/CPFL/Autoware/issues/1753>`_)
+  * Restructured folders
+  * Added unit test base
+  * Removed remaining folder
+  * Test WIP
+  * Added astar_util tests and base file for astar_search tests
+  * Updated to ROS Cpp Style guidelines
+  * Added test for SimpleNode constructor
+  * Updated Copyright date
+  * Added tests for astar algorithm
+  * Added default constructor to WaveFront struct
+  * Revert use_state_decision mode (94af7b6)
+  * Fix costmap topic names by merging costmap_generator
+* Contributors: Akihito Ohsato

src/ros/catkin/src/astar_search/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(astar_search)
+
+set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}")
+
+find_package(autoware_build_flags REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+  autoware_msgs
+  geometry_msgs
+  pcl_ros
+  roscpp
+  sensor_msgs
+  tf
+  tf_conversions
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES astar_search
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(astar_search
+  src/astar_search.cpp
+  src/astar_util.cpp
+)
+
+target_link_libraries(astar_search
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(astar_search
+  ${catkin_EXPORTED_TARGETS}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+)
+
+install(TARGETS astar_search
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest_gtest(
+    astar_search-test
+    test/test_astar_search.test
+    test/src/test_main.cpp
+    test/src/test_astar_util.cpp
+    test/src/test_astar_search.cpp
+    test/src/test_class.cpp
+  )
+  target_link_libraries(astar_search-test ${catkin_LIBRARIES} astar_search)
+endif()

src/ros/catkin/src/astar_search/include/astar_search/astar_search.h

@@ -0,0 +1,111 @@
+/*
+ * 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 ASTER_PLANNER_H
+#define ASTER_PLANNER_H
+
+#include <iostream>
+#include <vector>
+#include <queue>
+#include <string>
+#include <chrono>
+
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <geometry_msgs/PoseArray.h>
+#include <nav_msgs/Path.h>
+
+#include "astar_search/astar_util.h"
+
+class AstarSearch
+{
+  friend class TestClass;
+
+public:
+  AstarSearch();
+  ~AstarSearch();
+  void initialize(const nav_msgs::OccupancyGrid& costmap);
+  bool makePlan(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& goal_pose);
+  void reset();
+
+  const nav_msgs::Path& getPath() const
+  {
+    return path_;
+  }
+
+private:
+  void createStateUpdateTable();
+  bool search();
+  void poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y, int* index_theta);
+  void pointToIndex(const geometry_msgs::Point& point, int* index_x, int* index_y);
+  bool isOutOfRange(int index_x, int index_y);
+  void setPath(const SimpleNode& goal);
+  bool setStartNode(const geometry_msgs::Pose& start_pose);
+  bool setGoalNode(const geometry_msgs::Pose& goal_pose);
+  bool isGoal(double x, double y, double theta);
+  bool isObs(int index_x, int index_y);
+  bool detectCollision(const SimpleNode& sn);
+  bool calcWaveFrontHeuristic(const SimpleNode& sn);
+  bool detectCollisionWaveFront(const WaveFrontNode& sn);
+
+  // ros param
+  ros::NodeHandle n_;
+
+  // base configs
+  bool use_back_;                 // backward search
+  bool use_potential_heuristic_;  // potential cost function
+  bool use_wavefront_heuristic_;  // wavefront cost function
+  double time_limit_;             // planning time limit [msec]
+
+  // robot configs (TODO: obtain from vehicle_info)
+  double robot_length_;           // X [m]
+  double robot_width_;            // Y [m]
+  double robot_base2back_;        // base_link to rear [m]
+  double minimum_turning_radius_; // [m]]
+
+  // search configs
+  int theta_size_;                  // descritized angle table size [-]
+  double curve_weight_;             // curve moving cost [-]
+  double reverse_weight_;           // backward moving cost [-]
+  double lateral_goal_range_;       // reaching threshold, lateral error [m]
+  double longitudinal_goal_range_;  // reaching threshold, longitudinal error [m]
+  double angle_goal_range_;         // reaching threshold, angle error [deg]
+
+  // costmap configs
+  int obstacle_threshold_;            // obstacle threshold on grid [-]
+  double potential_weight_;           // weight of potential cost [-]
+  double distance_heuristic_weight_;  // obstacle threshold on grid [0,255]
+
+  // hybrid astar variables
+  std::vector<std::vector<NodeUpdate>> state_update_table_;
+  std::vector<std::vector<std::vector<AstarNode>>> nodes_;
+  std::priority_queue<SimpleNode, std::vector<SimpleNode>, std::greater<SimpleNode>> openlist_;
+  std::vector<SimpleNode> goallist_;
+
+  // costmap as occupancy grid
+  nav_msgs::OccupancyGrid costmap_;
+
+  // pose in costmap frame
+  geometry_msgs::PoseStamped start_pose_local_;
+  geometry_msgs::PoseStamped goal_pose_local_;
+  double goal_yaw_;
+
+  // result path
+  nav_msgs::Path path_;
+};
+
+#endif

src/ros/catkin/src/astar_search/include/astar_search/astar_util.h

@@ -0,0 +1,149 @@
+/*
+ * 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 ASTAR_UTIL_H
+#define ASTAR_UTIL_H
+
+#include <tf/tf.h>
+
+enum class STATUS : uint8_t
+{
+  NONE,
+  OPEN,
+  CLOSED,
+  OBS
+};
+
+struct AstarNode
+{
+  double x, y, theta;            // Coordinate of each node
+  STATUS status = STATUS::NONE;  // NONE, OPEN, CLOSED or OBS
+  double gc = 0;                 // Actual cost
+  double hc = 0;                 // heuristic cost
+  double move_distance = 0;      // actual move distance
+  bool back;                     // true if the current direction of the vehicle is back
+  AstarNode* parent = NULL;      // parent node
+};
+
+struct WaveFrontNode
+{
+  int index_x;
+  int index_y;
+  double hc;
+
+  WaveFrontNode();
+  WaveFrontNode(int x, int y, double cost);
+};
+
+struct NodeUpdate
+{
+  double shift_x;
+  double shift_y;
+  double rotation;
+  double step;
+  int index_theta;
+  bool curve;
+  bool back;
+};
+
+// For open list and goal list
+struct SimpleNode
+{
+  int index_x;
+  int index_y;
+  int index_theta;
+  double cost;
+
+  bool operator>(const SimpleNode& right) const
+  {
+    return cost > right.cost;
+  }
+
+  SimpleNode();
+  SimpleNode(int x, int y, int theta, double gc, double hc);
+};
+
+inline double calcDistance(double x1, double y1, double x2, double y2)
+{
+  return std::hypot(x2 - x1, y2 - y1);
+}
+
+inline double modifyTheta(double theta)
+{
+  if (theta < 0.0)
+    return theta + 2.0 * M_PI;
+  if (theta >= 2.0 * M_PI)
+    return theta - 2.0 * M_PI;
+
+  return theta;
+}
+
+inline geometry_msgs::Pose transformPose(const geometry_msgs::Pose& pose, const tf::Transform& tf)
+{
+  // Convert ROS pose to TF pose
+  tf::Pose tf_pose;
+  tf::poseMsgToTF(pose, tf_pose);
+
+  // Transform pose
+  tf_pose = tf * tf_pose;
+
+  // Convert TF pose to ROS pose
+  geometry_msgs::Pose ros_pose;
+  tf::poseTFToMsg(tf_pose, ros_pose);
+
+  return ros_pose;
+}
+
+inline WaveFrontNode getWaveFrontNode(int x, int y, double cost)
+{
+  WaveFrontNode node(x, y, cost);
+
+  return node;
+}
+
+inline geometry_msgs::Point calcRelativeCoordinate(geometry_msgs::Pose pose, tf::Point point)
+{
+  tf::Transform transform;
+  tf::poseMsgToTF(pose, transform);
+  transform = transform.inverse();
+
+  point = transform * point;
+  geometry_msgs::Point point_msg;
+  tf::pointTFToMsg(point, point_msg);
+
+  return point_msg;
+}
+
+inline double calcDiffOfRadian(double a, double b)
+{
+  double diff = std::fmod(std::fabs(a - b), 2.0 * M_PI);
+  if (diff < M_PI)
+    return diff;
+  else
+    return 2.0 * M_PI - diff;
+}
+
+inline geometry_msgs::Pose xytToPoseMsg(double x, double y, double theta)
+{
+  geometry_msgs::Pose p;
+  p.position.x = x;
+  p.position.y = y;
+  p.orientation = tf::createQuaternionMsgFromYaw(theta);
+
+  return p;
+}
+
+#endif

src/ros/catkin/src/astar_search/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>astar_search</name>
+  <version>1.12.0</version>
+  <description>The astar_search package</description>
+  <maintainer email="aohsato@gmail.com">Akihito Ohsato</maintainer>
+  <author email="aohsato@gmail.com">Akihito Ohsato</author>
+  <license>BSD</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>autoware_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>pcl_ros</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf</depend>
+  <depend>tf_conversions</depend>
+</package>

src/ros/catkin/src/astar_search/src/astar_search.cpp

@@ -0,0 +1,708 @@
+/*
+ * 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 "astar_search/astar_search.h"
+
+AstarSearch::AstarSearch()
+{
+  ros::NodeHandle private_nh_("~");
+
+  // base configs
+  private_nh_.param<bool>("use_back", use_back_, true);
+  private_nh_.param<bool>("use_potential_heuristic", use_potential_heuristic_, true);
+  private_nh_.param<bool>("use_wavefront_heuristic", use_wavefront_heuristic_, false);
+  private_nh_.param<double>("time_limit", time_limit_, 5000.0);
+
+  // robot configs
+  private_nh_.param<double>("robot_length", robot_length_, 4.5);
+  private_nh_.param<double>("robot_width", robot_width_, 1.75);
+  private_nh_.param<double>("robot_base2back", robot_base2back_, 1.0);
+  private_nh_.param<double>("minimum_turning_radius", minimum_turning_radius_, 6.0);
+
+  // search configs
+  private_nh_.param<int>("theta_size", theta_size_, 48);
+  private_nh_.param<double>("angle_goal_range", angle_goal_range_, 6.0);
+  private_nh_.param<double>("curve_weight", curve_weight_, 1.2);
+  private_nh_.param<double>("reverse_weight", reverse_weight_, 2.00);
+  private_nh_.param<double>("lateral_goal_range", lateral_goal_range_, 0.5);
+  private_nh_.param<double>("longitudinal_goal_range", longitudinal_goal_range_, 2.0);
+
+  // costmap configs
+  private_nh_.param<int>("obstacle_threshold", obstacle_threshold_, 100);
+  private_nh_.param<double>("potential_weight", potential_weight_, 10.0);
+  private_nh_.param<double>("distance_heuristic_weight", distance_heuristic_weight_, 1.0);
+
+  createStateUpdateTable();
+}
+
+AstarSearch::~AstarSearch()
+{
+}
+
+// state update table for hybrid astar
+void AstarSearch::createStateUpdateTable()
+{
+  // Vehicle moving for each angle
+  state_update_table_.resize(theta_size_);
+  double dtheta = 2.0 * M_PI / theta_size_;
+
+  // Minimum moving distance with one state update
+  //     arc  = r                       * theta
+  double step = minimum_turning_radius_ * dtheta;
+
+  for (int i = 0; i < theta_size_; i++)
+  {
+    double theta = dtheta * i;
+
+    // Calculate right and left circle
+    // Robot moves along these circles
+    double right_circle_center_x = minimum_turning_radius_ * std::sin(theta);
+    double right_circle_center_y = minimum_turning_radius_ * -std::cos(theta);
+    double left_circle_center_x = -right_circle_center_x;
+    double left_circle_center_y = -right_circle_center_y;
+
+    // Calculate x and y shift to next state
+    NodeUpdate nu;
+
+    // forward
+    nu.shift_x = step * std::cos(theta);
+    nu.shift_y = step * std::sin(theta);
+    nu.rotation = 0.0;
+    nu.index_theta = 0;
+    nu.step = step;
+    nu.curve = false;
+    nu.back = false;
+    state_update_table_[i].emplace_back(nu);
+
+    // forward right
+    nu.shift_x = right_circle_center_x + minimum_turning_radius_ * std::cos(M_PI_2 + theta - dtheta);
+    nu.shift_y = right_circle_center_y + minimum_turning_radius_ * std::sin(M_PI_2 + theta - dtheta);
+    nu.rotation = -dtheta;
+    nu.index_theta = -1;
+    nu.step = step;
+    nu.curve = true;
+    nu.back = false;
+    state_update_table_[i].emplace_back(nu);
+
+    // forward left
+    nu.shift_x = left_circle_center_x + minimum_turning_radius_ * std::cos(-M_PI_2 + theta + dtheta);
+    nu.shift_y = left_circle_center_y + minimum_turning_radius_ * std::sin(-M_PI_2 + theta + dtheta);
+    nu.rotation = dtheta;
+    nu.index_theta = 1;
+    nu.step = step;
+    nu.curve = true;
+    nu.back = false;
+    state_update_table_[i].emplace_back(nu);
+
+    if (use_back_)
+    {
+      // backward
+      nu.shift_x = step * std::cos(theta) * -1.0;
+      nu.shift_y = step * std::sin(theta) * -1.0;
+      nu.rotation = 0;
+      nu.index_theta = 0;
+      nu.step = step;
+      nu.curve = false;
+      nu.back = true;
+      state_update_table_[i].emplace_back(nu);
+
+      // backward right
+      nu.shift_x = right_circle_center_x + minimum_turning_radius_ * std::cos(M_PI_2 + theta + dtheta);
+      nu.shift_y = right_circle_center_y + minimum_turning_radius_ * std::sin(M_PI_2 + theta + dtheta);
+      nu.rotation = dtheta;
+      nu.index_theta = 1;
+      nu.step = step;
+      nu.curve = true;
+      nu.back = true;
+      state_update_table_[i].emplace_back(nu);
+
+      // backward left
+      nu.shift_x = left_circle_center_x + minimum_turning_radius_ * std::cos(-1.0 * M_PI_2 + theta - dtheta);
+      nu.shift_y = left_circle_center_y + minimum_turning_radius_ * std::sin(-1.0 * M_PI_2 + theta - dtheta);
+      nu.rotation = dtheta * -1.0;
+      nu.index_theta = -1;
+      nu.step = step;
+      nu.curve = true;
+      nu.back = true;
+      state_update_table_[i].emplace_back(nu);
+    }
+  }
+}
+
+void AstarSearch::initialize(const nav_msgs::OccupancyGrid& costmap)
+{
+  costmap_ = costmap;
+
+  int height = costmap_.info.height;
+  int width = costmap_.info.width;
+
+  // size initialization
+  nodes_.resize(height);
+  for (int i = 0; i < height; i++)
+  {
+    nodes_[i].resize(width);
+  }
+  for (int i = 0; i < height; i++)
+  {
+    for (int j = 0; j < width; j++)
+    {
+      nodes_[i][j].resize(theta_size_);
+    }
+  }
+
+  // cost initialization
+  for (int i = 0; i < height; i++)
+  {
+    for (int j = 0; j < width; j++)
+    {
+      // Index of subscribing OccupancyGrid message
+      int og_index = i * width + j;
+      int cost = costmap_.data[og_index];
+
+      // hc is set to be 0 when reset()
+      if (cost == 0)
+      {
+        continue;
+      }
+
+      // obstacle or unknown area
+      if (cost < 0 || obstacle_threshold_ <= cost)
+      {
+        nodes_[i][j][0].status = STATUS::OBS;
+      }
+
+      // the cost more than threshold is regarded almost same as an obstacle
+      // because of its very high cost
+      if (use_potential_heuristic_)
+      {
+        nodes_[i][j][0].hc = cost * potential_weight_;
+      }
+    }
+  }
+}
+
+bool AstarSearch::makePlan(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& goal_pose)
+{
+  if (!setStartNode(start_pose))
+  {
+    // ROS_WARN_STREAM("Invalid start pose");
+    return false;
+  }
+
+  if (!setGoalNode(goal_pose))
+  {
+    // ROS_WARN_STREAM("Invalid goal pose");
+    return false;
+  }
+
+  return search();
+}
+
+bool AstarSearch::setStartNode(const geometry_msgs::Pose& start_pose)
+{
+  // Get index of start pose
+  int index_x, index_y, index_theta;
+  start_pose_local_.pose = start_pose;
+  poseToIndex(start_pose_local_.pose, &index_x, &index_y, &index_theta);
+  SimpleNode start_sn(index_x, index_y, index_theta, 0, 0);
+
+  // Check if start is valid
+  if (isOutOfRange(index_x, index_y) || detectCollision(start_sn))
+  {
+    return false;
+  }
+
+  // Set start node
+  AstarNode& start_node = nodes_[index_y][index_x][index_theta];
+  start_node.x = start_pose_local_.pose.position.x;
+  start_node.y = start_pose_local_.pose.position.y;
+  start_node.theta = 2.0 * M_PI / theta_size_ * index_theta;
+  start_node.gc = 0;
+  start_node.move_distance = 0;
+  start_node.back = false;
+  start_node.status = STATUS::OPEN;
+  start_node.parent = NULL;
+
+  // set euclidean distance heuristic cost
+  if (!use_wavefront_heuristic_ && !use_potential_heuristic_)
+  {
+    start_node.hc = calcDistance(start_pose_local_.pose.position.x, start_pose_local_.pose.position.y,
+                                 goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) *
+                    distance_heuristic_weight_;
+  }
+  else if (use_potential_heuristic_)
+  {
+    start_node.gc += start_node.hc;
+    start_node.hc += calcDistance(start_pose_local_.pose.position.x, start_pose_local_.pose.position.y,
+                                  goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) +
+                     distance_heuristic_weight_;
+  }
+
+  // Push start node to openlist
+  start_sn.cost = start_node.gc + start_node.hc;
+  openlist_.push(start_sn);
+
+  return true;
+}
+
+bool AstarSearch::setGoalNode(const geometry_msgs::Pose& goal_pose)
+{
+  goal_pose_local_.pose = goal_pose;
+  goal_yaw_ = modifyTheta(tf::getYaw(goal_pose_local_.pose.orientation));
+
+  // Get index of goal pose
+  int index_x, index_y, index_theta;
+  poseToIndex(goal_pose_local_.pose, &index_x, &index_y, &index_theta);
+  SimpleNode goal_sn(index_x, index_y, index_theta, 0, 0);
+
+  // Check if goal is valid
+  if (isOutOfRange(index_x, index_y) || detectCollision(goal_sn))
+  {
+    return false;
+  }
+
+  // Calculate wavefront heuristic cost
+  if (use_wavefront_heuristic_)
+  {
+    // auto start = std::chrono::system_clock::now();
+    bool wavefront_result = calcWaveFrontHeuristic(goal_sn);
+    // auto end = std::chrono::system_clock::now();
+    // auto usec = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
+    // std::cout << "wavefront : " << usec / 1000.0 << "[msec]" << std::endl;
+
+    if (!wavefront_result)
+    {
+      // ROS_WARN("Reachable is false...");
+      return false;
+    }
+  }
+
+  return true;
+}
+
+void AstarSearch::poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y, int* index_theta)
+{
+  tf::Transform orig_tf;
+  tf::poseMsgToTF(costmap_.info.origin, orig_tf);
+  geometry_msgs::Pose pose2d = transformPose(pose, orig_tf.inverse());
+
+  *index_x = pose2d.position.x / costmap_.info.resolution;
+  *index_y = pose2d.position.y / costmap_.info.resolution;
+
+  tf::Quaternion quat;
+  tf::quaternionMsgToTF(pose2d.orientation, quat);
+  double yaw = tf::getYaw(quat);
+  if (yaw < 0)
+    yaw += 2.0 * M_PI;
+
+  // Descretize angle
+  static double one_angle_range = 2.0 * M_PI / theta_size_;
+  *index_theta = yaw / one_angle_range;
+  *index_theta %= theta_size_;
+}
+
+void AstarSearch::pointToIndex(const geometry_msgs::Point& point, int* index_x, int* index_y)
+{
+  geometry_msgs::Pose pose;
+  pose.position = point;
+  int index_theta;
+  poseToIndex(pose, index_x, index_y, &index_theta);
+}
+
+bool AstarSearch::isOutOfRange(int index_x, int index_y)
+{
+  if (index_x < 0 || index_x >= static_cast<int>(costmap_.info.width) || index_y < 0 ||
+      index_y >= static_cast<int>(costmap_.info.height))
+  {
+    return true;
+  }
+  return false;
+}
+
+bool AstarSearch::search()
+{
+  ros::WallTime begin = ros::WallTime::now();
+
+  // Start A* search
+  // If the openlist is empty, search failed
+  while (!openlist_.empty())
+  {
+    // Check time and terminate if the search reaches the time limit
+    ros::WallTime now = ros::WallTime::now();
+    double msec = (now - begin).toSec() * 1000.0;
+    if (msec > time_limit_)
+    {
+      // ROS_WARN("Exceed time limit of %lf [ms]", time_limit_);
+      return false;
+    }
+
+    // Pop minimum cost node from openlist
+    SimpleNode top_sn = openlist_.top();
+    openlist_.pop();
+
+    // Expand nodes from this node
+    AstarNode* current_an = &nodes_[top_sn.index_y][top_sn.index_x][top_sn.index_theta];
+    current_an->status = STATUS::CLOSED;
+
+    // Goal check
+    if (isGoal(current_an->x, current_an->y, current_an->theta))
+    {
+      // ROS_INFO("Search time: %lf [msec]", (now - begin).toSec() * 1000.0);
+      setPath(top_sn);
+      return true;
+    }
+
+    // Expand nodes
+    for (const auto& state : state_update_table_[top_sn.index_theta])
+    {
+      // Next state
+      double next_x = current_an->x + state.shift_x;
+      double next_y = current_an->y + state.shift_y;
+      double next_theta = modifyTheta(current_an->theta + state.rotation);
+      double move_cost = state.step;
+      double move_distance = current_an->move_distance + state.step;
+
+      // Increase reverse cost
+      if (state.back != current_an->back)
+        move_cost *= reverse_weight_;
+
+      // Calculate index of the next state
+      SimpleNode next_sn;
+      geometry_msgs::Point next_pos;
+      next_pos.x = next_x;
+      next_pos.y = next_y;
+      pointToIndex(next_pos, &next_sn.index_x, &next_sn.index_y);
+      next_sn.index_theta = top_sn.index_theta + state.index_theta;
+
+      // Avoid invalid index
+      next_sn.index_theta = (next_sn.index_theta + theta_size_) % theta_size_;
+
+      // Check if the index is valid
+      if (isOutOfRange(next_sn.index_x, next_sn.index_y) || detectCollision(next_sn))
+      {
+        continue;
+      }
+
+      AstarNode* next_an = &nodes_[next_sn.index_y][next_sn.index_x][next_sn.index_theta];
+      double next_gc = current_an->gc + move_cost;
+      double next_hc = nodes_[next_sn.index_y][next_sn.index_x][0].hc;  // wavefront or distance transform heuristic
+
+      // increase the cost with euclidean distance
+      if (use_potential_heuristic_)
+      {
+        next_gc += nodes_[next_sn.index_y][next_sn.index_x][0].hc;
+        next_hc += calcDistance(next_x, next_y, goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) *
+                   distance_heuristic_weight_;
+      }
+
+      // increase the cost with euclidean distance
+      if (!use_wavefront_heuristic_ && !use_potential_heuristic_)
+      {
+        next_hc = calcDistance(next_x, next_y, goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) *
+                  distance_heuristic_weight_;
+      }
+
+      // NONE
+      if (next_an->status == STATUS::NONE)
+      {
+        next_an->status = STATUS::OPEN;
+        next_an->x = next_x;
+        next_an->y = next_y;
+        next_an->theta = next_theta;
+        next_an->gc = next_gc;
+        next_an->hc = next_hc;
+        next_an->move_distance = move_distance;
+        next_an->back = state.back;
+        next_an->parent = current_an;
+        next_sn.cost = next_an->gc + next_an->hc;
+        openlist_.push(next_sn);
+        continue;
+      }
+
+      // OPEN or CLOSED
+      if (next_an->status == STATUS::OPEN || next_an->status == STATUS::CLOSED)
+      {
+        if (next_gc < next_an->gc)
+        {
+          next_an->status = STATUS::OPEN;
+          next_an->x = next_x;
+          next_an->y = next_y;
+          next_an->theta = next_theta;
+          next_an->gc = next_gc;
+          next_an->hc = next_hc;  // already calculated ?
+          next_an->move_distance = move_distance;
+          next_an->back = state.back;
+          next_an->parent = current_an;
+          next_sn.cost = next_an->gc + next_an->hc;
+          openlist_.push(next_sn);
+          continue;
+        }
+      }
+    }  // state update
+  }
+
+  // Failed to find path
+  // ROS_INFO("Open list is empty...");
+  return false;
+}
+
+void AstarSearch::setPath(const SimpleNode& goal)
+{
+  std_msgs::Header header;
+  header.stamp = ros::Time::now();
+  header.frame_id = costmap_.header.frame_id;
+  path_.header = header;
+
+  // From the goal node to the start node
+  AstarNode* node = &nodes_[goal.index_y][goal.index_x][goal.index_theta];
+
+  while (node != NULL)
+  {
+    // Set tf pose
+    tf::Vector3 origin(node->x, node->y, 0);
+    tf::Pose tf_pose;
+    tf_pose.setOrigin(origin);
+    tf_pose.setRotation(tf::createQuaternionFromYaw(node->theta));
+
+    // Set path as ros message
+    geometry_msgs::PoseStamped ros_pose;
+    tf::poseTFToMsg(tf_pose, ros_pose.pose);
+    ros_pose.header = header;
+    path_.poses.push_back(ros_pose);
+
+    // To the next node
+    node = node->parent;
+  }
+
+  // Reverse the vector to be start to goal order
+  std::reverse(path_.poses.begin(), path_.poses.end());
+}
+
+// Check if the next state is the goal
+// Check lateral offset, longitudinal offset and angle
+bool AstarSearch::isGoal(double x, double y, double theta)
+{
+  // To reduce computation time, we use square value for distance
+  static const double lateral_goal_range =
+      lateral_goal_range_ / 2.0;  // [meter], divide by 2 means we check left and right
+  static const double longitudinal_goal_range =
+      longitudinal_goal_range_ / 2.0;                                         // [meter], check only behind of the goal
+  static const double goal_angle = M_PI * (angle_goal_range_ / 2.0) / 180.0;  // degrees -> radian
+
+  // Calculate the node coordinate seen from the goal point
+  tf::Point p(x, y, 0);
+  geometry_msgs::Point relative_node_point = calcRelativeCoordinate(goal_pose_local_.pose, p);
+
+  // Check Pose of goal
+  if (relative_node_point.x < 0 &&  // shoud be behind of goal
+      std::fabs(relative_node_point.x) < longitudinal_goal_range &&
+      std::fabs(relative_node_point.y) < lateral_goal_range)
+  {
+    // Check the orientation of goal
+    if (calcDiffOfRadian(goal_yaw_, theta) < goal_angle)
+    {
+      return true;
+    }
+  }
+
+  return false;
+}
+
+bool AstarSearch::isObs(int index_x, int index_y)
+{
+  if (nodes_[index_y][index_x][0].status == STATUS::OBS)
+  {
+    return true;
+  }
+
+  return false;
+}
+
+bool AstarSearch::detectCollision(const SimpleNode& sn)
+{
+  // Define the robot as rectangle
+  static double left = -1.0 * robot_base2back_;
+  static double right = robot_length_ - robot_base2back_;
+  static double top = robot_width_ / 2.0;
+  static double bottom = -1.0 * robot_width_ / 2.0;
+  static double resolution = costmap_.info.resolution;
+
+  // Coordinate of base_link in OccupancyGrid frame
+  static double one_angle_range = 2.0 * M_PI / theta_size_;
+  double base_x = sn.index_x * resolution;
+  double base_y = sn.index_y * resolution;
+  double base_theta = sn.index_theta * one_angle_range;
+
+  // Calculate cos and sin in advance
+  double cos_theta = std::cos(base_theta);
+  double sin_theta = std::sin(base_theta);
+
+  // Convert each point to index and check if the node is Obstacle
+  for (double x = left; x < right; x += resolution)
+  {
+    for (double y = top; y > bottom; y -= resolution)
+    {
+      // 2D point rotation
+      int index_x = (x * cos_theta - y * sin_theta + base_x) / resolution;
+      int index_y = (x * sin_theta + y * cos_theta + base_y) / resolution;
+
+      if (isOutOfRange(index_x, index_y))
+      {
+        return true;
+      }
+      else if (nodes_[index_y][index_x][0].status == STATUS::OBS)
+      {
+        return true;
+      }
+    }
+  }
+
+  return false;
+}
+
+bool AstarSearch::calcWaveFrontHeuristic(const SimpleNode& sn)
+{
+  // Set start point for wavefront search
+  // This is goal for Astar search
+  nodes_[sn.index_y][sn.index_x][0].hc = 0;
+  WaveFrontNode wf_node(sn.index_x, sn.index_y, 1e-10);
+  std::queue<WaveFrontNode> qu;
+  qu.push(wf_node);
+
+  // State update table for wavefront search
+  // Nodes are expanded for each neighborhood cells (moore neighborhood)
+  double resolution = costmap_.info.resolution;
+  static std::vector<WaveFrontNode> updates = {
+    getWaveFrontNode(0, 1, resolution),
+    getWaveFrontNode(-1, 0, resolution),
+    getWaveFrontNode(1, 0, resolution),
+    getWaveFrontNode(0, -1, resolution),
+    getWaveFrontNode(-1, 1, std::hypot(resolution, resolution)),
+    getWaveFrontNode(1, 1, std::hypot(resolution, resolution)),
+    getWaveFrontNode(-1, -1, std::hypot(resolution, resolution)),
+    getWaveFrontNode(1, -1, std::hypot(resolution, resolution)),
+  };
+
+  // Get start index
+  int start_index_x;
+  int start_index_y;
+  int start_index_theta;
+  poseToIndex(start_pose_local_.pose, &start_index_x, &start_index_y, &start_index_theta);
+
+  // Whether the robot can reach goal
+  bool reachable = false;
+
+  // Start wavefront search
+  while (!qu.empty())
+  {
+    WaveFrontNode ref = qu.front();
+    qu.pop();
+
+    WaveFrontNode next;
+    for (const auto& u : updates)
+    {
+      next.index_x = ref.index_x + u.index_x;
+      next.index_y = ref.index_y + u.index_y;
+
+      // out of range OR already visited OR obstacle node
+      if (isOutOfRange(next.index_x, next.index_y) || nodes_[next.index_y][next.index_x][0].hc > 0 ||
+          nodes_[next.index_y][next.index_x][0].status == STATUS::OBS)
+      {
+        continue;
+      }
+
+      // Take the size of robot into account
+      if (detectCollisionWaveFront(next))
+      {
+        continue;
+      }
+
+      // Check if we can reach from start to goal
+      if (next.index_x == start_index_x && next.index_y == start_index_y)
+      {
+        reachable = true;
+      }
+
+      // Set wavefront heuristic cost
+      next.hc = ref.hc + u.hc;
+      nodes_[next.index_y][next.index_x][0].hc = next.hc;
+
+      qu.push(next);
+    }
+  }
+
+  // End of search
+  return reachable;
+}
+
+// Simple collidion detection for wavefront search
+bool AstarSearch::detectCollisionWaveFront(const WaveFrontNode& ref)
+{
+  // Define the robot as square
+  static double half = robot_width_ / 2;
+  double robot_x = ref.index_x * costmap_.info.resolution;
+  double robot_y = ref.index_y * costmap_.info.resolution;
+
+  for (double y = half; y > -1.0 * half; y -= costmap_.info.resolution)
+  {
+    for (double x = -1.0 * half; x < half; x += costmap_.info.resolution)
+    {
+      int index_x = (robot_x + x) / costmap_.info.resolution;
+      int index_y = (robot_y + y) / costmap_.info.resolution;
+
+      if (isOutOfRange(index_x, index_y))
+      {
+        return true;
+      }
+
+      if (nodes_[index_y][index_x][0].status == STATUS::OBS)
+      {
+        return true;
+      }
+    }
+  }
+
+  return false;
+}
+
+void AstarSearch::reset()
+{
+  path_.poses.clear();
+
+  // Clear queue
+  std::priority_queue<SimpleNode, std::vector<SimpleNode>, std::greater<SimpleNode>> empty;
+  std::swap(openlist_, empty);
+
+  // ros::WallTime begin = ros::WallTime::now();
+
+  // Reset node info here ...?
+  for (size_t i = 0; i < costmap_.info.height; i++)
+  {
+    for (size_t j = 0; j < costmap_.info.width; j++)
+    {
+      for (int k = 0; k < theta_size_; k++)
+      {
+        // other values will be updated during the search
+        nodes_[i][j][k].status = STATUS::NONE;
+        nodes_[i][j][k].hc = 0;
+      }
+    }
+  }
+
+  // ros::WallTime end = ros::WallTime::now();
+
+  // ROS_INFO("Reset time: %lf [ms]", (end - begin).toSec() * 1000);
+}

src/ros/catkin/src/astar_search/src/astar_util.cpp

@@ -0,0 +1,34 @@
+/*
+ * 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 "astar_search/astar_util.h"
+
+WaveFrontNode::WaveFrontNode() : index_x(0), index_y(0), hc(0)
+{
+}
+
+WaveFrontNode::WaveFrontNode(int x, int y, double cost) : index_x(x), index_y(y), hc(cost)
+{
+}
+
+SimpleNode::SimpleNode()
+{
+}
+
+SimpleNode::SimpleNode(int x, int y, int theta, double gc, double hc)
+  : index_x(x), index_y(y), index_theta(theta), cost(gc + hc)
+{
+}

src/ros/catkin/src/astar_search/test/src/test_astar_search.cpp

@@ -0,0 +1,169 @@
+/*
+ * Copyright 2018 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 "astar_search/astar_search.h"
+
+#include "test_class.h"
+
+TEST_F(TestSuite, checkPoseAndPoint_ToIndex)
+{
+  //Check poseToIndex
+
+  // start_pose is located at the center of the grid
+  test_obj_.poseToIndex(start_pose_, &index_x, &index_y, &index_theta);
+  ASSERT_EQ(index_x, test_obj_.costmap_.info.width/2) << "start_pose.index_x should be " << test_obj_.costmap_.info.width/2;
+  ASSERT_EQ(index_y, test_obj_.costmap_.info.height/2) << "start_pose.index_y should be " << test_obj_.costmap_.info.height/2;
+  ASSERT_EQ(index_theta, 0) << "start_pose.index_theta should be " << 0;
+
+  // goal_pose is 10 cells to the right from start_pose
+  test_obj_.poseToIndex(goal_pose_, &index_x, &index_y, &index_theta);
+  ASSERT_EQ(index_x, test_obj_.costmap_.info.width/2+10) << "goal_pose.index_x should be " << test_obj_.costmap_.info.width/2+10;
+  ASSERT_EQ(index_y, test_obj_.costmap_.info.height/2) << "goal_pose.index_y should be " << test_obj_.costmap_.info.height/2;
+  ASSERT_EQ(index_theta, 0) << "goal_pose.index_theta should be " << 0;
+
+  //Check pointToIndex
+  geometry_msgs::Point test_point;
+
+  test_point.x = start_pose_.position.x;
+  test_point.y = start_pose_.position.y;
+  test_point.z = start_pose_.position.z;
+
+  test_obj_.pointToIndex(test_point, &index_x, &index_y);
+  ASSERT_EQ(index_x, test_obj_.costmap_.info.width/2) << "start_point.index_x should be " << test_obj_.costmap_.info.width/2;
+  ASSERT_EQ(index_y, test_obj_.costmap_.info.height/2) << "start_point.index_y should be " << test_obj_.costmap_.info.height/2;
+
+  test_point.x = goal_pose_.position.x;
+  test_point.y = goal_pose_.position.y;
+  test_point.z = goal_pose_.position.z;
+
+  test_obj_.pointToIndex(test_point, &index_x, &index_y);
+  ASSERT_EQ(index_x, test_obj_.costmap_.info.width/2+10) << "goal_pose.index_x should be " << test_obj_.costmap_.info.width/2+10;
+  ASSERT_EQ(index_y, test_obj_.costmap_.info.height/2) << "goal_pose.index_y should be " << test_obj_.costmap_.info.height/2;
+}
+
+TEST_F(TestSuite, checkIsOutOfRange)
+{
+
+  // Check that the indexes are out and within range
+  index_x = test_obj_.costmap_.info.width/2;
+  index_y = test_obj_.costmap_.info.height/2;
+  ASSERT_FALSE(test_obj_.isOutOfRange(index_x, index_y)) << "[index_x,index_y] : [" << index_x << "," << index_y << "] Should be outOfRange";
+
+  // Check that the indexes are out and within range
+  index_x = test_obj_.costmap_.info.width + 1;
+  index_y = test_obj_.costmap_.info.height + 1;
+  ASSERT_TRUE(test_obj_.isOutOfRange(index_x, index_y)) << "[index_x,index_y] : [" << index_x << "," << index_y << "] Should be outOfRange";
+
+  index_x = -1;
+  index_y = -1;
+  ASSERT_TRUE(test_obj_.isOutOfRange(index_x, index_y)) << "[index_x,index_y] : [" << index_x << "," << index_y << "] Should be outOfRange";
+}
+
+TEST_F(TestSuite, checkSetNodes)
+{
+
+  // start_pose_ within range
+  ASSERT_TRUE(test_obj_.setStartNode(start_pose_)) << "Should be true";
+  // start_pose_ out of range
+  start_pose_.position.x = (test_obj_.costmap_.info.width + 10)*test_obj_.costmap_.info.resolution;
+  ASSERT_FALSE(test_obj_.setStartNode(start_pose_)) << "Should be false";
+
+  // goal_pose_ out of range
+  ASSERT_FALSE(test_obj_.setGoalNode(start_pose_)) << "Should be false";
+  // goal_pose_ within range
+  ASSERT_TRUE(test_obj_.setGoalNode(goal_pose_)) << "Should be true";
+
+  // Check isGoal
+  ASSERT_TRUE(test_obj_.isGoal(goal_pose_.position.x - 0.1, goal_pose_.position.y, 0)) << "Goal should be goal";
+  ASSERT_FALSE(test_obj_.isGoal(goal_pose_.position.x - 1, goal_pose_.position.y, 0)) << "Should NOT be goal";
+
+}
+
+TEST_F(TestSuite, checkIsObs)
+{
+
+  // Initialise costmap
+  test_obj_.astar_search_obj.initialize(test_obj_.costmap_);
+
+  ASSERT_TRUE(test_obj_.isObs(test_obj_.obstacle_indexes_[0].first, test_obj_.obstacle_indexes_[0].second)) << "[index_x,index_y] : [" << test_obj_.obstacle_indexes_[0].first << "," << test_obj_.obstacle_indexes_[0].second<< "] Should be obstacle";
+  ASSERT_FALSE(test_obj_.isObs(1,1)) << "[index_x,index_y] : [" << 1 << "," << 1 << "] Should NOT be obstacle";
+
+}
+
+TEST_F(TestSuite, checkDetectCollision)
+{
+
+  // Node at start_pose_
+  index_x = test_obj_.costmap_.info.width/2;
+  index_y = test_obj_.costmap_.info.height/2;
+  index_theta = 0;
+  SimpleNode start_sn(index_x, index_y, index_theta, 0, 0);
+  ASSERT_FALSE(test_obj_.detectCollision(start_sn)) << "start_pose_ node should not collide";
+
+  // Node just before obstacle
+  index_x = test_obj_.obstacle_indexes_[0].first - 1;
+  index_y = test_obj_.obstacle_indexes_[0].second;
+  index_theta = 0;
+  SimpleNode obs_sn(index_x, index_y, index_theta, 0, 0);
+  ASSERT_TRUE(test_obj_.detectCollision(obs_sn)) << "node before obstacle should collide";
+
+}
+
+TEST_F(TestSuite, checkSetPath)
+{
+
+  test_obj_.astar_search_obj.initialize(test_obj_.costmap_);
+  test_obj_.setStartNode(start_pose_);
+  test_obj_.setGoalNode(goal_pose_);
+
+  ASSERT_TRUE(test_obj_.search());
+  ASSERT_FALSE(test_obj_.astar_search_obj.getPath().poses.empty()) << "path should not be empty";
+
+  test_obj_.astar_search_obj.reset();
+  ASSERT_TRUE(test_obj_.astar_search_obj.getPath().poses.empty()) << "path should be empty after reset";
+
+  // Block goal with obstacle
+  int cnt = 0;
+  for (int row = 0; row < test_obj_.costmap_.info.height; ++row)
+  {
+    for (int col = 0; col < test_obj_.costmap_.info.width; ++col)
+    {
+      for (std::vector<std::pair<int, int>>::iterator it = test_obj_.obstacle_indexes_.begin(); it != test_obj_.obstacle_indexes_.end(); it++)
+      {
+        if (col == it->first && row == it->second)
+        {
+          test_obj_.costmap_.data.at(cnt) = 100;
+        }
+      }
+      cnt++;
+    }
+  }
+
+  test_obj_.astar_search_obj.initialize(test_obj_.costmap_);
+  test_obj_.setStartNode(start_pose_);
+  test_obj_.setGoalNode(goal_pose_);
+  ASSERT_FALSE(test_obj_.search());
+  ASSERT_TRUE(test_obj_.astar_search_obj.getPath().poses.empty()) << "path should be empty after reset";
+}
+
+TEST_F(TestSuite, checkMakePlan)
+{
+  test_obj_.astar_search_obj.initialize(test_obj_.costmap_);
+  ASSERT_TRUE(test_obj_.astar_search_obj.makePlan(start_pose_, goal_pose_)) << "makePlan should return True";
+}

src/ros/catkin/src/astar_search/test/src/test_astar_util.cpp

@@ -0,0 +1,160 @@
+/*
+ * Copyright 2018 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 "astar_search/astar_util.h"
+
+#include "test_class.h"
+
+//class TestSuite2: public ::testing::Test {
+//public:
+//  TestSuite2(){}
+//  ~TestSuite2(){}
+//};
+
+TEST_F(TestSuite, CheckGreaterThanOperator){
+
+  int x = 0;
+  int y = 0;
+  int theta = 0;
+  double gc = 0;
+  double hc = 10;
+
+  SimpleNode simple_node1;
+  SimpleNode simple_node2(x, y, theta, gc, hc);
+
+  ASSERT_TRUE(simple_node2 > simple_node1) << "Should be true";
+  ASSERT_TRUE(!(simple_node1 > simple_node2)) << "Should be false";
+}
+
+TEST_F(TestSuite, CalculateDistanceBetween2Points){
+  // Point 1
+  double x1 = 0.0;
+  double y1 = 0.0;
+  // Point 2
+  double x2 = 3.0;
+  double y2 = 4.0;
+  ASSERT_EQ(calcDistance(x1, y1, x2, y2), sqrt(x2*x2 + y2*y2)) << "Distance should be " << sqrt(x2*x2 + y2*y2);
+}
+
+TEST_F(TestSuite, CheckThetaWrapAround){
+  double theta = -90; //Degrees
+  double theta_new = (theta+360)*M_PI/180;
+  ASSERT_DOUBLE_EQ(modifyTheta(theta*M_PI/180), theta_new) << "Angle should be " << theta_new;
+
+  theta = 400;
+  theta_new = (theta-360)*M_PI/180;
+  ASSERT_DOUBLE_EQ(modifyTheta(theta*M_PI/180), theta_new) << "Angle should be " << theta_new;
+
+  theta = 60;
+  theta_new = theta*M_PI/180;
+  ASSERT_DOUBLE_EQ(modifyTheta(theta*M_PI/180), theta_new) << "Angle should be " << theta_new;
+}
+
+TEST_F(TestSuite, CheckTransformPose){
+
+  // Check translation of 1 along X axis
+  tf::Quaternion q(0,0,0,1);
+  tf::Vector3 v(1,0,0);
+  geometry_msgs::Pose in_pose, out_pose, expected_pose;
+
+  in_pose.position.x = 0;
+  in_pose.position.y = 0;
+  in_pose.position.z = 0;
+  in_pose.orientation.x = 0;
+  in_pose.orientation.y = 0;
+  in_pose.orientation.z = 0;
+  in_pose.orientation.w = 1;
+  expected_pose.position.x = 1;
+  expected_pose.position.y = 0;
+  expected_pose.position.z = 0;
+  expected_pose.orientation.x = 0;
+  expected_pose.orientation.y = 0;
+  expected_pose.orientation.z = 0;
+  expected_pose.orientation.w = 1;
+  tf::Transform translation(q, v);
+
+  out_pose = transformPose(in_pose, translation);
+
+  ASSERT_DOUBLE_EQ(out_pose.position.x, expected_pose.position.x) << "X Coordinate should be " << expected_pose.position.x;
+  ASSERT_DOUBLE_EQ(out_pose.position.y, expected_pose.position.y) << "Y Coordinate should be " << expected_pose.position.y;
+  ASSERT_DOUBLE_EQ(out_pose.position.z, expected_pose.position.z) << "Z Coordinate should be " << expected_pose.position.z;
+  ASSERT_DOUBLE_EQ(out_pose.orientation.x, expected_pose.orientation.x) << "X Quaternion should be " << expected_pose.orientation.x;
+  ASSERT_DOUBLE_EQ(out_pose.orientation.y, expected_pose.orientation.y) << "Y Quaternion should be " << expected_pose.orientation.y;
+  ASSERT_DOUBLE_EQ(out_pose.orientation.z, expected_pose.orientation.z) << "Z Quaternion should be " << expected_pose.orientation.z;
+  ASSERT_DOUBLE_EQ(out_pose.orientation.w, expected_pose.orientation.w) << "W Quaternion should be " << expected_pose.orientation.w;
+
+}
+
+TEST_F(TestSuite, CheckWaveFrontNodeConstruct){
+
+  WaveFrontNode node;
+  ASSERT_EQ(node.index_x, 0) << "index_x should be " << 0;
+  ASSERT_EQ(node.index_y, 0) << "index_y should be " << 0;
+  ASSERT_EQ(node.hc, 0) << "hc should be " << 0;
+
+  int x = 0;
+  int y = 0;
+  double cost = 10;
+  node = getWaveFrontNode(x, y, cost);
+  ASSERT_EQ(node.index_x, x) << "index_x should be " << x;
+  ASSERT_EQ(node.index_y, y) << "index_y should be " << y;
+  ASSERT_EQ(node.hc, cost) << "hc should be " << cost;
+}
+
+TEST_F(TestSuite, CheckRelativeCoordinate){
+
+  geometry_msgs::Pose in_pose;
+  in_pose.position.x = 1;
+  in_pose.position.y = -4;
+  in_pose.position.z = 8;
+  in_pose.orientation.x = 0;
+  in_pose.orientation.y = 0;
+  in_pose.orientation.z = 0;
+  in_pose.orientation.w = 1;
+
+  tf::Point point = tf::Point(0,0,0);
+
+  geometry_msgs::Point relative_point = calcRelativeCoordinate(in_pose, point);
+  geometry_msgs::Point expected_point;
+  expected_point.x = -in_pose.position.x;
+  expected_point.y = -in_pose.position.y;
+  expected_point.z = -in_pose.position.z;
+
+  ASSERT_EQ(relative_point.x, expected_point.x) << "X coord should be " << expected_point.x;
+  ASSERT_EQ(relative_point.y, expected_point.y) << "Y coord should be " << expected_point.y;
+  ASSERT_EQ(relative_point.z, expected_point.z) << "Z coord should be " << expected_point.z;
+}
+
+TEST_F(TestSuite, CheckRadianDifference){
+
+  // Diff < 180 degrees
+  double a = 0*M_PI/180;
+  double b = 90*M_PI/180;
+  ASSERT_DOUBLE_EQ(calcDiffOfRadian(a, b), 90*M_PI/180) << "diff should be " << 90*M_PI/180;
+
+  // 180 degrees < Diff < 360 degrees
+  a = 0*M_PI/180;
+  b = 190*M_PI/180;
+  ASSERT_DOUBLE_EQ(calcDiffOfRadian(a, b), 170*M_PI/180) << "diff should be " << 170*M_PI/180;
+
+  // Diff > 360 degrees
+  a = 0*M_PI/180;
+  b = 400*M_PI/180;
+  ASSERT_DOUBLE_EQ(calcDiffOfRadian(a, b), 40*M_PI/180) << "diff should be " << 40*M_PI/180;
+}

src/ros/catkin/src/astar_search/test/src/test_class.cpp

@@ -0,0 +1,130 @@
+/*
+ * Copyright 2018 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 "test_class.h"
+
+TestClass::TestClass()
+{
+
+  obstacle_indexes_.push_back(std::make_pair(0,0));
+
+  obstacle_indexes_.push_back(std::make_pair(24,0));
+  obstacle_indexes_.push_back(std::make_pair(24,1));
+  obstacle_indexes_.push_back(std::make_pair(24,2));
+  obstacle_indexes_.push_back(std::make_pair(24,3));
+  obstacle_indexes_.push_back(std::make_pair(24,4));
+  obstacle_indexes_.push_back(std::make_pair(24,5));
+  obstacle_indexes_.push_back(std::make_pair(24,6));
+  obstacle_indexes_.push_back(std::make_pair(24,7));
+  obstacle_indexes_.push_back(std::make_pair(24,8));
+  obstacle_indexes_.push_back(std::make_pair(24,9));
+  obstacle_indexes_.push_back(std::make_pair(24,10));
+
+  costmap_.header.seq = 1;
+  costmap_.header.stamp = ros::Time::now();
+  costmap_.header.frame_id = "world";
+
+  costmap_.info.map_load_time = costmap_.header.stamp;
+  costmap_.info.resolution = 1;
+  costmap_.info.width = 41;
+  costmap_.info.height = 11;
+
+  // Costmap origin set to be the centre of the grid (as in points2costmap.cpp)
+  costmap_.info.origin.position.x = -costmap_.info.resolution*costmap_.info.width/2;
+  costmap_.info.origin.position.y = -costmap_.info.resolution*costmap_.info.height/2;
+  costmap_.info.origin.position.z = 0;
+  costmap_.info.origin.orientation.x = 0;
+  costmap_.info.origin.orientation.y = 0;
+  costmap_.info.origin.orientation.z = 0;
+  costmap_.info.origin.orientation.w = 1;
+
+  for (int idx = 0; idx < costmap_.info.width*costmap_.info.height; ++idx)
+  {
+    costmap_.data.push_back(0);
+  }
+
+  int cnt = 0;
+  for (int row = 0; row < costmap_.info.height; ++row)
+  {
+    for (int col = 0; col < costmap_.info.width; ++col)
+    {
+      for (std::vector<std::pair<int, int>>::iterator it = obstacle_indexes_.begin(); it != obstacle_indexes_.begin()+1; it++)
+      {
+        if (col == it->first && row == it->second)
+        {
+          costmap_.data.at(cnt) = 100;
+        }
+      }
+      cnt++;
+    }
+  }
+
+  ros::NodeHandle nh("~");
+  nh.setParam("lateral_goal_range", 1.5);
+}
+
+void TestClass::createStateUpdateTable()
+{
+  astar_search_obj.createStateUpdateTable();
+}
+bool TestClass::search()
+{
+  return astar_search_obj.search();
+}
+void TestClass::poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y, int* index_theta)
+{
+  astar_search_obj.poseToIndex(pose, index_x, index_y, index_theta);
+}
+void TestClass::pointToIndex(const geometry_msgs::Point& point, int* index_x, int* index_y)
+{
+  astar_search_obj.pointToIndex(point, index_x, index_y);
+}
+bool TestClass::isOutOfRange(int index_x, int index_y)
+{
+  return astar_search_obj.isOutOfRange(index_x, index_y);
+}
+void TestClass::setPath(const SimpleNode& goal)
+{
+  astar_search_obj.setPath(goal);
+}
+bool TestClass::setStartNode(const geometry_msgs::Pose& start_pose)
+{
+  return astar_search_obj.setStartNode(start_pose);
+}
+bool TestClass::setGoalNode(const geometry_msgs::Pose& goal_pose)
+{
+  return astar_search_obj.setGoalNode(goal_pose);
+}
+bool TestClass::isGoal(double x, double y, double theta)
+{
+  return astar_search_obj.isGoal(x, y, theta);
+}
+bool TestClass::isObs(int index_x, int index_y)
+{
+  return astar_search_obj.isObs(index_x, index_y);
+}
+bool TestClass::detectCollision(const SimpleNode& sn)
+{
+  return astar_search_obj.detectCollision(sn);
+}
+bool TestClass::calcWaveFrontHeuristic(const SimpleNode& sn)
+{
+  return astar_search_obj.calcWaveFrontHeuristic(sn);
+}
+bool TestClass::detectCollisionWaveFront(const WaveFrontNode& sn)
+{
+  return astar_search_obj.detectCollisionWaveFront(sn);
+}

src/ros/catkin/src/astar_search/test/src/test_class.h

@@ -0,0 +1,83 @@
+/*
+ * Copyright 2018 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 "astar_search/astar_search.h"
+
+class TestClass{
+public:
+  TestClass();
+
+  AstarSearch astar_search_obj;
+
+  void createStateUpdateTable();
+  bool search();
+  void poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y, int* index_theta);
+  void pointToIndex(const geometry_msgs::Point& point, int* index_x, int* index_y);
+  bool isOutOfRange(int index_x, int index_y);
+  void setPath(const SimpleNode& goal);
+  bool setStartNode(const geometry_msgs::Pose& start_pose);
+  bool setGoalNode(const geometry_msgs::Pose& goal_pose);
+  bool isGoal(double x, double y, double theta);
+  bool isObs(int index_x, int index_y);
+  bool detectCollision(const SimpleNode& sn);
+  bool calcWaveFrontHeuristic(const SimpleNode& sn);
+  bool detectCollisionWaveFront(const WaveFrontNode& sn);
+
+  nav_msgs::OccupancyGrid costmap_;
+
+  std::vector<std::pair<int, int>> obstacle_indexes_;
+
+};
+
+class TestSuite: public ::testing::Test {
+public:
+  TestSuite(){}
+  ~TestSuite(){}
+
+  TestClass test_obj_;
+
+  geometry_msgs::Pose start_pose_;
+  geometry_msgs::Pose goal_pose_;
+  int index_x, index_y, index_theta;
+
+  virtual void SetUp()
+  {
+    test_obj_.astar_search_obj.initialize(test_obj_.costmap_);
+
+    start_pose_.position.x = 0;
+    start_pose_.position.y = 0;
+    start_pose_.position.z = 0;
+    start_pose_.orientation.x = 0;
+    start_pose_.orientation.y = 0;
+    start_pose_.orientation.z = 0;
+    start_pose_.orientation.w = 1;
+
+    goal_pose_.position.x = test_obj_.costmap_.info.resolution * 10;
+    goal_pose_.position.y = 0;
+    goal_pose_.position.z = 0;
+    goal_pose_.orientation.x = 0;
+    goal_pose_.orientation.y = 0;
+    goal_pose_.orientation.z = 0;
+    goal_pose_.orientation.w = 1;
+
+    index_x = 0;
+    index_y = 0;
+    index_theta = 0;
+  }
+};

src/ros/catkin/src/astar_search/test/src/test_main.cpp

@@ -0,0 +1,30 @@
+/*
+ * Copyright 2018 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>
+
+class TestSuite: public ::testing::Test {
+public:
+  TestSuite(){}
+  ~TestSuite(){}
+};
+
+int main(int argc, char **argv) {
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "TestNode");
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/astar_search/test/test_astar_search.test

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

src/ros/catkin/src/autoware_lanelet2_msgs/CHANGELOG.rst

@@ -0,0 +1,9 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package autoware_lanelet2_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.13.0 (2019-12-03)
+-------------------
+* Update package.xml of autoware_lanelet2_msgs for release.
+* Rename lanelet2_msgs to autoware_lanelet2_msgs
+* Contributors: Joshua Whitley, Kenji Miyake

src/ros/catkin/src/autoware_lanelet2_msgs/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(autoware_lanelet2_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  std_msgs
+)
+
+add_message_files(
+  FILES
+    MapBin.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+    std_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    message_runtime
+    std_msgs
+)

src/ros/catkin/src/autoware_lanelet2_msgs/msg/MapBin.msg

@@ -0,0 +1,13 @@
+# This contains the lanelet map in binary format.
+
+# Timestamp and frame_id of the map
+Header header
+
+# version of map format. keep this as empty string if unnecessary
+string format_version
+
+# version of map version. keep this as empty string if unnecessary
+string map_version
+
+# binary data of lanelet2 map. This is meant to be filled using toBinMsg() in lanelet2_extension library
+int8[] data

src/ros/catkin/src/autoware_lanelet2_msgs/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>autoware_lanelet2_msgs</name>
+  <version>1.13.0</version>
+  <description>The autoware_lanelet2_msgs package. Contains messages for lanelet2 maps</description>
+
+  <maintainer email="ryohsuke.mitsudome@tier4.jp">mitsudome-r</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/lanelet2_extension/CMakeLists.txt

@@ -0,0 +1,131 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(lanelet2_extension)
+
+find_package(PkgConfig)
+find_path(
+  GeographicLib_INCLUDE_DIR GeographicLib/Config.h
+  PATH_SUFFIXES GeographicLib
+)
+set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR})
+
+find_library(GeographicLib_LIBRARIES
+  NAMES Geographic
+)
+
+find_library(PUGIXML_LIBRARIES
+  NAMES pugixml
+)
+
+find_path(PUGIXML_INCLUDE_DIRS
+  NAMES pugixml.hpp
+  PATH_SUFFIXES pugixml
+)
+
+find_package(autoware_build_flags REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  amathutils_lib
+  autoware_lanelet2_msgs
+  autoware_msgs
+  geometry_msgs
+  lanelet2_core
+  lanelet2_io
+  lanelet2_maps
+  lanelet2_projection
+  lanelet2_routing
+  lanelet2_traffic_rules
+  lanelet2_validation
+  roscpp
+  roslint
+  visualization_msgs
+)
+
+set(ROSLINT_CPP_OPTS "--filter=-build/c++14")
+roslint_cpp()
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES lanelet2_extension_lib
+  CATKIN_DEPENDS
+    amathutils_lib
+    autoware_lanelet2_msgs
+    autoware_msgs
+    geometry_msgs
+    lanelet2_core
+    lanelet2_io
+    lanelet2_maps
+    lanelet2_projection
+    lanelet2_routing
+    lanelet2_traffic_rules
+    lanelet2_validation
+    visualization_msgs
+)
+
+include_directories(
+  include
+  ${GeographicLib_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+  ${PUGIXML_INCLUDE_DIRS}
+)
+
+add_definitions(${GeographicLib_DEFINITIONS})
+
+add_library(lanelet2_extension_lib
+  lib/autoware_osm_parser.cpp
+  lib/autoware_traffic_light.cpp
+  lib/message_conversion.cpp
+  lib/mgrs_projector.cpp
+  lib/query.cpp
+  lib/utilities.cpp
+  lib/visualization.cpp
+)
+
+add_dependencies(lanelet2_extension_lib
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(lanelet2_extension_lib
+  ${catkin_LIBRARIES}
+  ${GeographicLib_LIBRARIES}
+)
+
+add_executable(lanelet2_extension_sample src/sample_code.cpp)
+add_dependencies(lanelet2_extension_sample ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(lanelet2_extension_sample
+  ${catkin_LIBRARIES}
+  lanelet2_extension_lib
+)
+
+add_executable(autoware_lanelet2_validation src/validation.cpp)
+add_dependencies(autoware_lanelet2_validation ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(autoware_lanelet2_validation
+  ${catkin_LIBRARIES}
+  ${PUGIXML_LIBRAREIS}
+  lanelet2_extension_lib
+)
+
+install(TARGETS lanelet2_extension_lib lanelet2_extension_sample autoware_lanelet2_validation
+  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}
+)
+
+if(CATKIN_ENABLE_TESTING)
+ roslint_add_test()
+ find_package(rostest REQUIRED)
+ add_rostest_gtest(message_conversion-test test/test_message_conversion.test test/src/test_message_conversion.cpp)
+ target_link_libraries(message_conversion-test ${catkin_LIBRARIES} lanelet2_extension_lib)
+ add_rostest_gtest(projector-test test/test_projector.test test/src/test_projector.cpp)
+ target_link_libraries(projector-test ${catkin_LIBRARIES} lanelet2_extension_lib)
+ add_rostest_gtest(query-test test/test_query.test test/src/test_query.cpp)
+ target_link_libraries(query-test ${catkin_LIBRARIES} lanelet2_extension_lib)
+ add_rostest_gtest(regulatory_elements-test test/test_regulatory_elements.test test/src/test_regulatory_elements.cpp)
+ target_link_libraries(regulatory_elements-test ${catkin_LIBRARIES} lanelet2_extension_lib)
+ add_rostest_gtest(utilities-test test/test_utilities.test test/src/test_utilities.cpp)
+ target_link_libraries(utilities-test ${catkin_LIBRARIES} lanelet2_extension_lib)
+endif()

src/ros/catkin/src/lanelet2_extension/README.md

@@ -0,0 +1,65 @@
+# lanelet2_extension package
+This package contains external library for Lanelet2 and is meant to ease the use of Lanelet2 in Autoware.
+
+## Lanelet Format for Autoware
+Autoware uses extended Lanelet2 Format for Autoware, which means you need to add some tags to default OSM file if you want to fully use Lanelet2 maps. For details about custom tags, please refer to this [document](./docs/lanelet2_format_extension.md).
+
+## Code API
+### IO 
+#### Autoware OSM Parser
+Autoware Lanelet2 Format uses .osm extension as original Lanelet2.
+However, there are some custom tags that is used by the parser.
+
+Currently, this includes:
+* overwriting x,y values with `local_x` and `local_y` tags.
+* reading `<MapMetaInfo>` tag wich contains information about map format version and map version.
+
+The parser is registered as "autoware_osm_handler" as lanelet parser
+
+### Projection
+#### MGRS Projector
+MGRS projector projects latitude longitude into MGRS Coordinates. 
+
+### Regulatory Elements
+#### Autoware Traffic Light
+Autoware Traffic Light class allows you to retrieve information about traffic lights.
+Autoware Traffic Light class contains following members:
+* traffic light shape
+* light bulbs information of traffic lights
+* stopline associated to traffic light
+
+### Utility
+#### Message Conversion
+This contains functions to convert lanelet map objects into ROS messages.
+Currently it contains following conversions:
+* lanelet::LaneletMapPtr to/from lanelet_msgs::MapBinMsg
+* lanelet::Point3d to geometry_msgs::Point
+* lanelet::Point2d to geometry_msgs::Point
+* lanelet::BasicPoint3d to geometry_msgs::Point
+
+#### Query
+This module contains functions to retrieve various information from maps.
+e.g. crosswalks, trafficlights, stoplines
+
+#### Utilties
+This module contains other useful functions related to Lanelet.
+e.g. matching waypoint with lanelets
+
+### Visualization
+Visualization contains functions to convert lanelet objects into visualization marker messages.
+Currenly it contains following conversions:
+* lanelet::Lanelet to Triangle Markers
+* lanelet::LineString to LineStrip Markers
+* TrafficLights to Triangle Markers
+
+## Nodes
+### lanelet2_extension_sample
+Code for this explains how this lanelet2_extension library is used.
+The executable is not meanto to do anything. 
+
+### autoware_lanelet2_extension
+This node checks if an .osm file follows the Autoware version of Lanelet2 format.
+You can check by running:
+```
+rosrun lanelet2_extension autoware_lanelet2_validation _map_file:=<path/to/map.osm>
+```

src/ros/catkin/src/lanelet2_extension/docs/lanelet2_format_extension.md

@@ -0,0 +1,161 @@
+# Modifying Lanelet2 format for Autoware
+## Overview
+About the basics of the default format, please refer to main [Lanelet2 repository](https://github.com/fzi-forschungszentrum-informatik/Lanelet2). (see [here](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/LaneletPrimitives.md) about primitives)
+
+In addition to default Lanelet2 Format, users should add following mandatory/optional tags to their osm lanelet files as explained in reset of this document. 
+Users may use `autoware_lanelet2_validation` [node](../README.md#nodes) to check if their maps are valid. 
+
+## Mandatory Tags
+### Elevation Tags
+Elevation("ele") information for points(`node`) is optional in default Lanelet2 format.
+However, some of Autoware packages(e.g. trafficlight_recognizer) need elevation to be included in HD map. Therefore, users must make sure that all points in their osm maps contain elevation tags. 
+
+Here is an example osm syntax for node object.
+```
+<node id='1' visible='true' version='1' lat='49.00501435943' lon='8.41687458512'>
+  <tag k='ele' v='3.0'/> <!-- this tag is mandatory for Autoware!! --> 
+</node>
+```
+
+### TrafficLights
+Default Lanelet2 format uses LineString(`way`) or Polygon class to represent the shape of a traffic light. For Autoware, traffic light objects must be represented only by LineString to avoid confusion, where start point is at bottom left edge and end point is at bottom right edge. Also, "height" tag must be added in order to represent the size in verticle direction(not the position). 
+
+The Following image illustrates how LineString is used to represent shape of Traffic Light in Autoware.
+<img src="./traffic_light.png" width="600">
+
+
+Here is an example osm syntax for traffic light object.  
+```
+<way id='13' visible='true' version='1'>
+  <nd ref='6' />
+  <nd ref='5' />
+  <tag k='type' v='traffic_light' />
+  <tag k='subtype' v='red_yellow_green' />
+  <tag k='height' v='0.5'/> <!-- this tag is mandatory for Autoware!! --> 
+</way>
+```
+
+### Turn Diretions
+Users must add "turn_direction" tags to lanelets within intersections to indicate vehicle's turning direction. You do not need this tags for lanelets that are not in intersections. If you do not have this tag, Autoware will not be able to light up turning indicators. 
+This tags only take following values:
+* left
+* right
+* straight
+
+Following image illustrates how lanelets should be tagged.
+
+<img src="./turn_direction.png" width="600">
+
+Here is an example of osm sytax for lanelets in intersections. 
+```
+<relation id='1' visible='true' version='1'>
+  <member type='way' ref='2' role='left' />
+  <member type='way' ref='3' role='right' />
+  <member type='relation' ref='4' role='regulatory_element' />
+  <tag k='location' v='urban' />
+  <tag k='one_way' v='yes' />
+  <tag k='subtype' v='road' />
+  <tag k='type' v='lanelet' />
+  <tag k='turn_direction' v='left' /> <!-- this tag is mandatory for lanelets at intersections!! --> 
+</relation>
+```
+
+## Optional Taggings
+Following tags are optional tags that you may want to add depending on how you want to use your map in Autoware. 
+
+### Meta Info
+Users may add the `MetaInfo` element to their OSM file to indicate format version and map version of their OSM file. This information is not meant to influence Autoware vehicle's behavior, but is published as ROS message so that developers could know which map was used from ROSBAG log files. MetaInfo elements exists in the same hiararchy with `node`, `way`, and `relation` elements, otherwise JOSM wouldn't be able to load the file correctly.
+
+Here is an example of MetaInfo in osm file:
+```
+<?xml version='1.0' encoding='UTF-8'?>
+<osm version='0.6' generator='JOSM'>
+  <MetaInfo format_version='1.0' map_version='1.0'/>
+  <node>...</node>
+  <way>...</way>
+  <relation>...</relation>
+</osm>
+``` 
+
+### Local Coordinate Expression
+Sometimes users might want to create Lanelet2 maps that are not georeferenced. 
+In such a case, users may use "local_x", "local_y" taggings to express local positions instead of latitude and longitude. 
+Autoware Osm Parser will overwrite x,y positions with these tags when they are present.
+For z values, use "ele" tags as default Lanelet2 Format. 
+You would still need to fill in lat and lon attributes so that parser does not crush, but their values could be anything. 
+
+Here is example `node` element in osm with "local_x", "local_y" taggings:
+```
+<!-- lat/lon attributes are required, but their values can be anything --> 
+<node id='40648' visible='true' version='1' lat='0' lon='0'>
+  <tag k='local_x' v=2.54'/>
+  <tag k='local_y' v=4.38'/>
+  <tag k='ele' v='3.0'/>
+</node>
+```
+
+### Light Bulbs in Traffic Lights
+Default Lanelet format can only express shape (base + height) of traffic lights. 
+However, region_tlr node in Autoware uses positions of each light bulbs to recognize color of traffic light. If users may wish to use this node, "light_bulbs"(`way`) element must be registered to traffic_light regulatory_element object define position and color of each light bulb in traffic lights. If you are using other trafficlight_recognizer nodes(e.g. tlr_mxnet), which only uses bounding box of traffic light, then you do not need to add this object. 
+
+"light_bulbs" object is defined using LineString(`way`), and each node of line string is placed at the center of each light bulb. Also, each node should have "color" and optionally "arrow" tags to describe its type. Also, "traffic_light_id" tag is used to indicate which ID of relevant traffic_light element.  
+
+"color" tag is used to express the color of the light bulb. Currently only three values are used: 
+* red
+* yellow
+* green
+
+"arrow" tag is used to express the direction of the arrow of light bulb:
+* up
+* right
+* left
+* up_right
+* up_left
+
+Following image illustrates how "light_bulbs" LineString should be created.
+
+<img src="./light_bulbs.png" width="400">
+
+Here is an exmaple of osm syntax for light_bulb object:
+```
+<node id=1 version='1' lat='49.00541994701' lon='8.41565013855'>
+  <tag k='ele' v='5'/>
+  <tag k='color' v='red'/>
+</node>
+<node id=2 version='1' lat='49.00542091657' lon='8.4156469497'>
+  <tag k='ele' v='5'/>
+  <tag k='color' v='yellow'/>
+</node>
+<node id=3 version='1' lat='49.00542180052' lon='8.41564400223'>
+  <tag k='ele' v='5'/>
+  <tag k='color' v='green'/>
+</node>
+<node id=3 version='1' lat='49.00542180052' lon='8.41564400223'>
+  <tag k='ele' v='4.6'/>
+  <tag k='color' v='green'/>
+  <tag k=arrow v='right'/>
+</node>
+<way id=11 version='1'>
+  <nd ref=1 />
+  <nd ref=2 />
+  <nd ref=3 />
+  <tag k='traffic_light_id' v='10'/> <!-- id of linestring with type="traffic_light" --> 
+  <tag k='type' v='light_bulbs' />
+</way>
+```
+
+After creating "light_bulbs" elements, you have to register them to traffic_light regulatory element as role "light_bulbs".
+The following illustrates how light_bulbs are registered to traffic_light regulatory elements. 
+
+<img src="./traffic_light_regulatory_element.png" width="800">
+
+
+```
+<relation id='8' visible='true' version='1'>
+  <tag k='type' v='regulatory_element' />
+  <tag k='subtype' v='traffic_light' />
+  <member type='way' ref='9' role='ref_line' />
+  <member type='way' ref='10' role='refers' /> <!-- refers to the traffic light line string --> 
+  <member type='way' ref='11' role='light_bulbs'/> <!-- refers to the light_bulb line string --> 
+</relation>
+```

src/ros/catkin/src/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.h

@@ -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.
+ *
+ * Authors: Ryohsuke Mitsudome
+ */
+
+#ifndef LANELET2_EXTENSION_IO_AUTOWARE_OSM_PARSER_H
+#define LANELET2_EXTENSION_IO_AUTOWARE_OSM_PARSER_H
+
+#include <lanelet2_io/io_handlers/OsmHandler.h>
+
+#include <string>
+
+namespace lanelet
+{
+namespace io_handlers
+{
+class AutowareOsmParser : public OsmParser
+{
+public:
+  using OsmParser::OsmParser;
+
+  /**
+   * [parse parse osm file to laneletMap. It is generally same as default
+   * OsmParser, but it will overwrite x and y value with local_x and local_y
+   * tags if present]
+   * @param  filename [path to osm file]
+   * @param  errors   [any errors catched during parsing]
+   * @return          [returns LaneletMap]
+   */
+  std::unique_ptr<LaneletMap> parse(const std::string& filename, ErrorMessages& errors) const;  // NOLINT
+
+  /**
+   * [parseVersions parses MetaInfo tags from osm file]
+   * @param filename       [path to osm file]
+   * @param format_version [parsed information about map format version]
+   * @param map_version    [parsed information about map version]
+   */
+  static void parseVersions(const std::string& filename, std::string* format_version, std::string* map_version);
+
+  static constexpr const char* extension()
+  {
+    return ".osm";
+  }
+
+  static constexpr const char* name()
+  {
+    return "autoware_osm_handler";
+  }
+};
+
+}  // namespace io_handlers
+}  // namespace lanelet
+
+#endif  // LANELET2_EXTENSION_IO_AUTOWARE_OSM_PARSER_H

src/ros/catkin/src/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.h

@@ -0,0 +1,121 @@
+/*
+ * 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.
+ *
+ * Authors: Simon Thompson, Ryohsuke Mitsudome
+ */
+
+#ifndef LANELET2_EXTENSION_PROJECTION_MGRS_PROJECTOR_H
+#define LANELET2_EXTENSION_PROJECTION_MGRS_PROJECTOR_H
+
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+#include <lanelet2_io/Exceptions.h>
+#include <lanelet2_io/Projection.h>
+
+#include <string>
+
+namespace lanelet
+{
+namespace projection
+{
+class MGRSProjector : public Projector
+{
+public:
+  explicit MGRSProjector(Origin origin = Origin({ 0.0, 0.0 }));  // NOLINT
+
+  /**
+   * [MGRSProjector::forward projects gps lat/lon to MGRS 100km grid]
+   * @param  gps [point with latitude longitude information]
+   * @return     [projected point in MGRS coordinate]
+   */
+  BasicPoint3d forward(const GPSPoint& gps) const override;
+
+  /**
+   * [MGRSProjector::forward projects gpgs lat/lon to MGRS xyz coordinate]
+   * @param  gps       [point with latitude longitude information]
+   * @param  precision [resolution of MGRS Grid 0=100km, 1=10km, 2=1km, 3=100m,
+   * 4=10m, 5=1m]
+   * @return           [projected point in MGRS coordinate]
+   */
+  BasicPoint3d forward(const GPSPoint& gps, const int precision) const;
+
+  /**
+   * [MGRSProjector::reverse projects point within MGRS 100km grid into gps
+   * lat/lon (WGS84)]
+   * @param  mgrs_point [3d point in MGRS 100km grid]
+   * @return            [projected point in WGS84]
+   */
+  GPSPoint reverse(const BasicPoint3d& mgrs) const override;
+
+  /**
+   * [MGRSProjector::reverse projects point within MGRS grid into gps lat/lon
+   * (WGS84)]
+   * @param  mgrs_point [3d point in MGRS grid]
+   * @param  mgrs_code  [MGRS grid code]
+   * @return            [projected point in WGS84]
+   */
+  GPSPoint reverse(const BasicPoint3d& mgrs_point, const std::string& mgrs_code) const;
+
+  /**
+   * [MGRSProjector::setMGRSCode sets MGRS code used for reverse projection]
+   * @param mgrs_code [MGRS code. Minimum requirement is GZD and 100 km Grid
+   * Square ID. e.g. "4QFJ"]
+   */
+  void setMGRSCode(const std::string& mgrs_code);
+
+  /**
+   * [MGRSProjector::setMGRSCode sets MGRS code used for reverse projection from
+   * gps lat/lon values]
+   * @param gps       [gps point used to find GMRS Grid]
+   * @param precision [resolution of MGRS Grid 0=100km, 1=10km, 2=1km, 3=100m,
+   * 4=10m, 5=1m]
+   */
+  void setMGRSCode(const GPSPoint& gps, const int precision = 0);
+
+  /**
+   * [getProjectedMGRSGrid returns mgrs]
+   * @return [description]
+   */
+  std::string getProjectedMGRSGrid() const
+  {
+    return projected_grid_;
+  };
+
+  /**
+   * [isMGRSCodeSet checks if mgrs code is set for reverse projection]
+   * @return [true if mgrs_code member is set]
+   */
+  bool isMGRSCodeSet() const
+  {
+    return !mgrs_code_.empty();
+  };
+
+private:
+  /**
+   * mgrs grid code used for reverse function
+   */
+  std::string mgrs_code_;
+
+  /**
+   * mgrs grid code that was last projected in previous foward function.
+   * reverse function will use this if isMGRSCodeSet() returns false.
+   */
+  mutable std::string projected_grid_;
+};
+
+}  // namespace projection
+}  // namespace lanelet
+
+#endif  // LANELET2_EXTENSION_PROJECTION_MGRS_PROJECTOR_H

src/ros/catkin/src/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.h

@@ -0,0 +1,94 @@
+/*
+ * 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.
+ *
+ * Authors: Ryohsuke Mitsudome
+ */
+
+#ifndef LANELET2_EXTENSION_REGULATORY_ELEMENTS_AUTOWARE_TRAFFIC_LIGHT_H
+#define LANELET2_EXTENSION_REGULATORY_ELEMENTS_AUTOWARE_TRAFFIC_LIGHT_H
+
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <vector>
+
+namespace lanelet
+{
+namespace autoware
+{
+struct AutowareRoleNameString
+{
+  static constexpr const char LightBulbs[] = "light_bulbs";
+};
+
+class AutowareTrafficLight : public lanelet::TrafficLight
+{
+public:
+  using Ptr = std::shared_ptr<AutowareTrafficLight>;
+  static constexpr char RuleName[] = "traffic_light";
+
+  //! Directly construct a stop line from its required rule parameters.
+  //! Might modify the input data in oder to get correct tags.
+  static Ptr make(Id id, const AttributeMap& attributes, const LineStringsOrPolygons3d& trafficLights,
+                  const Optional<LineString3d>& stopLine = {}, const LineStrings3d& lightBulbs = {})
+  {
+    return Ptr{ new AutowareTrafficLight(id, attributes, trafficLights, stopLine, lightBulbs) };
+  }
+
+  /**
+   * @brief get the relevant traffic light bulbs
+   * @return the traffic light bulbs
+   *
+   * There might be multiple traffic light bulbs but they are required to show
+   * the same signal.
+   */
+  ConstLineStrings3d lightBulbs() const;
+
+  /**
+   * @brief add a new traffic light bulb
+   * @param primitive the traffic light bulb to add
+   *
+   * Traffic light bulbs are represented as linestrings with each point
+   * expressing position of each light bulb (lamp).
+   */
+  void addLightBulbs(const LineStringOrPolygon3d& primitive);
+
+  /**
+   * @brief remove a traffic light bulb
+   * @param primitive the primitive
+   * @return true if the traffic light bulb existed and was removed
+   */
+  bool removeLightBulbs(const LineStringOrPolygon3d& primitive);
+
+private:
+  // the following lines are required so that lanelet2 can create this object
+  // when loading a map with this regulatory element
+  friend class lanelet::RegisterRegulatoryElement<AutowareTrafficLight>;
+  AutowareTrafficLight(Id id, const AttributeMap& attributes, const LineStringsOrPolygons3d& trafficLights,
+                       const Optional<LineString3d>& stopLine, const LineStrings3d& lightBulbs);
+  explicit AutowareTrafficLight(const lanelet::RegulatoryElementDataPtr& data);
+};
+static lanelet::RegisterRegulatoryElement<AutowareTrafficLight> regAutowareTraffic;
+
+// moved to lanelet2_extension/lib/autoware_traffic_light.cpp to avoid multiple
+// defintion errors
+/*
+#if __cplusplus < 201703L
+constexpr char AutowareTrafficLight::RuleName[];      // instanciate string in
+cpp file #endif
+*/
+}  // namespace autoware
+}  // namespace lanelet
+
+#endif  // LANELET2_EXTENSION_REGULATORY_ELEMENTS_AUTOWARE_TRAFFIC_LIGHT_H

src/ros/catkin/src/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.h

@@ -0,0 +1,84 @@
+/*
+ * 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.
+ *
+ * Authors: Simon Thompson, Ryohsuke Mitsudome
+ */
+
+#ifndef LANELET2_EXTENSION_UTILITY_MESSAGE_CONVERSION_H
+#define LANELET2_EXTENSION_UTILITY_MESSAGE_CONVERSION_H
+
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Point32.h>
+#include <geometry_msgs/Polygon.h>
+#include <lanelet2_core/LaneletMap.h>
+#include <autoware_lanelet2_msgs/MapBin.h>
+
+namespace lanelet
+{
+namespace utils
+{
+namespace conversion
+{
+/**
+ * [toBinMsg convervets lanelet2 map to ROS message. Similar implementation to
+ * lanelet::io_handlers::BinHandler::write()]
+ * @param map [lanelet map data]
+ * @param msg [converted ROS message. Only "data" field is filled]
+ */
+void toBinMsg(const lanelet::LaneletMapPtr& map, autoware_lanelet2_msgs::MapBin* msg);
+
+/**
+ * [fromBinMsg converts ROS message into lanelet2 data. Similar implementation
+ * to lanelet::io_handlers::BinHandler::parse()]
+ * @param msg [ROS message for lanelet map]
+ * @param map [Converted lanelet2 data]
+ */
+void fromBinMsg(const autoware_lanelet2_msgs::MapBin& msg, lanelet::LaneletMapPtr map);
+
+/**
+ * [toGeomMsgPt converts various point types to geometry_msgs point]
+ * @param src [input point(geometry_msgs::Point3,
+ * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ]
+ * @param dst [converted geometry_msgs point]
+ */
+void toGeomMsgPt(const geometry_msgs::Point32& src, geometry_msgs::Point* dst);
+void toGeomMsgPt(const Eigen::Vector3d& src, geometry_msgs::Point* dst);
+void toGeomMsgPt(const lanelet::ConstPoint3d& src, geometry_msgs::Point* dst);
+void toGeomMsgPt(const lanelet::ConstPoint2d& src, geometry_msgs::Point* dst);
+
+/**
+ * [toGeomMsgPt converts various point types to geometry_msgs point]
+ * @param src [input point(geometry_msgs::Point3,
+ * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ]
+ * @return    [converted geometry_msgs point]
+ */
+geometry_msgs::Point toGeomMsgPt(const geometry_msgs::Point32& src);
+geometry_msgs::Point toGeomMsgPt(const Eigen::Vector3d& src);
+geometry_msgs::Point toGeomMsgPt(const lanelet::ConstPoint3d& src);
+geometry_msgs::Point toGeomMsgPt(const lanelet::ConstPoint2d& src);
+
+/**
+ * [toGeomMsgPt32 converts Eigen::Vector3d(lanelet:BasicPoint3d to
+ * geometry_msgs::Point32)]
+ * @param src [input point]
+ * @param dst [conveted point]
+ */
+void toGeomMsgPt32(const Eigen::Vector3d& src, geometry_msgs::Point32* dst);
+
+}  // namespace conversion
+}  // namespace utils
+}  // namespace lanelet
+
+#endif  // LANELET2_EXTENSION_UTILITY_MESSAGE_CONVERSION_H

src/ros/catkin/src/lanelet2_extension/include/lanelet2_extension/utility/query.h

@@ -0,0 +1,148 @@
+/*
+ * 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.
+ *
+ * Authors: Simon Thompson, Ryohsuke Mitsudome
+ */
+
+#ifndef LANELET2_EXTENSION_UTILITY_QUERY_H
+#define LANELET2_EXTENSION_UTILITY_QUERY_H
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <geometry_msgs/PolygonStamped.h>
+
+#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.h>
+
+#include <vector>
+#include <string>
+
+namespace lanelet
+{
+using TrafficLightConstPtr = std::shared_ptr<const lanelet::TrafficLight>;
+using AutowareTrafficLightConstPtr = std::shared_ptr<const lanelet::autoware::AutowareTrafficLight>;
+}  // namespace lanelet
+
+namespace lanelet
+{
+namespace utils
+{
+namespace query
+{
+/**
+ * [laneletLayer converts laneletLayer into lanelet vector]
+ * @param  ll_Map [input lanelet map]
+ * @return        [all lanelets in the map]
+ */
+lanelet::ConstLanelets laneletLayer(const lanelet::LaneletMapPtr ll_Map);
+
+/**
+ * [subtypeLanelets extracts Lanelet that has given subtype attribute]
+ * @param  lls     [input lanelets with various subtypes]
+ * @param  subtype [subtype of lanelets to be retrieved (e.g.
+ * lanelet::AttributeValueString::Road)]
+ * @return         [lanelets with given subtype]
+ */
+lanelet::ConstLanelets subtypeLanelets(const lanelet::ConstLanelets lls, const char subtype[]);
+
+/**
+ * [crosswalkLanelets extracts crosswalk lanelets]
+ * @param  lls [input lanelets with various subtypes]
+ * @return     [crosswalk lanelets]
+ */
+lanelet::ConstLanelets crosswalkLanelets(const lanelet::ConstLanelets lls);
+
+/**
+ * [roadLanelets extracts road lanelets]
+ * @param  lls [input lanelets with subtype road]
+ * @return     [road lanelets]
+ */
+lanelet::ConstLanelets roadLanelets(const lanelet::ConstLanelets lls);
+
+/**
+ * [trafficLights extracts Traffic Light regulatory element from lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [traffic light that are associated with input lanenets]
+ */
+std::vector<lanelet::TrafficLightConstPtr> trafficLights(const lanelet::ConstLanelets lanelets);
+
+/**
+ * [autowareTrafficLights extracts Autoware Traffic Light regulatory element
+ * from lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [autoware traffic light that are associated with input
+ * lanenets]
+ */
+std::vector<lanelet::AutowareTrafficLightConstPtr> autowareTrafficLights(const lanelet::ConstLanelets lanelets);
+
+/**
+ * [getTrafficLightStopLines extracts stoplines that are associated with
+ * traffic lights]
+ * @param lanelets [input lanelets]
+ * @return         [stop lines that are associated with input lanelets]
+ */
+std::vector<lanelet::ConstLineString3d> getTrafficLightStopLines(const lanelet::ConstLanelets lanelets);
+
+/**
+ * [getTrafficLightStopLines extracts stoplines that are associated with
+ * traffic lights]
+ * @param ll [input lanelet]
+ * @return   [stop lines that are associated with input lanelet]
+ */
+std::vector<lanelet::ConstLineString3d> getTrafficLightStopLines(const lanelet::ConstLanelet ll);
+
+/**
+ * [getStopSignStopLines extracts stoplines that are associated with any stop
+ * signs, regardless of the type of regulatory element]
+ * @param lanelets     [input lanelets]
+ * @param stop_sign_id [sign id of stop sign]
+ * @return             [array of stoplines]
+ */
+std::vector<lanelet::ConstLineString3d> getStopSignStopLines(const lanelet::ConstLanelets lanelets,
+                                                             const std::string& stop_sign_id = "stop_sign");
+
+/**
+ * [getTrafficSignStopLines extracts stoplines that are associated with
+ * traffic_sign regulatory elements ]
+ * @param lanelets     [input lanelets]
+ * @param stop_sign_id [sign id of stop sign]
+ * @return             [array of stoplines]
+ */
+std::vector<lanelet::ConstLineString3d> getTrafficSignStopLines(const lanelet::ConstLanelets lanelets,
+                                                                const std::string& stop_sign_id = "stop_sign");
+
+/**
+ * [getRightOfWayStopLines extracts stoplines that are associated with
+ * right_of_way regulatory elements ]
+ * @param lanelets     [input lanelets]
+ * @param stop_sign_id [sign id of stop sign]
+ * @return             [array of stoplines]
+ */
+std::vector<lanelet::ConstLineString3d> getRightOfWayStopLines(const lanelet::ConstLanelets lanelets);
+
+/**
+ * [getAllWayStopStopLines extracts stoplines that are associated with
+ * all_way_stop regulatory elements ]
+ * @param lanelets     [input lanelets]
+ * @param stop_sign_id [sign id of stop sign]
+ * @return             [array of stoplines]
+ */
+std::vector<lanelet::ConstLineString3d> getAllWayStopStopLines(const lanelet::ConstLanelets lanelets);
+
+}  // namespace query
+}  // namespace utils
+}  // namespace lanelet
+
+#endif  // LANELET2_EXTENSION_UTILITY_QUERY_H

src/ros/catkin/src/lanelet2_extension/include/lanelet2_extension/utility/utilities.h

@@ -0,0 +1,57 @@
+/*
+ * 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.
+ *
+ * Authors: Kenji Miyake, Ryohsuke Mitsudome
+ */
+
+#ifndef LANELET2_EXTENSION_UTILITY_UTILITIES_H
+#define LANELET2_EXTENSION_UTILITY_UTILITIES_H
+
+#include <geometry_msgs/Point.h>
+
+#include <lanelet2_routing/Route.h>
+#include <lanelet2_routing/RoutingGraph.h>
+
+#include <autoware_msgs/LaneArray.h>
+
+#include <map>
+
+namespace lanelet
+{
+namespace utils
+{
+/**
+ * [matchWaypointAndLanelet Matches waypoints and lanelets]
+ * @param lanelet_map          [pointer to lanelet2 map]
+ * @param routing_graph        [roughting graph of the map]
+ * @param lane_array           [lane array containing waypoints]
+ * @param waypointid2laneletid [object with key:"gid(gobal_id) of waypoints"
+ * value:"lanelet id"]
+ */
+void matchWaypointAndLanelet(const lanelet::LaneletMapPtr lanelet_map,
+                             const lanelet::routing::RoutingGraphPtr routing_graph,
+                             const autoware_msgs::LaneArray& lane_array,
+                             std::map<int, lanelet::Id>* waypointid2laneletid);
+
+/**
+ * @brief  Apply a patch for centerline because the original implementation
+ * doesn't have enough quality
+ */
+void overwriteLaneletsCenterline(lanelet::LaneletMapPtr lanelet_map, const bool force_overite = false);
+
+}  // namespace utils
+}  // namespace lanelet
+
+#endif  // LANELET2_EXTENSION_UTILITY_UTILITIES_H

src/ros/catkin/src/lanelet2_extension/include/lanelet2_extension/visualization/visualization.h

@@ -0,0 +1,155 @@
+/*
+ * 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.
+ *
+ * Authors: Simon Thompson, Ryohsuke Mitsudome
+ */
+
+#ifndef LANELET2_EXTENSION_VISUALIZATION_VISUALIZATION_H
+#define LANELET2_EXTENSION_VISUALIZATION_VISUALIZATION_H
+
+#include <geometry_msgs/PolygonStamped.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.h>
+#include <lanelet2_extension/utility/query.h>
+
+#include <string>
+#include <vector>
+
+namespace lanelet
+{
+namespace visualization
+{
+/**
+ * [polygon2Triangle converts polygon into vector of triangles. Used for
+ * triangulation]
+ * @param polygon        [input polygon]
+ * @param triangles [array of polygon message, each containing 3 vertices]
+ */
+void polygon2Triangle(const geometry_msgs::Polygon& polygon,
+                                     std::vector<geometry_msgs::Polygon>* triangles);
+/**
+ * [lanelet2Triangle converts lanelet into vector of triangles. Used for
+ * triangulation]
+ * @param ll        [input lanelet]
+ * @param triangles [array of polygon message, each containing 3 vertices]
+ */
+void lanelet2Triangle(const lanelet::ConstLanelet& ll, std::vector<geometry_msgs::Polygon>* triangles);
+/**
+ * [lanelet2Polygon converts lanelet into a polygon]
+ * @param ll      [input lanelet]
+ * @param polygon [polygon message containing shape of the input lanelet.]
+ */
+void lanelet2Polygon(const lanelet::ConstLanelet& ll, geometry_msgs::Polygon* polygon);
+
+/**
+ * [lineString2Marker creates marker to visualize shape of linestring]
+ * @param ls         [input linestring]
+ * @param line_strip [output marker message]
+ * @param frame_id   [frame id of the marker]
+ * @param ns         [namespace of the marker]
+ * @param c          [color of the marker]
+ * @param lss        [thickness of the marker]
+ */
+void lineString2Marker(const lanelet::ConstLineString3d ls, visualization_msgs::Marker* line_strip,
+                       const std::string frame_id, const std::string ns, const std_msgs::ColorRGBA c,
+                       const float lss = 0.1);
+/**
+ * [trafficLight2TriangleMarker creates marker to visualize shape of traffic
+ * lights]
+ * @param ls       [linestring that represents traffic light shape]
+ * @param marker   [created marker]
+ * @param ns       [namespace of the marker]
+ * @param cl       [color of the marker]
+ * @param duration [lifetime of the marker]
+ */
+void trafficLight2TriangleMarker(const lanelet::ConstLineString3d ls, visualization_msgs::Marker* marker,
+                                 const std::string ns, const std_msgs::ColorRGBA cl,
+                                 const ros::Duration duration = ros::Duration(), const double scale = 1.0);
+
+/**
+ * [laneletsBoundaryAsMarkerArray create marker array to visualize shape of
+ * boundaries of lanelets]
+ * @param  lanelets       [input lanelets]
+ * @param  c              [color of the boundary]
+ * @param  viz_centerline [flag to visuazlize centerline or not]
+ * @return                [created marker array]
+ */
+visualization_msgs::MarkerArray laneletsBoundaryAsMarkerArray(const lanelet::ConstLanelets& lanelets,
+                                                              const std_msgs::ColorRGBA c, const bool viz_centerline);
+/**
+ * [laneletsAsTriangleMarkerArray create marker array to visualize shape of the
+ * lanelet]
+ * @param  ns       [namespace of the marker]
+ * @param  lanelets [input lanelets]
+ * @param  c        [color of the marker]
+ * @return          [created marker]
+ */
+visualization_msgs::MarkerArray laneletsAsTriangleMarkerArray(const std::string ns,
+                                                              const lanelet::ConstLanelets& lanelets,
+                                                              const std_msgs::ColorRGBA c);
+
+/**
+ * [laneletDirectionAsMarkerArray create marker array to visualize direction of
+ * the lanelet]
+ * @param  lanelets [input lanelets]
+ * @return          [created marker array]
+ */
+visualization_msgs::MarkerArray laneletDirectionAsMarkerArray(const lanelet::ConstLanelets lanelets);
+
+/**
+ * [lineStringsAsMarkerArray creates marker array to visualize shape of
+ * linestrings]
+ * @param  line_strings [input linestrings]
+ * @param  name_space   [namespace of the marker]
+ * @param  c            [color of the marker]
+ * @param  lss          [thickness of the marker]
+ * @return              [created marker array]
+ */
+visualization_msgs::MarkerArray lineStringsAsMarkerArray(const std::vector<lanelet::ConstLineString3d> line_strings,
+                                                         const std::string name_space, const std_msgs::ColorRGBA c,
+                                                         const double lss);
+
+/**
+ * [autowareTrafficLightsAsMarkerArray creates marker array to visualize traffic
+ * lights]
+ * @param  tl_reg_elems [traffic light regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ * @return              [created marker array]
+ */
+visualization_msgs::MarkerArray autowareTrafficLightsAsMarkerArray(
+    const std::vector<lanelet::AutowareTrafficLightConstPtr> tl_reg_elems, const std_msgs::ColorRGBA c,
+    const ros::Duration duration = ros::Duration(), const double scale = 1.0);
+
+/**
+ * [trafficLightsAsTriangleMarkerArray creates marker array to visualize shape
+ * of traffic lights]
+ * @param  tl_reg_elems [traffic light regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ * @return              [created marker array]
+ */
+visualization_msgs::MarkerArray trafficLightsAsTriangleMarkerArray(
+    const std::vector<lanelet::TrafficLightConstPtr> tl_reg_elems, const std_msgs::ColorRGBA c,
+    const ros::Duration duration = ros::Duration(), const double scale = 1.0);
+
+}  // namespace visualization
+}  // namespace lanelet
+
+#endif  // LANELET2_EXTENSION_VISUALIZATION_VISUALIZATION_H

src/ros/catkin/src/lanelet2_extension/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lanelet2_extension</name>
+  <version>1.12.0</version>
+  <description>The lanelet2_extension pacakge contains libraries to handle Lanelet2 format data.</description>
+
+  <maintainer email="ryohsuke.mitsudome@tier4.jp">mitsudome-r</maintainer>
+
+  <license>Apach 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>amathutils_lib</depend>
+  <depend>roscpp</depend>
+  <depend>geographiclib</depend>
+  <depend>lanelet2_core</depend>
+  <depend>lanelet2_io</depend>
+  <depend>lanelet2_maps</depend>
+  <depend>lanelet2_projection</depend>
+  <depend>lanelet2_routing</depend>
+  <depend>lanelet2_traffic_rules</depend>
+  <depend>lanelet2_validation</depend>
+  <depend>autoware_lanelet2_msgs</depend>
+  <depend>autoware_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>pugixml-dev</depend>
+  <depend>roslint</depend>
+</package>

src/ros/catkin/src/lanelet2_extension/src/sample_code.cpp

@@ -0,0 +1,131 @@
+/*
+ * 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 <lanelet2_extension/projection/mgrs_projector.h>
+#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.h>
+
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_io/Io.h>
+#include <lanelet2_projection/UTM.h>
+
+#include <iostream>
+#include <string>
+
+void loadingAutowareOSMFile(const std::string map_file_path)
+{
+  lanelet::LaneletMapPtr lanelet_map;
+  lanelet::ErrorMessages errors;
+  lanelet::GPSPoint gps_point;
+  gps_point.lat = 49.0;
+  gps_point.lon = 8.4;
+  lanelet::Origin origin(gps_point);
+  lanelet::projection::UtmProjector projector(lanelet::Origin(gps_point), true, false);
+
+  // Autoware OSM file parser is registered into lanelet2 library.
+  // Therefore, you can used it by just specifying "autoware_osm_handler" parser
+  // in load function.
+  lanelet_map = lanelet::load(map_file_path, "autoware_osm_handler", projector, &errors);
+
+  // If you want to use default parser, explicitly name the parser
+  lanelet_map = lanelet::load(map_file_path, "osm_handler", projector, &errors);
+}
+
+void usingMGRSProjector()
+{
+  // MGRS Projector projects lat/lon to x,y,z point in MGRS 100km grid.
+  // The origin is automatically calcaulted so you don't have to select any
+  // origin.
+  lanelet::projection::MGRSProjector projector;
+
+  // Let's convert lat/lng point into mgrs xyz point.
+  lanelet::GPSPoint gps_point;
+  gps_point.lat = 49.0;
+  gps_point.lon = 8.4;
+  gps_point.ele = 0.0;
+
+  lanelet::BasicPoint3d mgrs_point = projector.forward(gps_point);
+  std::cout << mgrs_point << std::endl;
+
+  // You can get MGRS Code of projected grid.
+  std::string mgrs_grid = projector.getProjectedMGRSGrid();
+  std::cout << mgrs_grid << std::endl;
+
+  // You can also reverse project from MGRS point to lat/lon.
+  // You have to set which MGRS grid it is from or it will reuse last projected
+  // grid
+  lanelet::GPSPoint projected_gps_point = projector.reverse(mgrs_point);
+  std::cout << projected_gps_point.lat << " " << projected_gps_point.lon << std::endl;
+  lanelet::GPSPoint projected_gps_point2 = projector.reverse(mgrs_point, mgrs_grid);
+  std::cout << projected_gps_point2.lat << " " << projected_gps_point2.lon << " " << std::endl;
+}
+
+void usingAutowareTrafficLight(const std::string map_file_path)
+{
+  lanelet::LaneletMapPtr lanelet_map;
+  lanelet::ErrorMessages errors;
+  lanelet::projection::MGRSProjector projector;
+  lanelet_map = lanelet::load(map_file_path, "autoware_osm_handler", projector, &errors);
+
+  for (auto lanelet : lanelet_map->laneletLayer)
+  {
+    // You can access to traffic light element as AutowareTrafficLight class
+    auto autoware_traffic_lights = lanelet.regulatoryElementsAs<lanelet::autoware::AutowareTrafficLight>();
+    for (auto light : autoware_traffic_lights)
+    {
+      // You can access to the position of each lamps(light bulb) in traffic
+      // light
+      for (auto light_bulb_string : light->lightBulbs())
+      {
+        std::cout << light_bulb_string.id() << std::endl;
+      }
+      // Since AutowareTrafficLight class is inheriting lanelet::TrafficLight
+      // class, you can also acess to outline of traffic light by the same
+      // method.
+      for (auto light_string : light->trafficLights())
+      {
+        std::cout << light_string.id() << std::endl;
+      }
+    }
+
+    // You can also access to same traffic light element as default TrafficLight
+    // class
+    auto traffic_lights = lanelet.regulatoryElementsAs<lanelet::TrafficLight>();
+    for (auto light : traffic_lights)
+    {
+      for (auto light_string : light->trafficLights())
+      {
+        std::cout << light_string.id() << std::endl;
+      }
+    }
+  }
+}
+
+int main(int argc, char* argv[])
+{
+  ros::init(argc, argv, "lanelet2_extension_example");
+  ros::NodeHandle nh;
+  ros::NodeHandle pnh("~");
+
+  std::string map_file_path;
+  pnh.param("map_file", map_file_path, map_file_path);
+
+  loadingAutowareOSMFile(map_file_path);
+  usingMGRSProjector();
+  usingAutowareTrafficLight(map_file_path);
+  return 0;
+}

src/ros/catkin/src/lanelet2_extension/src/validation.cpp

@@ -0,0 +1,185 @@
+/*
+ * 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 <lanelet2_core/primitives/BasicRegulatoryElements.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_io/Io.h>
+#include <lanelet2_io/io_handlers/Factory.h>
+#include <lanelet2_io/io_handlers/Writer.h>
+#include <lanelet2_projection/UTM.h>
+#include <lanelet2_routing/RoutingGraph.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#include <lanelet2_extension/projection/mgrs_projector.h>
+#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.h>
+
+#include <iostream>
+#include <string>
+
+#include <ros/ros.h>
+
+#include <pugixml.hpp>
+
+namespace
+{
+namespace keyword
+{
+constexpr const char* Id = "id";
+constexpr const char* Osm = "osm";
+constexpr const char* Tag = "tag";
+constexpr const char* Key = "k";
+constexpr const char* Node = "node";
+constexpr const char* Elevation = "ele";
+}  // namespace keyword
+
+void printUsage()
+{
+  std::cout << "Usage:" << std::endl
+            << "rosrun lanelet2_extension autoware_lanelet2_validation"
+               "_map_file:=<path to osm file>"
+            << std::endl;
+}
+}  // namespace
+
+void validateElevationTag(const std::string filename)
+{
+  pugi::xml_document doc;
+  auto result = doc.load_file(filename.c_str());
+  if (!result)
+  {
+    ROS_FATAL_STREAM(result.description());
+    exit(1);
+  }
+
+  auto osmNode = doc.child("osm");
+  for (auto node = osmNode.child(keyword::Node); node;  // NOLINT
+       node = node.next_sibling(keyword::Node))
+  {
+    const auto id = node.attribute(keyword::Id).as_llong(lanelet::InvalId);
+    if (!node.find_child_by_attribute(keyword::Tag, keyword::Key, keyword::Elevation))
+    {
+      ROS_ERROR_STREAM("failed to find elevation tag for node: " << id);
+    }
+  }
+}
+
+void validateTrafficLight(const lanelet::LaneletMapPtr lanelet_map)
+{
+  if (!lanelet_map)
+  {
+    ROS_FATAL_STREAM("Missing map. Are you sure you set correct path for map?");
+    exit(1);
+  }
+
+  for (auto lanelet : lanelet_map->laneletLayer)
+  {
+    auto autoware_traffic_lights = lanelet.regulatoryElementsAs<lanelet::autoware::AutowareTrafficLight>();
+    if (autoware_traffic_lights.empty())
+      continue;
+    for (auto light : autoware_traffic_lights)
+    {
+      if (light->lightBulbs().size() == 0)
+      {
+        ROS_WARN_STREAM("regulatory element traffic light " << light->id()
+                                                            << " is missing optional light_bulb member. You won't "
+                                                               "be able to use region_tlr node with this map");
+      }
+      for (auto light_string : light->lightBulbs())
+      {
+        if (!light_string.hasAttribute("traffic_light_id"))
+        {
+          ROS_ERROR_STREAM("light_bulb " << light_string.id() << " is missing traffic_light_id tag");
+        }
+      }
+      for (auto base_string_or_poly : light->trafficLights())
+      {
+        if (!base_string_or_poly.isLineString())
+        {
+          ROS_ERROR_STREAM("traffic_light " << base_string_or_poly.id()
+                                            << " is polygon, and only linestring class is currently supported for "
+                                               "traffic lights");
+        }
+        auto base_string = static_cast<lanelet::LineString3d>(base_string_or_poly);
+        if (!base_string.hasAttribute("height"))
+        {
+          ROS_ERROR_STREAM("traffic_light " << base_string.id() << " is missing height tag");
+        }
+      }
+    }
+  }
+}
+
+void validateTurnDirection(const lanelet::LaneletMapPtr lanelet_map)
+{
+  if (!lanelet_map)
+  {
+    ROS_FATAL_STREAM("Missing map. Are you sure you set correct path for map?");
+    exit(1);
+  }
+
+  lanelet::traffic_rules::TrafficRulesPtr traffic_rules =
+      lanelet::traffic_rules::TrafficRulesFactory::create(lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+  lanelet::routing::RoutingGraphPtr vehicle_graph = lanelet::routing::RoutingGraph::build(*lanelet_map, *traffic_rules);
+
+  for (const auto& lanelet : lanelet_map->laneletLayer)
+  {
+    if (!traffic_rules->canPass(lanelet))
+    {
+      continue;
+    }
+
+    const auto conflicting_lanelets_or_areas = vehicle_graph->conflicting(lanelet);
+    if (conflicting_lanelets_or_areas.size() == 0)
+      continue;
+    if (!lanelet.hasAttribute("turn_direction"))
+    {
+      ROS_ERROR_STREAM("lanelet " << lanelet.id() << " seems to be intersecting other lanelet, but does "
+                                                     "not have turn_direction tagging.");
+    }
+  }
+}
+
+int main(int argc, char* argv[])
+{
+  ros::init(argc, argv, "autoware_lanelet_valdiation");
+  ros::NodeHandle node;
+  ros::NodeHandle private_rosnode("~");
+
+  if (!private_rosnode.hasParam("map_file"))
+  {
+    ROS_FATAL_STREAM("failed find map_file parameter! No file to load");
+    printUsage();
+    return 1;
+  }
+
+  std::string map_path = "";
+  private_rosnode.getParam("map_file", map_path);
+
+  lanelet::LaneletMapPtr lanelet_map;
+  lanelet::ErrorMessages errors;
+  lanelet::projection::MGRSProjector projector;
+  lanelet_map = lanelet::load(map_path, "autoware_osm_handler", projector, &errors);
+
+  std::cout << "starting validation" << std::endl;
+
+  validateElevationTag(map_path);
+  validateTrafficLight(lanelet_map);
+  validateTurnDirection(lanelet_map);
+
+  std::cout << "finished validation" << std::endl;
+
+  return 0;
+}

src/ros/catkin/src/lanelet2_extension/test/src/test_message_conversion.cpp

@@ -0,0 +1,140 @@
+/*
+ * 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 <math.h>
+#include <ros/ros.h>
+
+#include <gtest/gtest.h>
+
+#include <lanelet2_extension/utility/message_conversion.h>
+#include <lanelet2_extension/utility/query.h>
+
+using lanelet::Lanelet;
+using lanelet::LineString3d;
+using lanelet::Point3d;
+using lanelet::utils::getId;
+using lanelet::utils::conversion::toGeomMsgPt;
+
+class TestSuite : public ::testing::Test
+{
+public:
+  TestSuite() : single_lanelet_map_ptr(new lanelet::LaneletMap())
+  {
+    Point3d p1, p2, p3, p4, p5, p6, p7;
+    LineString3d traffic_light_base, traffic_light_bulbs, stop_line;
+
+    p1 = Point3d(getId(), 0., 0., 0.);
+    p2 = Point3d(getId(), 0., 1., 0.);
+
+    p3 = Point3d(getId(), 1., 0., 0.);
+    p4 = Point3d(getId(), 1., 1., 0.);
+
+    LineString3d ls_left(getId(), { p1, p2 });   // NOLINT
+    LineString3d ls_right(getId(), { p3, p4 });  // NOLINT
+
+    Lanelet lanelet(getId(), ls_left, ls_right);
+
+    single_lanelet_map_ptr->add(lanelet);
+  }
+  ~TestSuite()
+  {
+  }
+  lanelet::LaneletMapPtr single_lanelet_map_ptr;
+
+private:
+};
+
+TEST_F(TestSuite, BinMsgConversion)
+{
+  autoware_lanelet2_msgs::MapBin bin_msg;
+  lanelet::LaneletMapPtr regenerated_map(new lanelet::LaneletMap);
+
+  lanelet::utils::conversion::toBinMsg(single_lanelet_map_ptr, &bin_msg);
+
+  ASSERT_NE(0, bin_msg.data.size()) << "converted bin message does not have any data";
+
+  lanelet::utils::conversion::fromBinMsg(bin_msg, regenerated_map);
+
+  auto original_lanelet = lanelet::utils::query::laneletLayer(single_lanelet_map_ptr);
+  auto regenerated_lanelet = lanelet::utils::query::laneletLayer(regenerated_map);
+
+  ASSERT_EQ(original_lanelet.front().id(), regenerated_lanelet.front().id()) << "regerated map has different id";
+}
+
+TEST_F(TestSuite, ToGeomMsgPt)
+{
+  Point3d lanelet_pt(getId(), -0.1, 0.2, 3.0);
+
+  geometry_msgs::Point32 geom_pt32;
+  geom_pt32.x = -0.1;
+  geom_pt32.y = 0.2;
+  geom_pt32.z = 3.0;
+
+  geometry_msgs::Point geom_pt;
+  toGeomMsgPt(geom_pt32, &geom_pt);
+  ASSERT_FLOAT_EQ(geom_pt32.x, geom_pt.x) << " converted value is different from original geometry_msgs::Point";
+  ASSERT_FLOAT_EQ(geom_pt32.y, geom_pt.y) << " converted value is different from original geometry_msgs::Point";
+  ASSERT_FLOAT_EQ(geom_pt32.z, geom_pt.z) << " converted value is different from original geometry_msgs::Point";
+
+  geom_pt = toGeomMsgPt(geom_pt32);
+  ASSERT_FLOAT_EQ(geom_pt32.x, geom_pt.x) << " converted value is different from original geometry_msgs::Point";
+  ASSERT_FLOAT_EQ(geom_pt32.y, geom_pt.y) << " converted value is different from original geometry_msgs::Point";
+  ASSERT_FLOAT_EQ(geom_pt32.z, geom_pt.z) << " converted value is different from original geometry_msgs::Point";
+
+  toGeomMsgPt(lanelet_pt.basicPoint(), &geom_pt);
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().x(), geom_pt.x) << " converted value is different from original "
+                                                              "lanelet::basicPoint";
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().y(), geom_pt.y) << " converted value is different from original "
+                                                              "lanelet::basicPoint";
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().z(), geom_pt.z) << " converted value is different from original "
+                                                              "lanelet::basicPoint";
+
+  geom_pt = toGeomMsgPt(lanelet_pt.basicPoint());
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().x(), geom_pt.x) << " converted value is different from original "
+                                                              "lanelet::basicPoint";
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().y(), geom_pt.y) << " converted value is different from original "
+                                                              "lanelet::basicPoint";
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().z(), geom_pt.z) << " converted value is different from original "
+                                                              "lanelet::basicPoint";
+
+  toGeomMsgPt(lanelet_pt, &geom_pt);
+  ASSERT_DOUBLE_EQ(lanelet_pt.x(), geom_pt.x) << " converted value is different from original lanelet::Point3d";
+  ASSERT_DOUBLE_EQ(lanelet_pt.y(), geom_pt.y) << " converted value is different from original lanelet::Point3d";
+  ASSERT_DOUBLE_EQ(lanelet_pt.z(), geom_pt.z) << " converted value is different from original lanelet::Point3d";
+
+  geom_pt = toGeomMsgPt(lanelet_pt);
+  ASSERT_DOUBLE_EQ(lanelet_pt.x(), geom_pt.x) << " converted value is different from original lanelet::Point3d";
+  ASSERT_DOUBLE_EQ(lanelet_pt.y(), geom_pt.y) << " converted value is different from original lanelet::Point3d";
+  ASSERT_DOUBLE_EQ(lanelet_pt.z(), geom_pt.z) << " converted value is different from original lanelet::Point3d";
+
+  lanelet::ConstPoint2d point_2d = lanelet::utils::to2D(lanelet_pt);
+
+  toGeomMsgPt(point_2d, &geom_pt);
+  ASSERT_DOUBLE_EQ(point_2d.x(), geom_pt.x) << " converted value is different from original lanelet::Point2d";
+  ASSERT_DOUBLE_EQ(point_2d.y(), geom_pt.y) << " converted value is different from original lanelet::Point2d";
+  ASSERT_DOUBLE_EQ(0.0, geom_pt.z) << " converted value is different from original lanelet::Point2d";
+
+  geom_pt = toGeomMsgPt(point_2d);
+  ASSERT_DOUBLE_EQ(point_2d.x(), geom_pt.x) << " converted value is different from original lanelet::Point2d";
+  ASSERT_DOUBLE_EQ(point_2d.y(), geom_pt.y) << " converted value is different from original lanelet::Point2d";
+  ASSERT_DOUBLE_EQ(0.0, geom_pt.z) << " converted value is different from original lanelet::Point2d";
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "TestNode");
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/lanelet2_extension/test/src/test_projector.cpp

@@ -0,0 +1,87 @@
+/*
+ * 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 <math.h>
+
+#include <lanelet2_extension/projection/mgrs_projector.h>
+
+class TestSuite : public ::testing::Test
+{
+public:
+  TestSuite()
+  {
+  }
+  ~TestSuite()
+  {
+  }
+};
+
+TEST(TestSuite, ForwardProjection)
+{
+  lanelet::projection::MGRSProjector projector;
+  // lat/lon in Tokyo
+  lanelet::GPSPoint gps_point;
+  gps_point.lat = 35.652832;
+  gps_point.lon = 139.839478;
+  gps_point.ele = 12.3456789;
+  lanelet::BasicPoint3d mgrs_point = projector.forward(gps_point);
+
+  // projected z value should not change
+  ASSERT_DOUBLE_EQ(mgrs_point.z(), gps_point.ele) << "Forward projected z value should be " << gps_point.ele;
+
+  // https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html
+  // round the projected value to mm since the above reference only gives value
+  // in mm precision
+  ASSERT_EQ(projector.getProjectedMGRSGrid(), "54SUE") << "Projected grid should be "
+                                                       << "54SUE";
+  double rounded_x_mm = round(mgrs_point.x() * 1000) / 1000.0;
+  ASSERT_DOUBLE_EQ(rounded_x_mm, 94946.081) << "Forward projected x value should be " << 94946.081;
+  double rounded_y_mm = round(mgrs_point.y() * 1000) / 1000.0;
+  ASSERT_DOUBLE_EQ(rounded_y_mm, 46063.748) << "Forward projected y value should be " << 46063.748;
+}
+
+TEST(TestSuite, ReverseProjection)
+{
+  lanelet::projection::MGRSProjector projector;
+  lanelet::BasicPoint3d mgrs_point;
+  mgrs_point.x() = 94946.0;
+  mgrs_point.y() = 46063.0;
+  mgrs_point.z() = 12.3456789;
+
+  projector.setMGRSCode("54SUE");
+  lanelet::GPSPoint gps_point = projector.reverse(mgrs_point);
+
+  // projected z value should not change
+  ASSERT_DOUBLE_EQ(gps_point.ele, mgrs_point.z()) << "Reverse projected z value should be " << mgrs_point.z();
+
+  // https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html
+  // round the projected value since the above reference only gives value up to
+  // precision of 1e-8
+  double rounded_lat = round(gps_point.lat * 1e8) / 1e8;
+  ASSERT_DOUBLE_EQ(rounded_lat, 35.65282525) << "Reverse projected latitude value should be " << 35.65282525;
+  double rounded_lon = round(gps_point.lon * 1e8) / 1e8;
+  ASSERT_DOUBLE_EQ(rounded_lon, 139.83947721) << "Reverse projected longitude value should be " << 139.83947721;
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "TestNode");
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/lanelet2_extension/test/src/test_query.cpp

@@ -0,0 +1,132 @@
+/*
+ * 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 <gtest/gtest.h>
+#include <lanelet2_extension/utility/query.h>
+#include <math.h>
+#include <ros/ros.h>
+
+using lanelet::Lanelet;
+using lanelet::LineString3d;
+using lanelet::LineStringOrPolygon3d;
+using lanelet::Point3d;
+using lanelet::Points3d;
+using lanelet::utils::getId;
+
+class TestSuite : public ::testing::Test
+{
+public:
+  TestSuite() : sample_map_ptr(new lanelet::LaneletMap())
+  {  // NOLINT
+    // create sample lanelets
+    Point3d p1, p2, p3, p4;
+
+    p1 = Point3d(getId(), 0., 0., 0.);
+    p2 = Point3d(getId(), 0., 1., 0.);
+
+    p3 = Point3d(getId(), 1., 0., 0.);
+    p4 = Point3d(getId(), 1., 1., 0.);
+
+    LineString3d ls_left(getId(), { p1, p2 });   // NOLINT
+    LineString3d ls_right(getId(), { p3, p4 });  // NOLINT
+
+    Lanelet road_lanelet(getId(), ls_left, ls_right);
+    road_lanelet.attributes()[lanelet::AttributeName::Subtype] = lanelet::AttributeValueString::Road;
+
+    Lanelet crosswalk_lanelet(getId(), ls_left, ls_right);
+    crosswalk_lanelet.attributes()[lanelet::AttributeName::Subtype] = lanelet::AttributeValueString::Crosswalk;
+
+    // create sample traffic light
+    Point3d p5, p6, p7, p8, p9, p10, p11, p12;
+    LineString3d traffic_light_base, traffic_light_bulbs, stop_line;
+
+    p6 = Point3d(getId(), 0., 1., 4.);
+    p7 = Point3d(getId(), 1., 1., 4.);
+
+    p8 = Point3d(getId(), 0., 1., 4.5);
+    p9 = Point3d(getId(), 0.5, 1., 4.5);
+    p10 = Point3d(getId(), 1., 1., 4.5);
+
+    p11 = Point3d(getId(), 0., 0., 0.);
+    p12 = Point3d(getId(), 1., 0., 0.);
+
+    traffic_light_base = LineString3d(getId(), Points3d{ p6, p7 });        // NOLINT
+    traffic_light_bulbs = LineString3d(getId(), Points3d{ p8, p9, p10 });  // NOLINT
+    stop_line = LineString3d(getId(), Points3d{ p11, p12 });               // NOLINT
+
+    auto tl = lanelet::autoware::AutowareTrafficLight::make(getId(), lanelet::AttributeMap(), { traffic_light_base },
+                                                            stop_line, { traffic_light_bulbs });  // NOLINT
+
+    road_lanelet.addRegulatoryElement(tl);
+
+    // add items to map
+    sample_map_ptr->add(road_lanelet);
+    sample_map_ptr->add(crosswalk_lanelet);
+  }
+  ~TestSuite()
+  {
+  }
+
+  lanelet::LaneletMapPtr sample_map_ptr;
+
+private:
+};
+
+TEST_F(TestSuite, QueryLanelets)
+{
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr);
+  ASSERT_EQ(2, all_lanelets.size()) << "failed to retrieve all lanelets";
+
+  lanelet::ConstLanelets subtype_lanelets =
+      lanelet::utils::query::subtypeLanelets(all_lanelets, lanelet::AttributeValueString::Road);
+  ASSERT_EQ(1, subtype_lanelets.size()) << "failed to retrieve road lanelet by subtypeLanelets";
+
+  lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets);
+  ASSERT_EQ(1, road_lanelets.size()) << "failed to retrieve road lanelets";
+
+  lanelet::ConstLanelets crosswalk_lanelets = lanelet::utils::query::crosswalkLanelets(all_lanelets);
+  ASSERT_EQ(1, crosswalk_lanelets.size()) << "failed to retrieve crosswalk lanelets";
+}
+
+TEST_F(TestSuite, QueryTrafficLights)
+{
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr);
+
+  auto traffic_lights = lanelet::utils::query::trafficLights(all_lanelets);
+  ASSERT_EQ(1, traffic_lights.size()) << "failed to retrieve traffic lights";
+
+  auto autoware_traffic_lights = lanelet::utils::query::autowareTrafficLights(all_lanelets);
+  ASSERT_EQ(1, autoware_traffic_lights.size()) << "failed to retrieve autoware traffic lights";
+}
+
+TEST_F(TestSuite, QueryStopLine)
+{
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr);
+  lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets);
+
+  auto stop_lines = lanelet::utils::query::getTrafficLightStopLines(all_lanelets);
+  ASSERT_EQ(1, stop_lines.size()) << "failed to retrieve stop lines from all lanelets";
+
+  auto stop_lines2 = lanelet::utils::query::getTrafficLightStopLines(road_lanelets.front());
+  ASSERT_EQ(1, stop_lines2.size()) << "failed to retrieve stop lines from a lanelet";
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "TestNode");
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/lanelet2_extension/test/src/test_regulatory_elements.cpp

@@ -0,0 +1,133 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+
+#include <lanelet2_core/Attribute.h>
+#include <lanelet2_core/LaneletMap.h>
+
+#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.h>
+
+#include <math.h>
+#include <vector>
+
+using lanelet::LineString3d;
+using lanelet::LineStringOrPolygon3d;
+using lanelet::Point3d;
+using lanelet::Points3d;
+using lanelet::utils::getId;
+
+namespace
+{
+template <typename T>
+std::vector<T> convertToVector(T item)
+{
+  std::vector<T> vector = { item };
+  return vector;
+}
+}  // namespace
+
+class TestSuite : public ::testing::Test
+{
+public:
+  TestSuite()
+  {
+  }
+  ~TestSuite()
+  {
+  }
+};
+
+TEST(TestSuite, FactoryConstructsTrafficLight)
+{
+  Point3d p1, p2, p3, p4, p5, p6, p7;
+  LineStringOrPolygon3d traffic_light_base;
+  LineString3d traffic_light_bulbs, stop_line;
+
+  p1 = Point3d(getId(), 0., 1., 4.);
+  p2 = Point3d(getId(), 1., 1., 4.);
+
+  p3 = Point3d(getId(), 0., 1., 4.5);
+  p4 = Point3d(getId(), 0.5, 1., 4.5);
+  p5 = Point3d(getId(), 1., 1., 4.5);
+
+  p6 = Point3d(getId(), 0., 0., 0.);
+  p7 = Point3d(getId(), 1., 0., 0.);
+
+  Points3d base = { p1, p2 };
+  Points3d bulbs = { p3, p4, p5 };
+  Points3d stop = { p6, p7 };
+
+  traffic_light_base = LineString3d(getId(), base);
+  traffic_light_bulbs = LineString3d(getId(), bulbs);
+  stop_line = LineString3d(getId(), stop);
+
+  auto tl = lanelet::autoware::AutowareTrafficLight::make(getId(), lanelet::AttributeMap(),
+                                                          convertToVector(traffic_light_base), stop_line,
+                                                          convertToVector(traffic_light_bulbs));
+
+  auto factoryTl = lanelet::RegulatoryElementFactory::create(
+      tl->attribute(lanelet::AttributeName::Subtype).value(),
+      std::const_pointer_cast<lanelet::RegulatoryElementData>(tl->constData()));
+  EXPECT_TRUE(!!std::dynamic_pointer_cast<lanelet::TrafficLight>(factoryTl));
+}
+
+TEST(TestSuite, TrafficLightWorksAsExpected)
+{  // NOLINT
+  Point3d p1, p2, p3, p4, p5, p6, p7;
+  LineStringOrPolygon3d traffic_light_base, traffic_light_base2;
+  LineString3d traffic_light_bulbs, traffic_light_bulbs2, stop_line;
+
+  p1 = Point3d(getId(), 0., 1., 4.);
+  p2 = Point3d(getId(), 1., 1., 4.);
+
+  p3 = Point3d(getId(), 0., 1., 4.5);
+  p4 = Point3d(getId(), 0.5, 1., 4.5);
+  p5 = Point3d(getId(), 1., 1., 4.5);
+
+  p6 = Point3d(getId(), 0., 0., 0.);
+  p7 = Point3d(getId(), 1., 0., 0.);
+
+  Points3d base = { p1, p2 };
+  Points3d bulbs = { p3, p4, p5 };
+  Points3d stop = { p6, p7 };
+
+  traffic_light_base = { LineString3d(getId(), base) };
+  traffic_light_base2 = { LineString3d(getId(), base) };
+  traffic_light_bulbs = { LineString3d(getId(), bulbs) };
+  traffic_light_bulbs2 = { LineString3d(getId(), bulbs) };
+  stop_line = LineString3d(getId(), stop);
+
+  auto tl = lanelet::autoware::AutowareTrafficLight::make(getId(), lanelet::AttributeMap(),
+                                                          convertToVector(traffic_light_base), stop_line,
+                                                          convertToVector(traffic_light_bulbs));
+  tl->setStopLine(stop_line);
+  EXPECT_EQ(stop_line, tl->stopLine());
+  tl->addTrafficLight(traffic_light_base2);
+  EXPECT_EQ(2ul, tl->trafficLights().size());
+  tl->addLightBulbs(traffic_light_bulbs2);
+  EXPECT_EQ(2ul, tl->lightBulbs().size());
+  tl->removeLightBulbs(traffic_light_bulbs);
+  EXPECT_EQ(1ul, tl->lightBulbs().size());
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "TestNode");
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/lanelet2_extension/test/src/test_utilities.cpp

@@ -0,0 +1,145 @@
+/*
+ * 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 <gtest/gtest.h>
+#include <lanelet2_extension/utility/utilities.h>
+#include <lanelet2_routing/RoutingGraphContainer.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+#include <map>
+#include <ros/ros.h>
+
+using lanelet::Lanelet;
+using lanelet::LineString3d;
+using lanelet::Point3d;
+using lanelet::utils::getId;
+
+class TestSuite : public ::testing::Test
+{
+public:
+  TestSuite() : sample_map_ptr(new lanelet::LaneletMap())
+  {  // NOLINT
+    // create sample lanelets
+    Point3d p1, p2, p3, p4, p5, p6, p7, p8, p9, p10;
+
+    p1 = Point3d(getId(), 0., 0., 0.);
+    p2 = Point3d(getId(), 0., 1., 0.);
+    p3 = Point3d(getId(), 1., 0., 0.);
+    p4 = Point3d(getId(), 1., 1., 0.);
+
+    LineString3d ls_left(getId(), { p1, p2 });   // NOLINT
+    LineString3d ls_right(getId(), { p3, p4 });  // NOLINT
+
+    p5 = Point3d(getId(), 0., 2., 0.);
+    p6 = Point3d(getId(), 1., 2., 0.);
+
+    LineString3d ls_left2(getId(), { p2, p5 });   // NOLINT
+    LineString3d ls_right2(getId(), { p4, p6 });  // NOLINT
+
+    p7 = Point3d(getId(), 0., 3., 0.);
+    p8 = Point3d(getId(), 1., 3., 0.);
+
+    LineString3d ls_left3(getId(), { p5, p7 });   // NOLINT
+    LineString3d ls_right3(getId(), { p6, p8 });  // NOLINT
+
+    p9 = Point3d(getId(), 0., 1., 0.);
+    p10 = Point3d(getId(), 1., 1., 0.);
+
+    LineString3d ls_left4(getId(), { p9, p5 });   // NOLINT
+    LineString3d ls_right4(getId(), { p10, p6 });  // NOLINT
+
+    road_lanelet = Lanelet(getId(), ls_left, ls_right);
+    road_lanelet.attributes()[lanelet::AttributeName::Subtype] = lanelet::AttributeValueString::Road;
+
+    next_lanelet = Lanelet(getId(), ls_left2, ls_right2);
+    next_lanelet.attributes()[lanelet::AttributeName::Subtype] = lanelet::AttributeValueString::Road;
+
+    next_lanelet2 = Lanelet(getId(), ls_left3, ls_right3);
+    next_lanelet2.attributes()[lanelet::AttributeName::Subtype] = lanelet::AttributeValueString::Road;
+
+    merging_lanelet = Lanelet(getId(), ls_left4, ls_right4);
+    merging_lanelet.attributes()[lanelet::AttributeName::Subtype] = lanelet::AttributeValueString::Road;
+
+    sample_map_ptr->add(road_lanelet);
+    sample_map_ptr->add(next_lanelet);
+    sample_map_ptr->add(next_lanelet2);
+    sample_map_ptr->add(merging_lanelet);
+  }
+  ~TestSuite()
+  {
+  }
+
+  lanelet::LaneletMapPtr sample_map_ptr;
+  Lanelet road_lanelet;
+  Lanelet next_lanelet;
+  Lanelet next_lanelet2;
+  Lanelet merging_lanelet;
+
+private:
+};
+
+TEST_F(TestSuite, MatchWaypointAndLanelet)
+{
+  std::map<int, lanelet::Id> waypointid2laneletid;
+  autoware_msgs::LaneArray lane_array;
+  autoware_msgs::Lane lane;
+  autoware_msgs::Waypoint waypoint;
+
+  // waypoints that overlap with road_lanelet
+  for (int i = 1; i < 4; i++)
+  {
+    waypoint.gid = i;
+    waypoint.pose.pose.position.x = 0.5;
+    waypoint.pose.pose.position.y = i - 0.5;
+    lane.waypoints.push_back(waypoint);
+  }
+
+  // waypoint that overlaps with no lanelet
+  waypoint.gid = 4;
+  waypoint.pose.pose.position.x = 1.5;
+  waypoint.pose.pose.position.y = 1.5;
+  lane.waypoints.push_back(waypoint);
+
+  lane_array.lanes.push_back(lane);
+
+  lanelet::traffic_rules::TrafficRulesPtr traffic_rules =
+      lanelet::traffic_rules::TrafficRulesFactory::create(lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+  lanelet::routing::RoutingGraphPtr routing_graph =
+      lanelet::routing::RoutingGraph::build(*sample_map_ptr, *traffic_rules);
+
+  lanelet::utils::matchWaypointAndLanelet(sample_map_ptr, routing_graph, lane_array, &waypointid2laneletid);
+
+  ASSERT_EQ(3, waypointid2laneletid.size()) << "failed to match waypoints with lanelets";
+  ASSERT_EQ(road_lanelet.id(), waypointid2laneletid.at(1)) << "failed to match waypoints with lanelet";
+  ASSERT_EQ(next_lanelet.id(), waypointid2laneletid.at(2)) << "failed to match waypoints with lanelet";
+  ASSERT_EQ(next_lanelet2.id(), waypointid2laneletid.at(3)) << "failed to match waypoints with lanelet";
+}
+
+TEST_F(TestSuite, OverwriteLaneletsCenterline)
+{
+  lanelet::utils::overwriteLaneletsCenterline(sample_map_ptr);
+
+  for (const auto& lanelet : sample_map_ptr->laneletLayer)
+  {
+    ASSERT_TRUE(lanelet.hasCustomCenterline()) << "failed to calculate fine centerline";
+  }
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "TestNode");
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/lanelet2_extension/test/test_message_conversion.test

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

src/ros/catkin/src/lanelet2_extension/test/test_projector.test

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

src/ros/catkin/src/lanelet2_extension/test/test_query.test

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

src/ros/catkin/src/lanelet2_extension/test/test_regulatory_elements.test

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

src/ros/catkin/src/lanelet2_extension/test/test_utilities.test

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

src/ros/catkin/src/pilottoros/src/main.cpp

@@ -82,8 +82,10 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
 	qint64 timestamp;
 	    memcpy(&timestamp,(char *)strdata+4+nNameSize+4,8);
 		double ftimestamp = timestamp;
-		ftimestamp = ftimestamp/1000.0;
-		point_cloud->header.stamp = ftimestamp;
+		ftimestamp = ftimestamp*1000.0;
+        point_cloud->header.stamp  = ftimestamp;
+		point_cloud->header.stamp =  std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();  
+         
    // memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
     int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
     int i;

src/ros/catkin/src/points_preprocessor/CHANGELOG.rst

@@ -0,0 +1,365 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package points_preprocessor
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* Revert "Fix/health checker (`#2012 <https://github.com/CPFL/Autoware/issues/2012>`_)" (`#2037 <https://github.com/CPFL/Autoware/issues/2037>`_)
+  This reverts commit e4187a7138eb90ad6f119eb35f824b16465aefda.
+  Reverts `#2012 <https://github.com/CPFL/Autoware/issues/2012>`_
+  Merged without adequate description of the bug or fixes made
+* Fix/health checker (`#2012 <https://github.com/CPFL/Autoware/issues/2012>`_)
+* [fix] Install commands for all the packages (`#1861 <https://github.com/CPFL/Autoware/issues/1861>`_)
+  * Initial fixes to detection, sensing, semantics and utils
+  * fixing wrong filename on install command
+  * Fixes to install commands
+  * Hokuyo fix name
+  * Fix obj db
+  * Obj db include fixes
+  * End of final cleaning sweep
+  * Incorrect command order in runtime manager
+  * Param tempfile not required by runtime_manager
+  * * Fixes to runtime manager install commands
+  * Remove devel directory from catkin, if any
+  * Updated launch files for robosense
+  * Updated robosense
+  * Fix/add missing install (`#1977 <https://github.com/CPFL/Autoware/issues/1977>`_)
+  * Added launch install to lidar_kf_contour_track
+  * Added install to op_global_planner
+  * Added install to way_planner
+  * Added install to op_local_planner
+  * Added install to op_simulation_package
+  * Added install to op_utilities
+  * Added install to sync
+  * * Improved installation script for pointgrey packages
+  * Fixed nodelet error for gmsl cameras
+  * USe install space in catkin as well
+  * add install to catkin
+  * Fix install directives (`#1990 <https://github.com/CPFL/Autoware/issues/1990>`_)
+  * Fixed installation path
+  * Fixed params installation path
+  * Fixed cfg installation path
+  * Delete cache on colcon_release
+* Feature/autoware health checker (`#1943 <https://github.com/CPFL/Autoware/issues/1943>`_)
+* Fix license notice in corresponding package.xml
+* Contributors: Abraham Monrroy Cano, Geoffrey Biggs, Masaya Kataoka, 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)
+------------------
+* Moved configuration messages to autoware_config_msgs
+* renamed topics to match, sensing workflow (`#1600 <https://github.com/CPFL/Autoware/issues/1600>`_)
+  [fix] renamed topics to match, sensing workflow on CompareMapFilter
+* Feature/compare map filter (`#1559 <https://github.com/CPFL/Autoware/issues/1559>`_)
+  * add compare map filter
+  * add README
+  * add copyright
+  * change default parameter
+  * fix typo
+  * clang-format
+  * Revert "clang-format"
+  This reverts commit 95869328f35f6ed1e918c26901ad36ab9737e466.
+  * retry clang-format
+* Contributors: Abraham Monrroy, Esteve Fernandez, YamatoAndo
+
+1.8.0 (2018-08-31)
+------------------
+* Fix Indigo build issues
+* [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>`_)
+* Update OpenPlanner libraries (op_planner, op_utitity, op_ros_helpers)
+  Update ring ground filter with latest implementation
+  Update lidar_kf_contour_track with latest implementation
+  Add op_utilities nodes (op_bag_player, op_data_logger, op_pose2tf)
+  Modify autoware_msgs for OpenPlanner use (CloudCluster, DetectedObject, lane, waypoint)
+  Update UI computing.yaml for the new nodes and modifies parameters
+  Update UI sensing.yaml for updated ring_ground_filter params
+* Replaced yaml-cpp library flag
+* Fix finding yaml-cpp by pkg_check_modules
+* Add -lyaml-cpp
+* Remove yaml-cpp find package
+* [Fix] Extend and Update interface.yaml (`#1291 <https://github.com/CPFL/Autoware/pull/1291>`_)
+* Fix cmake and remove msg from runtime manager
+* Fix assertion condition
+* fix review comments
+  - fix CMakeFiles.txt and package.xml related yaml-cpp
+  - use input_topics_size\_
+  - add brackets
+* apply clang-format
+* Modify runtime_manager
+* apply clang-format
+* Modify points_concat_filter to support up to 8 lidars
+* Contributors: Akihito Ohsato, Esteve Fernandez, Kenji Funaoka, Yusuke FUJII, hatem-darweesh
+
+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
+* Editorial changes to README files. See `#1124 <https://github.com/CPFL/Autoware/pull/1124>`_. (`#1125 <https://github.com/CPFL/Autoware/pull/1125>`_)
+* Contributors: Abraham Monrroy, David, 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
+* decoupled ray ground filter into lib and exe, added unit test (`#932 <https://github.com/cpfl/autoware/issues/932>`_)
+* - Add new Node for object polygon representation and tracking (kf_contour_tracker)
+  - Add launch file and tune tracking parameters
+  - Test with Moriyama rosbag
+* Feature/ring_ground_filter parameter description (`#884 <https://github.com/cpfl/autoware/issues/884>`_)
+  * Added a README file for ground_filter tuning
+  * Moved and formatted Patipon instructions on ring_ground_filter
+* Feature/fusion_filter - fusion multiple lidar (`#842 <https://github.com/cpfl/autoware/issues/842>`_)
+  * Add fusion_filter to merge multiple lidar pointclouds
+  * Refactor fusion_filter
+  * Apply clang-format and rebase develop
+  * Add fusion_filter launch and runtime_manager config
+  * Fix names, fusion_filter -> points_concat_filter
+  * Fix build error in ros-indigo
+  * Fix some default message/frame names
+  * Refactor code and apply clang-format
+  * Add configrations for runtime_manager
+  * Fix CMake
+* Feature/cloud transformer (`#860 <https://github.com/cpfl/autoware/issues/860>`_)
+  * Added Cloud transformer node
+  transforms pointcloud to a target frame
+  * Added support for XYZIR point type
+  * Added error checks when transformation unavailable
+* Solved conflicts by ring filter config message naming change
+* Add ground_filter config for runtime_manager (`#828 <https://github.com/cpfl/autoware/issues/828>`_)
+* Added Compilation fix for Kinect
+* Added descriptions to the params in launch file
+* Ray Ground Filter Initial Commit
+* Contributors: AMC, Abraham Monrroy, Akihito Ohsato, Yamato ANDO, christopherho-ApexAI, hatem-darweesh
+
+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
+* [hotfix] fixes to lidar_tracker package(`#787 <https://github.com/cpfl/autoware/issues/787>`_)
+  -Fixed a typo in the ground_filter launch file from points_preprocessor
+  -Fixed ID duplication in kf_lidar_tracker
+  Tested on Ubuntu 14.04 and 16.04
+* Contributors: Abraham Monrroy, Yusuke FUJII
+
+1.4.0 (2017-08-04)
+------------------
+* link to documentation
+* version number must equal current release number so we can start releasing in the future
+* added changelogs
+* Update ground_filter.launch
+* Update ground_filter.launch
+  Added params to launch file
+* Typo Fix
+* Fixed a bug that caused missing points
+* Fixed linking error on 16.04
+* Modified as suggested by @dejanpan on `#655 <https://github.com/cpfl/autoware/issues/655>`_
+* -Standarized code
+  -Added support for the 3 Velodyne Sensors models (use model_sensor {16,32,64})
+  -Parametrized
+* Test adding interface
+* Try to add interface
+* New version of ground_filter
+* Contributors: AMC, Abraham Monrroy, Dejan Pangercic, Patiphon Narksri
+
+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)
+------------------
+* Rename package name.
+  data_filter -> filters
+  points_filter -> points_downsample
+* Contributors: yukikitsukawa

src/ros/catkin/src/points_preprocessor/CMakeLists.txt

@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(points_preprocessor)
+
+find_package(autoware_build_flags REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  autoware_config_msgs
+  autoware_health_checker
+  cv_bridge
+  pcl_conversions
+  pcl_ros
+  roscpp
+  roslint
+  sensor_msgs
+  std_msgs
+  tf
+  tf2
+  tf2_eigen
+  tf2_ros
+  velodyne_pointcloud
+)
+
+catkin_package()
+
+set(ROSLINT_CPP_OPTS "--filter=-build/c++14")
+roslint_cpp(
+  nodes/ray_ground_filter/ray_ground_filter.cpp
+  nodes/ray_ground_filter/ray_ground_filter_main.cpp
+  include/points_preprocessor/ray_ground_filter/ray_ground_filter.h
+)
+
+find_package(Qt5Core REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(PCL 1.7 REQUIRED)
+find_package(OpenMP)
+
+# Resolve system dependency on yaml-cpp, which apparently does not
+# provide a CMake find_package() module.
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
+find_path(YAML_CPP_INCLUDE_DIR NAMES yaml_cpp.h PATHS ${YAML_CPP_INCLUDE_DIRS})
+find_library(YAML_CPP_LIBRARY NAMES YAML_CPP PATHS ${YAML_CPP_LIBRARY_DIRS})
+link_directories(${YAML_CPP_LIBRARY_DIRS})
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
+
+link_directories(${PCL_LIBRARY_DIRS})
+
+# Space Filter
+add_executable(space_filter
+  nodes/space_filter/space_filter.cpp
+)
+target_link_libraries(space_filter
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+)
+
+add_dependencies(space_filter ${catkin_EXPORTED_TARGETS})
+
+# Ring Ground Filter
+add_definitions(${PCL_DEFINITIONS})
+
+add_executable(ring_ground_filter
+  nodes/ring_ground_filter/ring_ground_filter.cpp
+)
+
+target_include_directories(ring_ground_filter PRIVATE
+  ${PCL_INCLUDE_DIRS}
+)
+
+target_link_libraries(ring_ground_filter
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${Qt5Core_LIBRARIES}
+)
+
+add_dependencies(ring_ground_filter ${catkin_EXPORTED_TARGETS})
+
+# Ray Ground Filter
+add_library(ray_ground_filter_lib SHARED
+  nodes/ray_ground_filter/ray_ground_filter.cpp
+)
+
+if(OPENMP_FOUND)
+  set_target_properties(ray_ground_filter_lib PROPERTIES
+    COMPILE_FLAGS ${OpenMP_CXX_FLAGS}
+    LINK_FLAGS ${OpenMP_CXX_FLAGS}
+  )
+endif()
+
+target_include_directories(ray_ground_filter_lib PRIVATE
+  ${OpenCV_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+)
+
+target_link_libraries(ray_ground_filter_lib
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+  ${Qt5Core_LIBRARIES}
+)
+
+add_dependencies(ray_ground_filter_lib ${catkin_EXPORTED_TARGETS})
+
+add_executable(ray_ground_filter
+  nodes/ray_ground_filter/ray_ground_filter_main.cpp
+)
+target_link_libraries(ray_ground_filter ray_ground_filter_lib)
+add_dependencies(ray_ground_filter ${catkin_EXPORTED_TARGETS})
+
+# Points Concat filter
+add_executable(points_concat_filter
+  nodes/points_concat_filter/points_concat_filter.cpp
+)
+
+target_include_directories(points_concat_filter PRIVATE
+  ${PCL_INCLUDE_DIRS}
+)
+
+target_link_libraries(points_concat_filter
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${YAML_CPP_LIBRARIES}
+)
+
+add_dependencies(points_concat_filter ${catkin_EXPORTED_TARGETS})
+
+#Cloud Transformer
+add_executable(cloud_transformer
+  nodes/cloud_transformer/cloud_transformer_node.cpp
+)
+
+target_include_directories(cloud_transformer PRIVATE
+  ${PCL_INCLUDE_DIRS}
+)
+
+target_link_libraries(cloud_transformer
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${Qt5Core_LIBRARIES}
+)
+add_dependencies(cloud_transformer ${catkin_EXPORTED_TARGETS})
+
+#Compare Map Filter
+add_executable(compare_map_filter
+  nodes/compare_map_filter/compare_map_filter.cpp
+)
+
+target_include_directories(compare_map_filter PRIVATE
+  ${PCL_INCLUDE_DIRS}
+)
+
+target_link_libraries(compare_map_filter
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${Qt5Core_LIBRARIES}
+)
+add_dependencies(compare_map_filter ${catkin_EXPORTED_TARGETS})
+
+### Unit Tests ###
+#if (CATKIN_ENABLE_TESTING)
+#    find_package(rostest REQUIRED)
+#    find_package(roslaunch REQUIRED)
+
+#    add_rostest_gtest(test_points_preprocessor
+#      test/test_points_preprocessor.test
+#      test/src/test_points_preprocessor.cpp)
+#   target_include_directories(test_points_preprocessor PRIVATE
+#          nodes/ray_ground_filter/include
+#            test/include)
+#    target_link_libraries(test_points_preprocessor
+#            ray_ground_filter_lib
+#            ${catkin_LIBRARIES})
+#endif ()
+
+install(
+  TARGETS
+    cloud_transformer
+    points_concat_filter
+    ray_ground_filter_lib
+    ray_ground_filter
+    ring_ground_filter
+    space_filter
+    compare_map_filter
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
+
+if (CATKIN_ENABLE_TESTING)
+  roslint_add_test()
+endif()

src/ros/catkin/src/points_preprocessor/include/gencolors.cpp

@@ -0,0 +1,146 @@
+#ifndef GENCOLORS_CPP_
+#define GENCOLORS_CPP_
+
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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.
+//
+//M*/
+#include "opencv2/core/core.hpp"
+//#include "precomp.hpp"
+#include <opencv2/opencv.hpp>
+
+#include <iostream>
+
+using namespace cv;
+
+static void downsamplePoints( const Mat& src, Mat& dst, size_t count )
+{
+    CV_Assert( count >= 2 );
+    CV_Assert( src.cols == 1 || src.rows == 1 );
+    CV_Assert( src.total() >= count );
+    CV_Assert( src.type() == CV_8UC3);
+
+    dst.create( 1, (int)count, CV_8UC3 );
+    //TODO: optimize by exploiting symmetry in the distance matrix
+    Mat dists( (int)src.total(), (int)src.total(), CV_32FC1, Scalar(0) );
+    if( dists.empty() )
+        std::cerr << "Such big matrix cann't be created." << std::endl;
+
+    for( int i = 0; i < dists.rows; i++ )
+    {
+        for( int j = i; j < dists.cols; j++ )
+        {
+            float dist = (float)norm(src.at<Point3_<uchar> >(i) - src.at<Point3_<uchar> >(j));
+            dists.at<float>(j, i) = dists.at<float>(i, j) = dist;
+        }
+    }
+
+    double maxVal;
+    Point maxLoc;
+    minMaxLoc(dists, 0, &maxVal, 0, &maxLoc);
+
+    dst.at<Point3_<uchar> >(0) = src.at<Point3_<uchar> >(maxLoc.x);
+    dst.at<Point3_<uchar> >(1) = src.at<Point3_<uchar> >(maxLoc.y);
+
+    Mat activedDists( 0, dists.cols, dists.type() );
+    Mat candidatePointsMask( 1, dists.cols, CV_8UC1, Scalar(255) );
+    activedDists.push_back( dists.row(maxLoc.y) );
+    candidatePointsMask.at<uchar>(0, maxLoc.y) = 0;
+
+    for( size_t i = 2; i < count; i++ )
+    {
+        activedDists.push_back(dists.row(maxLoc.x));
+        candidatePointsMask.at<uchar>(0, maxLoc.x) = 0;
+
+        Mat minDists;
+        reduce( activedDists, minDists, 0, CV_REDUCE_MIN );
+        minMaxLoc( minDists, 0, &maxVal, 0, &maxLoc, candidatePointsMask );
+        dst.at<Point3_<uchar> >((int)i) = src.at<Point3_<uchar> >(maxLoc.x);
+    }
+}
+
+void generateColors( std::vector<Scalar>& colors, size_t count, size_t factor=100 )
+{
+    if( count < 1 )
+        return;
+
+    colors.resize(count);
+
+    if( count == 1 )
+    {
+        colors[0] = Scalar(0,0,255); // red
+        return;
+    }
+    if( count == 2 )
+    {
+        colors[0] = Scalar(0,0,255); // red
+        colors[1] = Scalar(0,255,0); // green
+        return;
+    }
+
+    // Generate a set of colors in RGB space. A size of the set is severel times (=factor) larger then
+    // the needed count of colors.
+    Mat bgr( 1, (int)(count*factor), CV_8UC3 );
+    randu( bgr, 0, 256 );
+
+    // Convert the colors set to Lab space.
+    // Distances between colors in this space correspond a human perception.
+    Mat lab;
+    cvtColor( bgr, lab, cv::COLOR_BGR2Lab);
+
+    // Subsample colors from the generated set so that
+    // to maximize the minimum distances between each other.
+    // Douglas-Peucker algorithm is used for this.
+    Mat lab_subset;
+    downsamplePoints( lab, lab_subset, count );
+
+    // Convert subsampled colors back to RGB
+    Mat bgr_subset;
+    cvtColor( lab_subset, bgr_subset, cv::COLOR_BGR2Lab );
+
+    CV_Assert( bgr_subset.total() == count );
+    for( size_t i = 0; i < count; i++ )
+    {
+        Point3_<uchar> c = bgr_subset.at<Point3_<uchar> >((int)i);
+        colors[i] = Scalar(c.x, c.y, c.z);
+    }
+}
+
+#endif //GENCOLORS_CPP

src/ros/catkin/src/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h

@@ -0,0 +1,179 @@
+/*
+ * Copyright 2017-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.
+ ********************
+ *  v1.0: amc-nu (abrahammonrroy@yahoo.com)
+ */
+#ifndef POINTS_PREPROCESSOR_RAY_GROUND_FILTER_RAY_GROUND_FILTER_H
+#define POINTS_PREPROCESSOR_RAY_GROUND_FILTER_RAY_GROUND_FILTER_H
+
+#include <iostream>
+#include <algorithm>
+#include <vector>
+#include <memory>
+#include <string>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_types.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/normal_3d_omp.h>
+#include <pcl/filters/extract_indices.h>
+#include <velodyne_pointcloud/point_types.h>
+#include "autoware_config_msgs/ConfigRayGroundFilter.h"
+
+#include <tf2/transform_datatypes.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_eigen/tf2_eigen.h>
+
+// headers in Autoware Health Checker
+#include <autoware_health_checker/health_checker/health_checker.h>
+
+#include <opencv2/core/version.hpp>
+#if (CV_MAJOR_VERSION == 3)
+#include "gencolors.cpp"
+#else
+#include <opencv2/contrib/contrib.hpp>
+#endif
+
+class RayGroundFilter
+{
+private:
+  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
+  ros::NodeHandle node_handle_;
+  ros::Subscriber points_node_sub_;
+  ros::Subscriber config_node_sub_;
+  ros::Publisher groundless_points_pub_;
+  ros::Publisher ground_points_pub_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  std::string input_point_topic_;
+  std::string base_frame_;
+
+  double general_max_slope_;            // degrees
+  double local_max_slope_;              // degrees
+  double radial_divider_angle_;         // distance in rads between dividers
+  double concentric_divider_distance_;  // distance in meters between concentric divisions
+  double min_height_threshold_;         // minimum height threshold regardless the slope, useful for close points
+  double clipping_height_;              // the points higher than this will be removed from the input cloud.
+  double min_point_distance_;           // minimum distance from the origin to consider a point as valid
+  double reclass_distance_threshold_;   // distance between points at which re classification will occur
+
+  size_t radial_dividers_num_;
+  size_t concentric_dividers_num_;
+
+  std::vector<cv::Scalar> colors_;
+  const size_t color_num_ = 60;  // different number of color to generate
+
+  struct PointXYZIRTColor
+  {
+    pcl::PointXYZI point;
+
+    float radius;  // cylindric coords on XY Plane
+    float theta;   // angle deg on XY plane
+
+    size_t radial_div;      // index of the radial divsion to which this point belongs to
+    size_t concentric_div;  // index of the concentric division to which this points belongs to
+
+    size_t red;    // Red component  [0-255]
+    size_t green;  // Green Component[0-255]
+    size_t blue;   // Blue component [0-255]
+
+    size_t original_index;  // index of this point in the source pointcloud
+  };
+  typedef std::vector<PointXYZIRTColor> PointCloudXYZIRTColor;
+
+  void update_config_params(const autoware_config_msgs::ConfigRayGroundFilter::ConstPtr& param);
+
+  /*!
+   * Output transformed PointCloud from in_cloud_ptr->header.frame_id to in_target_frame
+   * @param[in] in_target_frame Coordinate system to perform transform
+   * @param[in] in_cloud_ptr PointCloud to perform transform
+   * @param[out] out_cloud_ptr Resulting transformed PointCloud
+   * @retval true transform successed
+   * @retval false transform faild
+   */
+  bool TransformPointCloud(const std::string& in_target_frame, const sensor_msgs::PointCloud2::ConstPtr& in_cloud_ptr,
+                           const sensor_msgs::PointCloud2::Ptr& out_cloud_ptr);
+
+  void publish_cloud(const ros::Publisher& in_publisher,
+                     const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_to_publish_ptr,
+                     const std_msgs::Header& in_header);
+
+  /*!
+   *
+   * @param[in] in_cloud Input Point Cloud to be organized in radial segments
+   * @param[out] out_organized_points Custom Point Cloud filled with XYZRTZColor data
+   * @param[out] out_radial_divided_indices Indices of the points in the original cloud for each radial segment
+   * @param[out] out_radial_ordered_clouds Vector of Points Clouds, each element will contain the points ordered
+   */
+  void ConvertXYZIToRTZColor(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud,
+                             const std::shared_ptr<PointCloudXYZIRTColor>& out_organized_points,
+                             const std::shared_ptr<std::vector<pcl::PointIndices> >& out_radial_divided_indices,
+                             const std::shared_ptr<std::vector<PointCloudXYZIRTColor> >& out_radial_ordered_clouds);
+
+  /*!
+   * Classifies Points in the PointCoud as Ground and Not Ground
+   * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud ordered by radial distance from the origin
+   * @param out_ground_indices Returns the indices of the points classified as ground in the original PointCloud
+   * @param out_no_ground_indices Returns the indices of the points classified as not ground in the original PointCloud
+   */
+  void ClassifyPointCloud(const std::vector<PointCloudXYZIRTColor>& in_radial_ordered_clouds,
+                          const pcl::PointIndices::Ptr& out_ground_indices,
+                          const pcl::PointIndices::Ptr& out_no_ground_indices);
+
+  /*!
+   * Removes the points higher than a threshold
+   * @param in_cloud_ptr PointCloud to perform Clipping
+   * @param in_clip_height Maximum allowed height in the cloud
+   * @param out_clipped_cloud_ptr Resultung PointCloud with the points removed
+   */
+  void ClipCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, const double in_clip_height,
+                 pcl::PointCloud<pcl::PointXYZI>::Ptr out_clipped_cloud_ptr);
+
+  /*!
+   * Returns the resulting complementary PointCloud, one with the points kept and the other removed as indicated
+   * in the indices
+   * @param in_cloud_ptr Input PointCloud to which the extraction will be performed
+   * @param in_indices Indices of the points to be both removed and kept
+   * @param out_only_indices_cloud_ptr Resulting PointCloud with the indices kept
+   * @param out_removed_indices_cloud_ptr Resulting PointCloud with the indices removed
+   */
+  void ExtractPointsIndices(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr,
+                            const pcl::PointIndices& in_indices,
+                            pcl::PointCloud<pcl::PointXYZI>::Ptr out_only_indices_cloud_ptr,
+                            pcl::PointCloud<pcl::PointXYZI>::Ptr out_removed_indices_cloud_ptr);
+
+  /*!
+   * Removes points up to a certain distance in the XY Plane
+   * @param in_cloud_ptr Input PointCloud
+   * @param in_min_distance Minimum valid distance, points closer than this will be removed.
+   * @param out_filtered_cloud_ptr Resulting PointCloud with the invalid points removed.
+   */
+  void RemovePointsUpTo(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, double in_min_distance,
+                        pcl::PointCloud<pcl::PointXYZI>::Ptr out_filtered_cloud_ptr);
+
+  void CloudCallback(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud);
+
+  friend class RayGroundFilter_clipCloud_Test;
+
+public:
+  RayGroundFilter();
+  void Run();
+};
+
+#endif  // POINTS_PREPROCESSOR_RAY_GROUND_FILTER_RAY_GROUND_FILTER_H

src/ros/catkin/src/points_preprocessor/interface.yaml

@@ -0,0 +1,18 @@
+- name: /cloud_transformer
+  publish: [/points_transformed]
+  subscribe: [/points_raw]
+- name: /points_concat_filter
+  publish: [/points_concat]
+  subscribe: [/config/points_concat_filter, /lidar*/points_raw]
+- name: /ray_ground_filter
+  publish: [/points_no_ground, /points_ground]
+  subscribe: [/points_raw, /config/ray_ground_filter]
+- name: /ring_ground_filter
+  publish: [/points_no_ground, /points_ground]
+  subscribe: [/config/ring_ground_filter, /points_raw]
+- name: /space_filter
+  publish: [/points_clipped]
+  subscribe: [/points_raw]
+- name: /compare_map_filter
+  publish: [/points_ground, /points_no_ground]
+  subscribe: [/config/compare_map_filter, /points_raw, /points_map]

src/ros/catkin/src/points_preprocessor/launch/cloud_transformer.launch

@@ -0,0 +1,14 @@
+<!-- Launch file for Cloud Transformer -->
+<launch>
+
+        <arg name="input_point_topic" default="/points_raw" /> <!-- input_point_topic, the coordinates of each point in this topic will be transformed to the new frame. -->
+        <arg name="output_point_topic" default="/points_transformed" /> <!-- output_point_topic, output topic name -->
+        <arg name="target_frame" default="base_link" /> <!-- target_frame, coordinate frame system target -->
+
+        <!-- rosrun points_preprocessor ray_ground_filter -->
+        <node pkg="points_preprocessor" type="cloud_transformer" name="cloud_transformer" output="screen">
+                <param name="input_point_topic" value="$(arg input_point_topic)" />
+                <param name="output_point_topic" value="$(arg output_point_topic)" />
+                <param name="target_frame" value="$(arg target_frame)" />
+        </node>
+</launch>

src/ros/catkin/src/points_preprocessor/launch/compare_map_filter.launch

@@ -0,0 +1,22 @@
+<launch>
+  <arg name="input_point_topic" default="/points_raw" />
+  <arg name="input_map_topic" default="/points_map" />
+  <arg name="output_match_topic" default="/points_ground" />
+  <arg name="output_unmatch_topic" default="/points_no_ground" />
+
+  <arg name="distance_threshold" default="0.3" />
+  <arg name="min_clipping_height" default="-2.0" />
+  <arg name="max_clipping_height" default="0.5" />
+
+  <node pkg="points_preprocessor" type="compare_map_filter" name="compare_map_filter">
+    <remap from="/points_raw" to="$(arg input_point_topic)"/>
+    <remap from="/points_map" to="$(arg input_map_topic)"/>
+    <remap from="/points_ground" to="$(arg output_match_topic)"/>
+    <remap from="/points_no_ground" to="$(arg output_unmatch_topic)"/>
+
+    <param name="distance_threshold" value="$(arg distance_threshold)" />
+    <param name="min_clipping_height" value="$(arg min_clipping_height)" />
+    <param name="max_clipping_height" value="$(arg max_clipping_height)" />
+  </node>
+
+</launch>

src/ros/catkin/src/points_preprocessor/launch/points_concat_filter.launch

@@ -0,0 +1,13 @@
+<!-- -->
+<launch>
+  <arg name="input_topics" default="[/points_alpha, /points_beta]" />
+  <arg name="output_topic" default="/points_concat" />
+  <arg name="output_frame_id" default="velodyne" />
+
+  <node pkg="points_preprocessor" type="points_concat_filter"
+        name="points_concat_filter" output="screen">
+    <param name="output_frame_id" value="$(arg output_frame_id)" />
+    <param name="input_topics" value="$(arg input_topics)" />
+    <remap from="/points_concat" to="$(arg output_topic)" />
+  </node>
+</launch>

src/ros/catkin/src/points_preprocessor/launch/ray_ground_filter.launch

@@ -0,0 +1,31 @@
+<!-- Launch file for Ray Ground Filter -->
+<launch>
+  <arg name="input_point_topic" default="/points_raw" />  <!-- input_point_topic, ground filtering will be performed over the pointcloud in this topic. -->
+  <arg name="base_frame" default="base_link" />  <!-- Coordinate system to perform transform (default base_link) -->
+  <arg name="clipping_height" default="2.0" />  <!-- Remove Points above this height value (default 2.0 meters) -->
+  <arg name="min_point_distance" default="1.85" />  <!-- Removes Points closer than this distance from the sensor origin (default 1.85 meters) -->
+  <arg name="radial_divider_angle" default="0.08" />  <!-- Angle of each Radial division on the XY Plane (default 0.08 degrees)-->
+  <arg name="concentric_divider_distance" default="0.0" />  <!-- Distance of each concentric division on the XY Plane (default 0.0 meters) -->
+  <arg name="local_max_slope" default="8" />  <!-- Max Slope of the ground between Points (default 8 degrees) -->
+  <arg name="general_max_slope" default="5" />  <!-- Max Slope of the ground in the entire PointCloud, used when reclassification occurs (default 5 degrees)-->
+  <arg name="min_height_threshold" default="0.5" />  <!-- Minimum height threshold between points (default 0.05 meters)-->
+  <arg name="reclass_distance_threshold" default="0.2" />  <!-- Distance between points at which re classification will occur (default 0.2 meters)-->
+  <arg name="no_ground_point_topic" default="/points_no_ground" />
+  <arg name="ground_point_topic" default="/points_ground" />
+
+  <!-- rosrun points_preprocessor ray_ground_filter -->
+  <node pkg="points_preprocessor" type="ray_ground_filter" name="ray_ground_filter" output="log">
+    <param name="input_point_topic" value="$(arg input_point_topic)" />
+    <param name="base_frame" value="$(arg base_frame)" />
+    <param name="clipping_height" value="$(arg clipping_height)" />
+    <param name="min_point_distance" value="$(arg min_point_distance)" />
+    <param name="radial_divider_angle" value="$(arg radial_divider_angle)" />
+    <param name="concentric_divider_distance" value="$(arg concentric_divider_distance)" />
+    <param name="local_max_slope" value="$(arg local_max_slope)" />
+    <param name="general_max_slope" value="$(arg general_max_slope)" />
+    <param name="min_height_threshold" value="$(arg min_height_threshold)" />
+    <param name="reclass_distance_threshold" value="$(arg reclass_distance_threshold)" />
+    <param name="no_ground_point_topic" value="$(arg no_ground_point_topic)" />
+    <param name="ground_point_topic" value="$(arg ground_point_topic)" />
+  </node>
+</launch>

src/ros/catkin/src/points_preprocessor/launch/ring_ground_filter.launch

@@ -0,0 +1,27 @@
+<!-- Launch file for Ground Filter -->
+<launch>
+
+        <arg name="point_topic" default="/velodyne_points" />
+        <arg name="remove_floor" default="true" />
+        <arg name="sensor_model" default="64" />        
+        <arg name="sensor_height" default="2.0" />
+        
+        <arg name="max_slope" default="10.0" />
+        <arg name="vertical_thres" default="0.08" />
+                
+        <arg name="no_ground_point_topic" default="/points_no_ground" />
+        <arg name="ground_point_topic" default="/points_ground" />
+        
+        <!-- rosrun lidar_tracker ground_filter -->
+        <node pkg="points_preprocessor" type="ring_ground_filter" name="ring_ground_filter" output="log">
+                <param name="point_topic" value="$(arg point_topic)" />
+                <param name="remove_floor" value="$(arg remove_floor)" />
+                <param name="sensor_model" value="$(arg sensor_model)" />
+                <param name="sensor_height" value="$(arg sensor_height)" />
+                <param name="max_slope" value="$(arg max_slope)" />
+                <param name="vertical_thres" value="$(arg vertical_thres)" />
+                <param name="no_ground_point_topic" value="$(arg no_ground_point_topic)" />
+                <param name="ground_point_topic" value="$(arg ground_point_topic)" />                               
+        </node>
+
+</launch>

src/ros/catkin/src/points_preprocessor/launch/space_filter.launch

@@ -0,0 +1,24 @@
+<!-- Launch file for Space Filter -->
+<launch>
+  <arg name="subscribe_topic" default="/points_raw" />
+  <arg name="vertical_removal" default="true" />
+  <arg name="below_distance" default="-1.3" />
+  <arg name="above_distance" default="0.5" />
+  
+  <arg name="lateral_removal" default="false" />
+  <arg name="left_distance" default="5" />
+  <arg name="right_distance" default="5" />
+
+  <!-- rosrun lidar_tracker space_filter -->
+  <node pkg="points_preprocessor" type="space_filter" name="space_filter">
+    <param name="subscribe_topic" value="$(arg subscribe_topic)" />
+    <param name="vertical_removal" value="$(arg vertical_removal)" />
+    <param name="below_distance" value="$(arg below_distance)" />
+    <param name="above_distance" value="$(arg above_distance)" />
+
+    <param name="lateral_removal" value="$(arg lateral_removal)" />
+    <param name="left_distance" value="$(arg left_distance)" />
+    <param name="right_distance" value="$(arg right_distance)" />
+  </node>
+
+</launch>

src/ros/catkin/src/points_preprocessor/nodes/cloud_transformer/cloud_transformer_node.cpp

@@ -0,0 +1,187 @@
+/*
+ * Copyright 2017-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.
+ ********************
+ *  v1.0: amc-nu (abrahammonrroy@yahoo.com)
+ */
+#include <iostream>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_types.h>
+#include <velodyne_pointcloud/point_types.h>
+#include <tf/transform_listener.h>
+#include <pcl_ros/transforms.h>
+
+class CloudTransformerNode
+{
+private:
+
+  ros::NodeHandle     node_handle_;
+  ros::Subscriber     points_node_sub_;
+  ros::Publisher      transformed_points_pub_;
+
+  std::string         input_point_topic_;
+  std::string         target_frame_;
+  std::string         output_point_topic_;
+
+  tf::TransformListener *tf_listener_ptr_;
+
+  bool                transform_ok_;
+
+  void publish_cloud(const ros::Publisher& in_publisher,
+                     const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &in_cloud_msg)
+  {
+    in_publisher.publish(in_cloud_msg);
+  }
+
+  void transformXYZIRCloud(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>& in_cloud,
+                           pcl::PointCloud<velodyne_pointcloud::PointXYZIR>& out_cloud,
+                           const tf::StampedTransform& in_tf_stamped_transform)
+  {
+    Eigen::Matrix4f transform;
+    pcl_ros::transformAsMatrix(in_tf_stamped_transform, transform);
+
+    if (&in_cloud != &out_cloud)
+    {
+      out_cloud.header   = in_cloud.header;
+      out_cloud.is_dense = in_cloud.is_dense;
+      out_cloud.width    = in_cloud.width;
+      out_cloud.height   = in_cloud.height;
+      out_cloud.points.reserve (out_cloud.points.size ());
+      out_cloud.points.assign (in_cloud.points.begin (), in_cloud.points.end ());
+      out_cloud.sensor_orientation_ = in_cloud.sensor_orientation_;
+      out_cloud.sensor_origin_      = in_cloud.sensor_origin_;
+      }
+    if (in_cloud.is_dense)
+      {
+      for (size_t i = 0; i < out_cloud.points.size (); ++i)
+        {
+        //out_cloud.points[i].getVector3fMap () = transform * in_cloud.points[i].getVector3fMap ();
+        Eigen::Matrix<float, 3, 1> pt (in_cloud[i].x, in_cloud[i].y, in_cloud[i].z);
+        out_cloud[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) +
+                          transform (0, 1) * pt.coeffRef (1) +
+                          transform (0, 2) * pt.coeffRef (2) +
+                          transform (0, 3));
+        out_cloud[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) +
+                          transform (1, 1) * pt.coeffRef (1) +
+                          transform (1, 2) * pt.coeffRef (2) +
+                          transform (1, 3));
+        out_cloud[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) +
+                          transform (2, 1) * pt.coeffRef (1) +
+                          transform (2, 2) * pt.coeffRef (2) +
+                          transform (2, 3));
+        }
+      }
+    else
+    {
+      // Dataset might contain NaNs and Infs, so check for them first,
+      for (size_t i = 0; i < out_cloud.points.size (); ++i)
+      {
+        if (!pcl_isfinite (in_cloud.points[i].x) ||
+                   !pcl_isfinite (in_cloud.points[i].y) ||
+                   !pcl_isfinite (in_cloud.points[i].z))
+          {continue;}
+        //out_cloud.points[i].getVector3fMap () = transform * in_cloud.points[i].getVector3fMap ();
+        Eigen::Matrix<float, 3, 1> pt (in_cloud[i].x, in_cloud[i].y, in_cloud[i].z);
+        out_cloud[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) +
+                          transform (0, 1) * pt.coeffRef (1) +
+                          transform (0, 2) * pt.coeffRef (2) +
+                          transform (0, 3));
+        out_cloud[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) +
+                          transform (1, 1) * pt.coeffRef (1) +
+                          transform (1, 2) * pt.coeffRef (2) +
+                          transform (1, 3));
+        out_cloud[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) +
+                          transform (2, 1) * pt.coeffRef (1) +
+                          transform (2, 2) * pt.coeffRef (2) +
+                          transform (2, 3));
+      }
+    }
+  }
+
+  void CloudCallback(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &in_sensor_cloud)
+  {
+    pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::Ptr transformed_cloud_ptr (new pcl::PointCloud<velodyne_pointcloud::PointXYZIR>);
+
+    bool do_transform = false;
+    tf::StampedTransform transform;
+    if (target_frame_ != in_sensor_cloud->header.frame_id)
+    {
+      try {
+        tf_listener_ptr_->lookupTransform(target_frame_, in_sensor_cloud->header.frame_id, ros::Time(0),
+                                          transform);
+        do_transform = true;
+      }
+      catch (tf::TransformException ex) {
+        ROS_ERROR("cloud_transformer: %s NOT Transforming.", ex.what());
+        do_transform = false;
+        transform_ok_ = false;
+      }
+    }
+    if (do_transform)
+    {
+      transformXYZIRCloud(*in_sensor_cloud, *transformed_cloud_ptr, transform);
+      transformed_cloud_ptr->header.frame_id = target_frame_;
+      if (!transform_ok_)
+        {ROS_INFO("cloud_transformer: Correctly Transformed"); transform_ok_=true;}
+    }
+    else
+      { pcl::copyPointCloud(*in_sensor_cloud, *transformed_cloud_ptr);}
+
+    publish_cloud(transformed_points_pub_, transformed_cloud_ptr);
+  }
+
+public:
+  CloudTransformerNode(tf::TransformListener* in_tf_listener_ptr):node_handle_("~"), transform_ok_(false)
+  {
+    tf_listener_ptr_ = in_tf_listener_ptr;
+  }
+  void Run()
+  {
+    ROS_INFO("Initializing Cloud Transformer, please wait...");
+    node_handle_.param<std::string>("input_point_topic", input_point_topic_, "/points_raw");
+    ROS_INFO("Input point_topic: %s", input_point_topic_.c_str());
+
+    node_handle_.param<std::string>("target_frame", target_frame_, "velodyne");
+    ROS_INFO("Target Frame in TF (target_frame) : %s", target_frame_.c_str());
+
+    node_handle_.param<std::string>("output_point_topic", output_point_topic_, "/points_transformed");
+    ROS_INFO("output_point_topic: %s", output_point_topic_.c_str());
+
+    ROS_INFO("Subscribing to... %s", input_point_topic_.c_str());
+    points_node_sub_ = node_handle_.subscribe(input_point_topic_, 1, &CloudTransformerNode::CloudCallback, this);
+
+    transformed_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(output_point_topic_, 2);
+
+    ROS_INFO("Ready");
+
+    ros::spin();
+
+  }
+
+};
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "cloud_transformer");
+  tf::TransformListener tf_listener;
+  CloudTransformerNode app(&tf_listener);
+
+  app.Run();
+
+  return 0;
+
+}

src/ros/catkin/src/points_preprocessor/nodes/compare_map_filter/README.md

@@ -0,0 +1,51 @@
+# Compare Map Filter
+
+Autoware package that compare the LiDAR PointCloud and PointCloud Map and then extract (or eliminate) coincident points.
+
+### Requirements
+
+* PointCloud Map with extremely few unnecessary PointCloud (people, cars, etc.).
+
+### How to launch
+
+* From a sourced terminal:
+    - `roslaunch points_preprocessor compare_map_filter.launch`
+
+* From Runtime Manager:
+
+Sensing Tab -> Points Preprocessor -> `compare_map_filter`
+You can change the config, as well as other parameters, by clicking [app]
+
+### Parameters
+
+Launch file available parameters:
+
+|Parameter| Type| Description|
+----------|-----|--------
+|`input_point_topic`|*String*|PointCloud source topic. Default `/points_raw`.|
+|`input_map_topic`|*String*|PointCloud Map topic. Default `/points_map`.|
+|`output_match_topic`|*String*|Comparing LiDAR PointCloud and PointCloud Map, topic of the PointCloud which matched. Default `/match_points`.|
+|`output_unmatch_topic`|*String*|Comparing LiDAR PointCloud and PointCloud Map, topic of the PointCloud which unmatched. Default `/unmatch_points`.|
+|`distance_threshold`|*Double*|Threshold for comparing LiDAR PointCloud and PointCloud Map. Euclidean distance (mether).  Default `0.3`.|
+|`min_clipping_height`|*Double*|Remove the points where the height is lower than the threshold. (Based on sensor coordinates). Default `-2.0`.|
+|`max_clipping_height`|*Double*|Remove the points where the height is higher than the threshold. (Based on sensor coordinates). Default `0.5`.|
+
+### Subscribed topics
+
+|Topic|Type|Objective|
+------|----|---------
+|`/points_raw`|`sensor_msgs/PointCloud2`|PointCloud source topic.|
+|`/points_map`|`sensor_msgs/PointCloud2`|PointCloud Map topic.|
+|`/config/compare_map_filter`|`autoware_msgs/ConfigCompareMapFilter`|Configuration adjustment for threshold.|
+|`/tf`|TF|sensor frame <-> map frame|
+
+### Published topics
+
+|Topic|Type|Objective|
+------|----|---------
+|`/match_points`|`sensor_msgs/PointCloud2`|Comparing LiDAR PointCloud and PointCloud Map, topic of the PointCloud which matched.|
+|`/unmatch_points`|`sensor_msgs/PointCloud2`|Comparing LiDAR PointCloud and PointCloud Map, topic of the PointCloud which unmatched.|
+
+### Video
+
+[![Compare Map Filter Autoware](https://img.youtube.com/vi/7nK6JrI7TAI/0.jpg)](https://www.youtube.com/watch?v=7nK6JrI7TAI)

src/ros/catkin/src/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp

@@ -0,0 +1,222 @@
+/*
+ *  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.
+ */
+
+#include <ros/ros.h>
+
+#include <tf/tf.h>
+
+#include <sensor_msgs/point_cloud_conversion.h>
+#include <sensor_msgs/PointCloud2.h>
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/kdtree/kdtree_flann.h>
+
+#include <autoware_config_msgs/ConfigCompareMapFilter.h>
+
+class CompareMapFilter
+{
+public:
+  CompareMapFilter();
+
+private:
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+
+  ros::Subscriber config_sub_;
+  ros::Subscriber sensor_points_sub_;
+  ros::Subscriber map_sub_;
+  ros::Publisher match_points_pub_;
+  ros::Publisher unmatch_points_pub_;
+
+  tf::TransformListener* tf_listener_;
+
+  pcl::KdTreeFLANN<pcl::PointXYZI> tree_;
+
+  double distance_threshold_;
+  double min_clipping_height_;
+  double max_clipping_height_;
+
+  std::string map_frame_;
+
+  void configCallback(const autoware_config_msgs::ConfigCompareMapFilter::ConstPtr& config_msg_ptr);
+  void pointsMapCallback(const sensor_msgs::PointCloud2::ConstPtr& map_cloud_msg_ptr);
+  void sensorPointsCallback(const sensor_msgs::PointCloud2::ConstPtr& sensorTF_cloud_msg_ptr);
+  void searchMatchingCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr,
+                           pcl::PointCloud<pcl::PointXYZI>::Ptr match_cloud_ptr,
+                           pcl::PointCloud<pcl::PointXYZI>::Ptr unmatch_cloud_ptr);
+};
+
+CompareMapFilter::CompareMapFilter()
+  : nh_()
+  , nh_private_("~")
+  , tf_listener_(new tf::TransformListener)
+  , distance_threshold_(0.3)
+  , min_clipping_height_(-2.0)
+  , max_clipping_height_(0.5)
+  , map_frame_("/map")
+{
+  nh_private_.param("distance_threshold", distance_threshold_, distance_threshold_);
+  nh_private_.param("min_clipping_height", min_clipping_height_, min_clipping_height_);
+  nh_private_.param("max_clipping_height", max_clipping_height_, max_clipping_height_);
+
+  config_sub_ = nh_.subscribe("/config/compare_map_filter", 10, &CompareMapFilter::configCallback, this);
+  sensor_points_sub_ = nh_.subscribe("/points_raw", 1, &CompareMapFilter::sensorPointsCallback, this);
+  map_sub_ = nh_.subscribe("/points_map", 10, &CompareMapFilter::pointsMapCallback, this);
+  match_points_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/points_ground", 10);
+  unmatch_points_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/points_no_ground", 10);
+}
+
+void CompareMapFilter::configCallback(const autoware_config_msgs::ConfigCompareMapFilter::ConstPtr& config_msg_ptr)
+{
+  distance_threshold_ = config_msg_ptr->distance_threshold;
+  min_clipping_height_ = config_msg_ptr->min_clipping_height;
+  max_clipping_height_ = config_msg_ptr->max_clipping_height;
+}
+
+void CompareMapFilter::pointsMapCallback(const sensor_msgs::PointCloud2::ConstPtr& map_cloud_msg_ptr)
+{
+  pcl::PointCloud<pcl::PointXYZI>::Ptr map_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+  pcl::fromROSMsg(*map_cloud_msg_ptr, *map_cloud_ptr);
+  tree_.setInputCloud(map_cloud_ptr);
+
+  map_frame_ = map_cloud_msg_ptr->header.frame_id;
+}
+
+void CompareMapFilter::sensorPointsCallback(const sensor_msgs::PointCloud2::ConstPtr& sensorTF_cloud_msg_ptr)
+{
+  const ros::Time sensor_time = sensorTF_cloud_msg_ptr->header.stamp;
+  const std::string sensor_frame = sensorTF_cloud_msg_ptr->header.frame_id;
+
+  pcl::PointCloud<pcl::PointXYZI>::Ptr sensorTF_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+  pcl::fromROSMsg(*sensorTF_cloud_msg_ptr, *sensorTF_cloud_ptr);
+
+  pcl::PointCloud<pcl::PointXYZI>::Ptr sensorTF_clipping_height_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+  sensorTF_clipping_height_cloud_ptr->header = sensorTF_cloud_ptr->header;
+  for (size_t i = 0; i < sensorTF_cloud_ptr->points.size(); ++i)
+  {
+    if (sensorTF_cloud_ptr->points[i].z > min_clipping_height_ &&
+        sensorTF_cloud_ptr->points[i].z < max_clipping_height_)
+    {
+      sensorTF_clipping_height_cloud_ptr->points.push_back(sensorTF_cloud_ptr->points[i]);
+    }
+  }
+
+  pcl::PointCloud<pcl::PointXYZI>::Ptr mapTF_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+  try
+  {
+    tf_listener_->waitForTransform(map_frame_, sensor_frame, sensor_time, ros::Duration(3.0));
+    pcl_ros::transformPointCloud(map_frame_, sensor_time, *sensorTF_clipping_height_cloud_ptr, sensor_frame,
+                                 *mapTF_cloud_ptr, *tf_listener_);
+  }
+  catch (tf::TransformException& ex)
+  {
+    ROS_ERROR("Transform error: %s", ex.what());
+    return;
+  }
+
+  pcl::PointCloud<pcl::PointXYZI>::Ptr mapTF_match_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+  pcl::PointCloud<pcl::PointXYZI>::Ptr mapTF_unmatch_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+  searchMatchingCloud(mapTF_cloud_ptr, mapTF_match_cloud_ptr, mapTF_unmatch_cloud_ptr);
+
+  sensor_msgs::PointCloud2 mapTF_match_cloud_msg;
+  pcl::toROSMsg(*mapTF_match_cloud_ptr, mapTF_match_cloud_msg);
+  mapTF_match_cloud_msg.header.stamp = sensor_time;
+  mapTF_match_cloud_msg.header.frame_id = map_frame_;
+  mapTF_match_cloud_msg.fields = sensorTF_cloud_msg_ptr->fields;
+
+  sensor_msgs::PointCloud2 sensorTF_match_cloud_msg;
+  try
+  {
+    pcl_ros::transformPointCloud(sensor_frame, mapTF_match_cloud_msg, sensorTF_match_cloud_msg, *tf_listener_);
+  }
+  catch (tf::TransformException& ex)
+  {
+    ROS_ERROR("Transform error: %s", ex.what());
+    return;
+  }
+  match_points_pub_.publish(sensorTF_match_cloud_msg);
+
+  sensor_msgs::PointCloud2 mapTF_unmatch_cloud_msg;
+  pcl::toROSMsg(*mapTF_unmatch_cloud_ptr, mapTF_unmatch_cloud_msg);
+  mapTF_unmatch_cloud_msg.header.stamp = sensor_time;
+  mapTF_unmatch_cloud_msg.header.frame_id = map_frame_;
+  mapTF_unmatch_cloud_msg.fields = sensorTF_cloud_msg_ptr->fields;
+
+  sensor_msgs::PointCloud2 sensorTF_unmatch_cloud_msg;
+  try
+  {
+    pcl_ros::transformPointCloud(sensor_frame, mapTF_unmatch_cloud_msg, sensorTF_unmatch_cloud_msg, *tf_listener_);
+  }
+  catch (tf::TransformException& ex)
+  {
+    ROS_ERROR("Transform error: %s", ex.what());
+    return;
+  }
+  unmatch_points_pub_.publish(sensorTF_unmatch_cloud_msg);
+}
+
+void CompareMapFilter::searchMatchingCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr,
+                                           pcl::PointCloud<pcl::PointXYZI>::Ptr match_cloud_ptr,
+                                           pcl::PointCloud<pcl::PointXYZI>::Ptr unmatch_cloud_ptr)
+{
+  match_cloud_ptr->points.clear();
+  unmatch_cloud_ptr->points.clear();
+
+  match_cloud_ptr->points.reserve(in_cloud_ptr->points.size());
+  unmatch_cloud_ptr->points.reserve(in_cloud_ptr->points.size());
+
+  std::vector<int> nn_indices(1);
+  std::vector<float> nn_dists(1);
+  const double squared_distance_threshold = distance_threshold_ * distance_threshold_;
+
+  for (size_t i = 0; i < in_cloud_ptr->points.size(); ++i)
+  {
+    tree_.nearestKSearch(in_cloud_ptr->points[i], 1, nn_indices, nn_dists);
+    if (nn_dists[0] <= squared_distance_threshold)
+    {
+      match_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
+    }
+    else
+    {
+      unmatch_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
+    }
+  }
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "compare_map_filter");
+  CompareMapFilter node;
+  ros::spin();
+
+  return 0;
+}

src/ros/catkin/src/points_preprocessor/nodes/points_concat_filter/points_concat_filter.cpp

@@ -0,0 +1,143 @@
+/*
+ * 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 <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_ros/transforms.h>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <tf/tf.h>
+#include <tf/transform_listener.h>
+#include <velodyne_pointcloud/point_types.h>
+#include <yaml-cpp/yaml.h>
+
+class PointsConcatFilter
+{
+public:
+  PointsConcatFilter();
+
+private:
+  typedef pcl::PointXYZI PointT;
+  typedef pcl::PointCloud<PointT> PointCloudT;
+  typedef sensor_msgs::PointCloud2 PointCloudMsgT;
+  typedef message_filters::sync_policies::ApproximateTime<PointCloudMsgT, PointCloudMsgT, PointCloudMsgT,
+                                                          PointCloudMsgT, PointCloudMsgT, PointCloudMsgT,
+                                                          PointCloudMsgT, PointCloudMsgT>
+      SyncPolicyT;
+
+  ros::NodeHandle node_handle_, private_node_handle_;
+  message_filters::Subscriber<PointCloudMsgT> *cloud_subscribers_[8];
+  message_filters::Synchronizer<SyncPolicyT> *cloud_synchronizer_;
+  ros::Subscriber config_subscriber_;
+  ros::Publisher cloud_publisher_;
+  tf::TransformListener tf_listener_;
+
+  size_t input_topics_size_;
+  std::string input_topics_;
+  std::string output_frame_id_;
+
+  void pointcloud_callback(const PointCloudMsgT::ConstPtr &msg1, const PointCloudMsgT::ConstPtr &msg2,
+                           const PointCloudMsgT::ConstPtr &msg3, const PointCloudMsgT::ConstPtr &msg4,
+                           const PointCloudMsgT::ConstPtr &msg5, const PointCloudMsgT::ConstPtr &msg6,
+                           const PointCloudMsgT::ConstPtr &msg7, const PointCloudMsgT::ConstPtr &msg8);
+};
+
+PointsConcatFilter::PointsConcatFilter() : node_handle_(), private_node_handle_("~"), tf_listener_()
+{
+  private_node_handle_.param("input_topics", input_topics_, std::string("[/points_alpha, /points_beta]"));
+  private_node_handle_.param("output_frame_id", output_frame_id_, std::string("velodyne"));
+
+  YAML::Node topics = YAML::Load(input_topics_);
+  input_topics_size_ = topics.size();
+  if (input_topics_size_ < 2 || 8 < input_topics_size_)
+  {
+    ROS_ERROR("The size of input_topics must be between 2 and 8");
+    ros::shutdown();
+  }
+  for (size_t i = 0; i < 8; ++i)
+  {
+    if (i < input_topics_size_)
+    {
+      cloud_subscribers_[i] =
+          new message_filters::Subscriber<PointCloudMsgT>(node_handle_, topics[i].as<std::string>(), 1);
+    }
+    else
+    {
+      cloud_subscribers_[i] =
+          new message_filters::Subscriber<PointCloudMsgT>(node_handle_, topics[0].as<std::string>(), 1);
+    }
+  }
+  cloud_synchronizer_ = new message_filters::Synchronizer<SyncPolicyT>(
+      SyncPolicyT(10), *cloud_subscribers_[0], *cloud_subscribers_[1], *cloud_subscribers_[2], *cloud_subscribers_[3],
+      *cloud_subscribers_[4], *cloud_subscribers_[5], *cloud_subscribers_[6], *cloud_subscribers_[7]);
+  cloud_synchronizer_->registerCallback(
+      boost::bind(&PointsConcatFilter::pointcloud_callback, this, _1, _2, _3, _4, _5, _6, _7, _8));
+  cloud_publisher_ = node_handle_.advertise<PointCloudMsgT>("/points_concat", 1);
+}
+
+void PointsConcatFilter::pointcloud_callback(const PointCloudMsgT::ConstPtr &msg1, const PointCloudMsgT::ConstPtr &msg2,
+                                             const PointCloudMsgT::ConstPtr &msg3, const PointCloudMsgT::ConstPtr &msg4,
+                                             const PointCloudMsgT::ConstPtr &msg5, const PointCloudMsgT::ConstPtr &msg6,
+                                             const PointCloudMsgT::ConstPtr &msg7, const PointCloudMsgT::ConstPtr &msg8)
+{
+  assert(2 <= input_topics_size_ && input_topics_size_ <= 8);
+
+  PointCloudMsgT::ConstPtr msgs[8] = { msg1, msg2, msg3, msg4, msg5, msg6, msg7, msg8 };
+  PointCloudT::Ptr cloud_sources[8];
+  PointCloudT::Ptr cloud_concatenated(new PointCloudT);
+
+  // transform points
+  try
+  {
+    for (size_t i = 0; i < input_topics_size_; ++i)
+    {
+      // Note: If you use kinetic, you can directly receive messages as
+      // PointCloutT.
+      cloud_sources[i] = PointCloudT().makeShared();
+      pcl::fromROSMsg(*msgs[i], *cloud_sources[i]);
+      tf_listener_.waitForTransform(output_frame_id_, msgs[i]->header.frame_id, ros::Time(0), ros::Duration(1.0));
+      pcl_ros::transformPointCloud(output_frame_id_, ros::Time(0), *cloud_sources[i], msgs[i]->header.frame_id, *cloud_sources[i], tf_listener_);
+    }
+  }
+  catch (tf::TransformException &ex)
+  {
+    ROS_ERROR("%s", ex.what());
+    return;
+  }
+
+  // merge points
+  for (size_t i = 0; i < input_topics_size_; ++i)
+  {
+    *cloud_concatenated += *cloud_sources[i];
+  }
+
+  // publsh points
+  cloud_concatenated->header = pcl_conversions::toPCL(msgs[0]->header);
+  cloud_concatenated->header.frame_id = output_frame_id_;
+  cloud_publisher_.publish(cloud_concatenated);
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "points_concat_filter");
+  PointsConcatFilter node;
+  ros::spin();
+  return 0;
+}

src/ros/catkin/src/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp

@@ -0,0 +1,478 @@
+/*
+ * Copyright 2017-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.
+ ********************
+ *  v1.0: amc-nu (abrahammonrroy@yahoo.com)
+ */
+#include <iostream>
+#include <algorithm>
+#include <vector>
+#include <string>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_types.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/normal_3d_omp.h>
+#include <pcl/filters/extract_indices.h>
+#include <velodyne_pointcloud/point_types.h>
+#include "autoware_config_msgs/ConfigRayGroundFilter.h"
+
+#include <opencv2/core/version.hpp>
+#if (CV_MAJOR_VERSION == 3)
+#include "gencolors.cpp"
+#else
+#include <opencv2/contrib/contrib.hpp>
+#endif
+
+#include "points_preprocessor/ray_ground_filter/ray_ground_filter.h"
+
+void RayGroundFilter::update_config_params(const autoware_config_msgs::ConfigRayGroundFilter::ConstPtr& param)
+{
+  general_max_slope_ = param->general_max_slope;
+  local_max_slope_ = param->local_max_slope;
+  radial_divider_angle_ = param->radial_divider_angle;
+  concentric_divider_distance_ = param->concentric_divider_distance;
+  min_height_threshold_ = param->min_height_threshold;
+  clipping_height_ = param->clipping_height;
+  min_point_distance_ = param->min_point_distance;
+  reclass_distance_threshold_ = param->reclass_distance_threshold;
+}
+
+bool RayGroundFilter::TransformPointCloud(const std::string& in_target_frame,
+                                          const sensor_msgs::PointCloud2::ConstPtr& in_cloud_ptr,
+                                          const sensor_msgs::PointCloud2::Ptr& out_cloud_ptr)
+{
+
+  
+  if (in_target_frame == in_cloud_ptr->header.frame_id)
+  {
+    *out_cloud_ptr = *in_cloud_ptr;
+    return true;
+  }
+
+  geometry_msgs::TransformStamped transform_stamped;
+  try
+  {
+    transform_stamped = tf_buffer_.lookupTransform(in_target_frame, in_cloud_ptr->header.frame_id,
+                                                   in_cloud_ptr->header.stamp, ros::Duration(1.0));
+  }
+  catch (tf2::TransformException& ex)
+  {
+    ROS_WARN("%s", ex.what());
+    return false;
+  }
+  // tf2::doTransform(*in_cloud_ptr, *out_cloud_ptr, transform_stamped);
+  Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
+  pcl_ros::transformPointCloud(mat, *in_cloud_ptr, *out_cloud_ptr);
+  out_cloud_ptr->header.frame_id = in_target_frame;
+  return true;
+}
+
+void RayGroundFilter::publish_cloud(const ros::Publisher& in_publisher,
+                                    const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_to_publish_ptr,
+                                    const std_msgs::Header& in_header)
+{
+  sensor_msgs::PointCloud2::Ptr cloud_msg_ptr(new sensor_msgs::PointCloud2);
+  sensor_msgs::PointCloud2::Ptr trans_cloud_msg_ptr(new sensor_msgs::PointCloud2);
+  pcl::toROSMsg(*in_cloud_to_publish_ptr, *cloud_msg_ptr);
+  cloud_msg_ptr->header.frame_id = base_frame_;
+  cloud_msg_ptr->header.stamp = in_header.stamp;
+  const bool succeeded = TransformPointCloud(in_header.frame_id, cloud_msg_ptr, trans_cloud_msg_ptr);
+  if (!succeeded)
+  {
+    ROS_ERROR_STREAM_THROTTLE(10, "Failed transform from " << cloud_msg_ptr->header.frame_id << " to "
+                                                           << in_header.frame_id);
+    return;
+  }
+  in_publisher.publish(*trans_cloud_msg_ptr);
+}
+
+/*!
+ *
+ * @param[in] in_cloud Input Point Cloud to be organized in radial segments
+ * @param[out] out_organized_points Custom Point Cloud filled with XYZRTZColor data
+ * @param[out] out_radial_divided_indices Indices of the points in the original cloud for each radial segment
+ * @param[out] out_radial_ordered_clouds Vector of Points Clouds, each element will contain the points ordered
+ */
+void RayGroundFilter::ConvertXYZIToRTZColor(
+    const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud,
+    const std::shared_ptr<PointCloudXYZIRTColor>& out_organized_points,
+    const std::shared_ptr<std::vector<pcl::PointIndices> >& out_radial_divided_indices,
+    const std::shared_ptr<std::vector<PointCloudXYZIRTColor> >& out_radial_ordered_clouds)
+{
+  out_organized_points->resize(in_cloud->points.size());
+  out_radial_divided_indices->clear();
+  out_radial_divided_indices->resize(radial_dividers_num_);
+  out_radial_ordered_clouds->resize(radial_dividers_num_);
+
+  int nancount = 0;
+  for (size_t i = 0; i < in_cloud->points.size(); i++)
+  {
+   // std::cout<<" i : "<<i<<std::endl;
+    PointXYZIRTColor new_point;
+    auto radius = static_cast<float>(
+        sqrt(in_cloud->points[i].x * in_cloud->points[i].x + in_cloud->points[i].y * in_cloud->points[i].y));
+    //    std::cout<<" y : "<<in_cloud->points[i].y<<" x: "<<in_cloud->points[i].x<<std::endl;
+    auto theta = static_cast<float>(atan2(in_cloud->points[i].y, in_cloud->points[i].x) * 180 / M_PI);
+    if (theta < 0)
+    {
+      theta += 360;
+    }
+    if (theta >= 360)
+    {
+      theta -= 360;
+    }
+
+    if(std::isnan(theta))
+    {
+      nancount++;
+  //std::cout<<"nan point "<<std::endl;
+      continue;
+    }
+
+   // std::cout<<" theta "<<theta<<" angle : "<<radial_divider_angle_<<std::endl;
+
+    auto radial_div = (size_t)floor(theta / radial_divider_angle_);
+
+    auto concentric_div = (size_t)floor(fabs(radius / concentric_divider_distance_));
+
+   // std::cout<<" center  i  "<<i<<std::endl;
+
+    new_point.point = in_cloud->points[i];
+    new_point.radius = radius;
+    new_point.theta = theta;
+    new_point.radial_div = radial_div;
+    new_point.concentric_div = concentric_div;
+    new_point.red = (size_t)colors_[new_point.radial_div % color_num_].val[0];
+    new_point.green = (size_t)colors_[new_point.radial_div % color_num_].val[1];
+    new_point.blue = (size_t)colors_[new_point.radial_div % color_num_].val[2];
+    new_point.original_index = i;
+
+    out_organized_points->at(i) = new_point;
+
+  //  std::cout<<" size : "<<in_cloud->points.size()<<"  div "<<radial_div<<std::endl;
+    // radial divisions
+    out_radial_divided_indices->at(radial_div).indices.push_back(i);
+
+    out_radial_ordered_clouds->at(radial_div).push_back(new_point);
+   // std::cout<<" end i  "<<i<<std::endl;
+  }  // end for
+
+  std::cout<<"  nan point size : "<<nancount<<"  p size : " <<in_cloud->points.size()<<std::endl;
+// order radial points on each division
+#pragma omp for
+  for (size_t i = 0; i < radial_dividers_num_; i++)
+  {
+    std::sort(out_radial_ordered_clouds->at(i).begin(), out_radial_ordered_clouds->at(i).end(),
+              [](const PointXYZIRTColor& a, const PointXYZIRTColor& b) { return a.radius < b.radius; });  // NOLINT
+  }
+}
+
+/*!
+ * Classifies Points in the PointCoud as Ground and Not Ground
+ * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud ordered by radial distance from the origin
+ * @param out_ground_indices Returns the indices of the points classified as ground in the original PointCloud
+ * @param out_no_ground_indices Returns the indices of the points classified as not ground in the original PointCloud
+ */
+void RayGroundFilter::ClassifyPointCloud(const std::vector<PointCloudXYZIRTColor>& in_radial_ordered_clouds,
+                                         const pcl::PointIndices::Ptr& out_ground_indices,
+                                         const pcl::PointIndices::Ptr& out_no_ground_indices)
+{
+  out_ground_indices->indices.clear();
+  out_no_ground_indices->indices.clear();
+#pragma omp for
+  for (size_t i = 0; i < in_radial_ordered_clouds.size(); i++)  // sweep through each radial division
+  {
+    float prev_radius = 0.f;
+    float prev_height = 0.f;
+    bool prev_ground = false;
+    bool current_ground = false;
+    for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); j++)  // loop through each point in the radial div
+    {
+      float points_distance = in_radial_ordered_clouds[i][j].radius - prev_radius;
+      float height_threshold = tan(DEG2RAD(local_max_slope_)) * points_distance;
+      float current_height = in_radial_ordered_clouds[i][j].point.z;
+      float general_height_threshold = tan(DEG2RAD(general_max_slope_)) * in_radial_ordered_clouds[i][j].radius;
+
+      // for points which are very close causing the height threshold to be tiny, set a minimum value
+      if (points_distance > concentric_divider_distance_ && height_threshold < min_height_threshold_)
+      {
+        height_threshold = min_height_threshold_;
+      }
+
+      // check current point height against the LOCAL threshold (previous point)
+      if (current_height <= (prev_height + height_threshold) && current_height >= (prev_height - height_threshold))
+      {
+        // Check again using general geometry (radius from origin) if previous points wasn't ground
+        if (!prev_ground)
+        {
+          if (current_height <= general_height_threshold && current_height >= -general_height_threshold)
+          {
+            current_ground = true;
+          }
+          else
+          {
+            current_ground = false;
+          }
+        }
+        else
+        {
+          current_ground = true;
+        }
+      }
+      else
+      {
+        // check if previous point is too far from previous one, if so classify again
+        if (points_distance > reclass_distance_threshold_ &&
+            (current_height <= height_threshold && current_height >= -height_threshold))
+        {
+          current_ground = true;
+        }
+        else
+        {
+          current_ground = false;
+        }
+      }
+
+      if (current_ground)
+      {
+        out_ground_indices->indices.push_back(in_radial_ordered_clouds[i][j].original_index);
+        prev_ground = true;
+      }
+      else
+      {
+        out_no_ground_indices->indices.push_back(in_radial_ordered_clouds[i][j].original_index);
+        prev_ground = false;
+      }
+
+      prev_radius = in_radial_ordered_clouds[i][j].radius;
+      prev_height = in_radial_ordered_clouds[i][j].point.z;
+    }
+  }
+}
+
+/*!
+ * Removes the points higher than a threshold
+ * @param in_cloud_ptr PointCloud to perform Clipping
+ * @param in_clip_height Maximum allowed height in the cloud
+ * @param out_clipped_cloud_ptr Resultung PointCloud with the points removed
+ */
+void RayGroundFilter::ClipCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, const double in_clip_height,
+                                pcl::PointCloud<pcl::PointXYZI>::Ptr out_clipped_cloud_ptr)
+{
+  pcl::ExtractIndices<pcl::PointXYZI> extractor;
+  extractor.setInputCloud(in_cloud_ptr);
+  pcl::PointIndices indices;
+
+#pragma omp for
+  for (size_t i = 0; i < in_cloud_ptr->points.size(); i++)
+  {
+    if (in_cloud_ptr->points[i].z > in_clip_height)
+    {
+      indices.indices.push_back(i);
+    }
+  }
+  extractor.setIndices(boost::make_shared<pcl::PointIndices>(indices));
+  extractor.setNegative(true);  // true removes the indices, false leaves only the indices
+  extractor.filter(*out_clipped_cloud_ptr);
+}
+
+/*!
+ * Returns the resulting complementary PointCloud, one with the points kept and the other removed as indicated
+ * in the indices
+ * @param in_cloud_ptr Input PointCloud to which the extraction will be performed
+ * @param in_indices Indices of the points to be both removed and kept
+ * @param out_only_indices_cloud_ptr Resulting PointCloud with the indices kept
+ * @param out_removed_indices_cloud_ptr Resulting PointCloud with the indices removed
+ */
+void RayGroundFilter::ExtractPointsIndices(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr,
+                                           const pcl::PointIndices& in_indices,
+                                           pcl::PointCloud<pcl::PointXYZI>::Ptr out_only_indices_cloud_ptr,
+                                           pcl::PointCloud<pcl::PointXYZI>::Ptr out_removed_indices_cloud_ptr)
+{
+  pcl::ExtractIndices<pcl::PointXYZI> extract_ground;
+  extract_ground.setInputCloud(in_cloud_ptr);
+  extract_ground.setIndices(boost::make_shared<pcl::PointIndices>(in_indices));
+
+  extract_ground.setNegative(false);  // true removes the indices, false leaves only the indices
+  extract_ground.filter(*out_only_indices_cloud_ptr);
+
+  extract_ground.setNegative(true);  // true removes the indices, false leaves only the indices
+  extract_ground.filter(*out_removed_indices_cloud_ptr);
+}
+
+/*!
+ * Removes points up to a certain distance in the XY Plane
+ * @param in_cloud_ptr Input PointCloud
+ * @param in_min_distance Minimum valid distance, points closer than this will be removed.
+ * @param out_filtered_cloud_ptr Resulting PointCloud with the invalid points removed.
+ */
+void RayGroundFilter::RemovePointsUpTo(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr, double in_min_distance,
+                                       pcl::PointCloud<pcl::PointXYZI>::Ptr out_filtered_cloud_ptr)
+{
+  pcl::ExtractIndices<pcl::PointXYZI> extractor;
+  extractor.setInputCloud(in_cloud_ptr);
+  pcl::PointIndices indices;
+
+#pragma omp for
+  for (size_t i = 0; i < in_cloud_ptr->points.size(); i++)
+  {
+    if (sqrt(in_cloud_ptr->points[i].x * in_cloud_ptr->points[i].x +
+             in_cloud_ptr->points[i].y * in_cloud_ptr->points[i].y) < in_min_distance)
+    {
+      indices.indices.push_back(i);
+    }
+  }
+  extractor.setIndices(boost::make_shared<pcl::PointIndices>(indices));
+  extractor.setNegative(true);  // true removes the indices, false leaves only the indices
+  extractor.filter(*out_filtered_cloud_ptr);
+}
+
+void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud)
+{
+  
+   std::cout<<" call bacck frame id: "<<in_sensor_cloud->header.frame_id<<std::endl;
+  health_checker_ptr_->NODE_ACTIVATE();
+  health_checker_ptr_->CHECK_RATE("topic_rate_points_raw_slow", 8, 5, 1, "topic points_raw subscribe rate slow.");
+
+  sensor_msgs::PointCloud2::Ptr trans_sensor_cloud(new sensor_msgs::PointCloud2);
+  const bool succeeded = TransformPointCloud(base_frame_, in_sensor_cloud, trans_sensor_cloud);
+  if (!succeeded)
+  {
+    ROS_ERROR_STREAM_THROTTLE(10, "Failed transform from " << base_frame_ << " to "
+                                                           << in_sensor_cloud->header.frame_id);
+    return;
+  }
+
+  pcl::PointCloud<pcl::PointXYZI>::Ptr current_sensor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+  pcl::fromROSMsg(*trans_sensor_cloud, *current_sensor_cloud_ptr);
+
+  pcl::PointCloud<pcl::PointXYZI>::Ptr clipped_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+
+  // remove points above certain point
+  ClipCloud(current_sensor_cloud_ptr, clipping_height_, clipped_cloud_ptr);
+
+  // remove closer points than a threshold
+  pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+  RemovePointsUpTo(clipped_cloud_ptr, min_point_distance_, filtered_cloud_ptr);
+ 
+
+  // GetCloud Normals
+  // pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_with_normals_ptr (new pcl::PointCloud<pcl::PointXYZINormal>);
+  // GetCloudNormals(current_sensor_cloud_ptr, cloud_with_normals_ptr, 5.0);
+
+  std::shared_ptr<PointCloudXYZIRTColor> organized_points(new PointCloudXYZIRTColor);
+  std::shared_ptr<std::vector<pcl::PointIndices> > radial_division_indices(new std::vector<pcl::PointIndices>);
+  std::shared_ptr<std::vector<PointCloudXYZIRTColor> > radial_ordered_clouds(new std::vector<PointCloudXYZIRTColor>);
+
+  radial_dividers_num_ = ceil(360 / radial_divider_angle_);
+
+  ConvertXYZIToRTZColor(filtered_cloud_ptr, organized_points, radial_division_indices, radial_ordered_clouds);
+
+  pcl::PointIndices::Ptr ground_indices(new pcl::PointIndices), no_ground_indices(new pcl::PointIndices);
+
+  //std::cout<<" start class "<<std::endl;
+  ClassifyPointCloud(*radial_ordered_clouds, ground_indices, no_ground_indices);
+   //std::cout<<" throuh class "<<std::endl;
+
+  pcl::PointCloud<pcl::PointXYZI>::Ptr ground_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+  pcl::PointCloud<pcl::PointXYZI>::Ptr no_ground_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+
+  ExtractPointsIndices(filtered_cloud_ptr, *ground_indices, ground_cloud_ptr, no_ground_cloud_ptr);
+
+  publish_cloud(ground_points_pub_, ground_cloud_ptr, in_sensor_cloud->header);
+  publish_cloud(groundless_points_pub_, no_ground_cloud_ptr, in_sensor_cloud->header);
+}
+
+RayGroundFilter::RayGroundFilter() : node_handle_("~"), tf_listener_(tf_buffer_)
+
+{
+  ros::NodeHandle nh;
+  ros::NodeHandle pnh("~");
+  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh, pnh);
+  health_checker_ptr_->ENABLE();
+}
+
+void RayGroundFilter::Run()
+{
+  // Model   |   Horizontal   |   Vertical   | FOV(Vertical)    degrees / rads
+  // ----------------------------------------------------------
+  // HDL-64  |0.08-0.35(0.32) |     0.4      |  -24.9 <=x<=2.0   (26.9  / 0.47)
+  // HDL-32  |     0.1-0.4    |     1.33     |  -30.67<=x<=10.67 (41.33 / 0.72)
+  // VLP-16  |     0.1-0.4    |     2.0      |  -15.0<=x<=15.0   (30    / 0.52)
+  // VLP-16HD|     0.1-0.4    |     1.33     |  -10.0<=x<=10.0   (20    / 0.35)
+  ROS_INFO("Initializing Ground Filter, please wait...");
+  node_handle_.param<std::string>("input_point_topic", input_point_topic_, "/points_raw");
+  ROS_INFO("Input point_topic: %s", input_point_topic_.c_str());
+
+  node_handle_.param<std::string>("base_frame", base_frame_, "base_link");
+ // base_frame_ = "hello";
+  ROS_INFO("base_frame: %s", base_frame_.c_str());
+
+  node_handle_.param("general_max_slope", general_max_slope_, 3.0);
+  ROS_INFO("general_max_slope[deg]: %f", general_max_slope_);
+
+  node_handle_.param("local_max_slope", local_max_slope_, 5.0);
+  ROS_INFO("local_max_slope[deg]: %f", local_max_slope_);
+
+  node_handle_.param("radial_divider_angle", radial_divider_angle_, 0.1);  // 1 degree default
+  radial_divider_angle_ = 0.3;
+  ROS_INFO("radial_divider_angle[deg]: %f", radial_divider_angle_);
+  node_handle_.param("concentric_divider_distance", concentric_divider_distance_, 0.0);  // 0.0 meters default
+  ROS_INFO("concentric_divider_distance[meters]: %f", concentric_divider_distance_);
+ // node_handle_.param("min_height_threshold", min_height_threshold_, 0.05);  // 0.05 meters default
+   node_handle_.param("min_height_threshold", min_height_threshold_, -1.6);  // 0.05 meters default
+   min_height_threshold_ = -1.6;
+  ROS_INFO("min_height_threshold[meters]: %f", min_height_threshold_);
+ // node_handle_.param("clipping_height", clipping_height_, 2.0);  // 2.0 meters default above the car
+  node_handle_.param("clipping_height", clipping_height_, 0.3);  // 2.0 meters default above the car
+  clipping_height_ = 6.3;
+  ROS_INFO("clipping_height[meters]: %f", clipping_height_);
+  node_handle_.param("min_point_distance", min_point_distance_, 1.85);  // 1.85 meters default
+  ROS_INFO("min_point_distance[meters]: %f", min_point_distance_);
+  node_handle_.param("reclass_distance_threshold", reclass_distance_threshold_, 0.2);  // 0.5 meters default
+  ROS_INFO("reclass_distance_threshold[meters]: %f", reclass_distance_threshold_);
+
+
+#if (CV_MAJOR_VERSION == 3)
+  generateColors(colors_, color_num_);
+#else
+  cv::generateColors(colors_, color_num_);
+#endif
+
+  radial_dividers_num_ = ceil(360 / radial_divider_angle_);
+  ROS_INFO("Radial Divisions: %d", (int)radial_dividers_num_);
+
+  std::string no_ground_topic, ground_topic;
+  node_handle_.param<std::string>("no_ground_point_topic", no_ground_topic, "/points_no_ground");
+  ROS_INFO("No Ground Output Point Cloud no_ground_point_topic: %s", no_ground_topic.c_str());
+  node_handle_.param<std::string>("ground_point_topic", ground_topic, "/points_ground");
+  ROS_INFO("Only Ground Output Point Cloud ground_topic: %s", ground_topic.c_str());
+
+  ROS_INFO("Subscribing to... %s", input_point_topic_.c_str());
+  points_node_sub_ = node_handle_.subscribe(input_point_topic_, 1, &RayGroundFilter::CloudCallback, this);
+
+  config_node_sub_ =
+      node_handle_.subscribe("/config/ray_ground_filter", 1, &RayGroundFilter::update_config_params, this);
+
+  groundless_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(no_ground_topic, 2);
+  ground_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(ground_topic, 2);
+
+  ROS_INFO("Ready");
+
+  ros::spin();
+}

src/ros/catkin/src/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter_main.cpp

@@ -0,0 +1,30 @@
+/*
+ * Copyright 2017-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.
+ ********************
+ * amc-nu (abrahammonrroy@yahoo.com)
+ */
+
+#include <ros/ros.h>
+#include "points_preprocessor/ray_ground_filter/ray_ground_filter.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "ray_ground_filter");
+  RayGroundFilter app;
+
+  app.Run();
+
+  return 0;
+}

src/ros/catkin/src/points_preprocessor/nodes/ring_ground_filter/README.md

@@ -0,0 +1,27 @@
+# Some Guidelines for Tuning the Ring Ground Filter
+
+Author	: Patiphon Narksri
+
+*Problem:* Some parts of vertical objects are detected as ground points.
+
+*FIX:* One workaround for this is to set the "clipping_threshold" parameter in the launch file.
+	  By setting this threshold, any points higher than this threshold will be detected as vertical points.
+	  However, because all points that are higher than the threshold will be detected as vertical points, slopes might be incorrectly detected as vertical points as well.
+
+---
+
+*Problem:* Some small objects like curbs are missing.
+*FIX:* Try to lower the "gap_thres" parameter.
+	  However, lowering this parameter too much might result in mis-detecting slopes as vertical points.
+	  Usually, 0.15 - 0.2 is enough for detecting curbs.
+
+---
+
+*Problem:* Ring shaped noise (ground points being detected as vertical points) occurs nearby the vehicle.
+*FIX:* Try to lower the "points_distance" parameter.
+	  However, lowering this parameter too much might result in mis-detecting vertical objects which are far away from the vehicle as ground points.
+
+---
+
+*Problem:* Line shaped noise (in radial direction) occurs near edges of vertical objects.
+*FIX:* Decrease the "min_points" parameter. However, by doing so, some parts of vertical objects will be mis-detected as ground points.

src/ros/catkin/src/points_preprocessor/nodes/ring_ground_filter/ring_ground_filter.cpp

@@ -0,0 +1,352 @@
+/*
+ * ring_ground_filter.cpp
+ *
+ * Created on  : June 5, 2018
+ * Author  : Patiphon Narksri
+ *
+ */
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_types.h>
+#include <velodyne_pointcloud/point_types.h>
+#include <opencv/cv.h>
+
+enum Label
+{
+  GROUND,
+  VERTICAL,
+  UNKNOWN //Initial state, not classified
+};
+
+class GroundFilter
+{
+public:
+
+  GroundFilter();
+
+private:
+
+  ros::NodeHandle node_handle_;
+  ros::Subscriber points_node_sub_;
+  ros::Publisher groundless_points_pub_;
+  ros::Publisher ground_points_pub_;
+
+  std::string point_topic_;
+  std::string no_ground_topic, ground_topic;
+  int     sensor_model_;
+  double     sensor_height_;
+  double     max_slope_;
+  double vertical_thres_;
+  bool    floor_removal_;
+
+  int     vertical_res_;
+  int     horizontal_res_;
+  cv::Mat   index_map_;
+  Label     class_label_[64];
+  double  radius_table_[64];
+
+  //boost::chrono::high_resolution_clock::time_point t1_;
+  //boost::chrono::high_resolution_clock::time_point t2_;
+  //boost::chrono::nanoseconds elap_time_;
+  ros::Time t1_;
+  ros::Time t2_;
+  ros::Duration elap_time_;
+
+  const int   DEFAULT_HOR_RES = 2000;
+
+  void InitLabelArray(int in_model);
+  void InitRadiusTable(int in_model);
+  void InitDepthMap(int in_width);
+  void VelodyneCallback(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &in_cloud_msg);
+  void FilterGround(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &in_cloud_msg,
+        pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &out_groundless_points,
+        pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &out_ground_points);
+
+};
+
+GroundFilter::GroundFilter() : node_handle_("~")
+{
+  ROS_INFO("Inititalizing Ground Filter...");
+  node_handle_.param<std::string>("point_topic", point_topic_, "/points_raw");
+  ROS_INFO("Input Point Cloud: %s", point_topic_.c_str());
+   node_handle_.param("remove_floor",  floor_removal_,  true);
+   ROS_INFO("Floor Removal: %d", floor_removal_);
+  node_handle_.param("sensor_model", sensor_model_, 64);
+  ROS_INFO("Sensor Model: %d", sensor_model_);
+  node_handle_.param("sensor_height", sensor_height_, 1.80);
+  ROS_INFO("Sensor Height: %f", sensor_height_);
+  node_handle_.param("max_slope", max_slope_, 10.0);
+  ROS_INFO("Max Slope: %f", max_slope_);
+  node_handle_.param("vertical_thres", vertical_thres_, 0.08);
+  ROS_INFO("Vertical Threshold: %f", vertical_thres_);
+
+  node_handle_.param<std::string>("no_ground_point_topic", no_ground_topic, "/points_no_ground");
+  ROS_INFO("No Ground Output Point Cloud: %s", no_ground_topic.c_str());
+  node_handle_.param<std::string>("ground_point_topic", ground_topic, "/points_ground");
+  ROS_INFO("Only Ground Output Point Cloud: %s", ground_topic.c_str());
+
+
+  int default_horizontal_res;
+  switch(sensor_model_)
+  {
+    case 64:
+      default_horizontal_res = 2083;
+      break;
+    case 32:
+      default_horizontal_res = 2250;
+      break;
+    case 16:
+      default_horizontal_res = 1800;
+      break;
+    default:
+      default_horizontal_res = DEFAULT_HOR_RES;
+      break;
+  }
+  node_handle_.param("horizontal_res", horizontal_res_, default_horizontal_res);
+
+  points_node_sub_ = node_handle_.subscribe(point_topic_, 10000, &GroundFilter::VelodyneCallback, this);
+  groundless_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(no_ground_topic, 10000);
+  ground_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(ground_topic, 10000);
+
+  vertical_res_ = sensor_model_;
+  InitLabelArray(sensor_model_);
+  InitRadiusTable(sensor_model_);
+}
+
+void GroundFilter::InitLabelArray(int in_model)
+{
+  for(int a = 0; a < vertical_res_; a++)
+  {
+    class_label_[a] = UNKNOWN;
+  }
+}
+
+void GroundFilter::InitRadiusTable(int in_model)
+{
+  double a;
+  double b;
+  double theta;
+  switch (in_model)
+  {
+    case 64:
+      a = 1.0/3*M_PI/180;
+      b = max_slope_*M_PI/180;
+      for (int i = 0; i < 64; i++)
+      {
+        if (i <= 31)
+        {
+          if (i == 31) a = -a;
+          theta = (1.0/3*i - 2.0)*M_PI/180;
+          radius_table_[i] = fabs(sensor_height_*(1.0/(tan(theta)+tan(b)) - 1.0/(tan(a+theta)+tan(b))));
+        }
+        else
+        {
+          a = 0.5*M_PI/180;
+          theta = (8.83 + (0.5)*(i - 32.0))*M_PI/180;
+          radius_table_[i] = fabs(sensor_height_*(1.0/(tan(theta)+tan(b)) - 1.0/(tan(a+theta)+tan(b))));
+        }
+      }
+      break;
+    case 32:
+      a = 4.0/3*M_PI/180;
+      b = max_slope_*M_PI/180;
+      for (int i = 0; i < 32; i++)
+      {
+        theta = (-31.0/3 + (4.0/3)*i)*180/M_PI;
+        radius_table_[i] = fabs(sensor_height_*(1.0/(tan(theta)+tan(b)) - 1.0/(tan(a+theta)+tan(b))));
+      }
+      break;
+    case 16:
+      a = 2.0*M_PI/180;
+      b = max_slope_*M_PI/180;
+      for (int i = 0; i < 16; i++)
+      {
+        theta = (-30.0/2 + (2.0)*i)*180/M_PI;
+        radius_table_[i] = fabs(sensor_height_*(1.0/(tan(theta)+tan(b)) - 1.0/(tan(a+theta)+tan(b))));
+      }
+      break;
+    default:
+      a = 1.0/3*M_PI/180;
+      b = max_slope_*M_PI/180;
+      for (int i = 0; i < 64; i++)
+      {
+        if (i <= 31)
+        {
+          if (i == 31) a = -a;
+          theta = (1.0/3*i - 2.0)*M_PI/180;
+          radius_table_[i] = fabs(sensor_height_*(1.0/(tan(theta)+tan(b)) - 1.0/(tan(a+theta)+tan(b))));
+        }
+        else
+        {
+          a = 0.5*M_PI/180;
+          theta = (8.83 + (0.5)*(i - 32.0))*M_PI/180;
+          radius_table_[i] = fabs(sensor_height_*(1.0/(tan(theta)+tan(b)) - 1.0/(tan(a+theta)+tan(b))));
+        }
+      }
+      break;
+  }
+}
+
+void GroundFilter::InitDepthMap(int in_width)
+{
+  const int mOne = -1;
+  index_map_ = cv::Mat_<int>(vertical_res_, in_width, mOne);
+}
+
+void GroundFilter::FilterGround(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &in_cloud_msg,
+      pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &out_groundless_points,
+      pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &out_ground_points)
+{
+
+  velodyne_pointcloud::PointXYZIR point;
+  InitDepthMap(horizontal_res_);
+
+  for (size_t i = 0; i < in_cloud_msg->points.size(); i++)
+  {
+    double u = atan2(in_cloud_msg->points[i].y,in_cloud_msg->points[i].x) * 180/M_PI;
+    if (u < 0) { u = 360 + u; }
+    int column = horizontal_res_ - (int)((double)horizontal_res_ * u / 360.0) - 1;
+    int row = vertical_res_ - 1 - in_cloud_msg->points[i].ring;
+    index_map_.at<int>(row, column) = i;
+  }
+
+  for (int i = 0; i < horizontal_res_; i++)
+  {
+    Label point_class[vertical_res_];
+    int point_index[vertical_res_];
+    int point_index_size = 0;
+    double z_max = 0;
+    double z_min = 0;
+    double r_ref = 0;
+    std::copy(class_label_, class_label_ + vertical_res_, point_class);
+    for (int j = 0; j < vertical_res_; j++)
+    {
+      if (index_map_.at<int>(j,i) > -1 && point_class[j] == UNKNOWN)
+      {
+        double x0 = in_cloud_msg->points[index_map_.at<int>(j, i)].x;
+        double y0 = in_cloud_msg->points[index_map_.at<int>(j, i)].y;
+        double z0 = in_cloud_msg->points[index_map_.at<int>(j, i)].z;
+        double r0 = sqrt(x0*x0 + y0*y0);
+        double r_diff = fabs(r0 - r_ref);
+        if (r_diff < radius_table_[j] || r_ref == 0)
+        {
+          r_ref = r0;
+          if (z0 > z_max || r_ref == 0) z_max = z0;
+          if (z0 < z_min || r_ref == 0) z_min = z0;
+          point_index[point_index_size] = j;
+          point_index_size++;
+        }
+        else
+        {
+          if (point_index_size > 1 && (z_max - z_min) > vertical_thres_)
+          {
+            for (int m = 0; m < point_index_size; m++)
+            {
+              int index = index_map_.at<int>(point_index[m],i);
+              point.x = in_cloud_msg->points[index].x;
+              point.y = in_cloud_msg->points[index].y;
+              point.z = in_cloud_msg->points[index].z;
+              point.intensity = in_cloud_msg->points[index].intensity;
+              point.ring = in_cloud_msg->points[index].ring;
+              out_groundless_points.push_back(point);
+              point_class[point_index[m]] = VERTICAL;
+            }
+            point_index_size = 0;
+          }
+          else
+          {
+            for (int m = 0; m < point_index_size; m++)
+            {
+              int index = index_map_.at<int>(point_index[m],i);
+              point.x = in_cloud_msg->points[index].x;
+              point.y = in_cloud_msg->points[index].y;
+              point.z = in_cloud_msg->points[index].z;
+              point.intensity = in_cloud_msg->points[index].intensity;
+              point.ring = in_cloud_msg->points[index].ring;
+              out_ground_points.push_back(point);
+              point_class[point_index[m]] = GROUND;
+            }
+            point_index_size = 0;
+          }
+          r_ref = r0;
+          z_max = z0;
+          z_min = z0;
+          point_index[point_index_size] = j;
+          point_index_size++;
+        }
+      }
+      if (j == vertical_res_ - 1 && point_index_size != 0)
+      {
+          if (point_index_size > 1 && (z_max - z_min) > vertical_thres_)
+          {
+            for (int m = 0; m < point_index_size; m++)
+            {
+              int index = index_map_.at<int>(point_index[m],i);
+              point.x = in_cloud_msg->points[index].x;
+              point.y = in_cloud_msg->points[index].y;
+              point.z = in_cloud_msg->points[index].z;
+              point.intensity = in_cloud_msg->points[index].intensity;
+              point.ring = in_cloud_msg->points[index].ring;
+              out_groundless_points.push_back(point);
+              point_class[point_index[m]] = VERTICAL;
+            }
+            point_index_size = 0;
+          }
+          else
+          {
+            for (int m = 0; m < point_index_size; m++)
+            {
+              int index = index_map_.at<int>(point_index[m],i);
+              point.x = in_cloud_msg->points[index].x;
+              point.y = in_cloud_msg->points[index].y;
+              point.z = in_cloud_msg->points[index].z;
+              point.intensity = in_cloud_msg->points[index].intensity;
+              point.ring = in_cloud_msg->points[index].ring;
+              out_ground_points.push_back(point);
+              point_class[point_index[m]] = GROUND;
+            }
+            point_index_size = 0;
+          }
+      }
+    }
+  }
+}
+
+void GroundFilter::VelodyneCallback(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr &in_cloud_msg)
+{
+
+  //t1_ = ros::Time().now();
+  pcl::PointCloud<velodyne_pointcloud::PointXYZIR> vertical_points;
+  pcl::PointCloud<velodyne_pointcloud::PointXYZIR> ground_points;
+  vertical_points.header = in_cloud_msg->header;
+  ground_points.header = in_cloud_msg->header;
+  vertical_points.clear();
+  ground_points.clear();
+
+  FilterGround(in_cloud_msg, vertical_points, ground_points);
+
+  if (!floor_removal_)
+  {
+    vertical_points = *in_cloud_msg;
+  }
+
+  groundless_points_pub_.publish(vertical_points);
+  ground_points_pub_.publish(ground_points);
+  //t2_ = boost::chrono::high_resolution_clock::now();
+  //t2_ = ros::Time().now();
+  //elap_time_ = t2_ - t1_;//boost::chrono::duration_cast<boost::chrono::nanoseconds>(t2_-t1_);
+  //std::cout << "Computational Time for one frame: " << elap_time_ << '\n';
+}
+
+int main(int argc, char **argv)
+{
+
+  ros::init(argc, argv, "ring_ground_filter");
+  GroundFilter node;
+  ros::spin();
+
+  return 0;
+
+}

src/ros/catkin/src/points_preprocessor/nodes/space_filter/space_filter.cpp

@@ -0,0 +1,154 @@
+/*
+ * space_filter.cpp
+ *
+ *  Created on: Nov 4, 2016
+ *      Author: ne0
+ */
+#include <ros/ros.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/filters/extract_indices.h>
+#include <sensor_msgs/point_cloud_conversion.h>
+#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/PointCloud2.h>
+
+
+class SpaceFilter
+{
+public:
+  SpaceFilter();
+
+private:
+
+  ros::NodeHandle node_handle_;
+  ros::Subscriber cloud_sub_;
+  ros::Publisher   cloud_pub_;
+
+  std::string   subscribe_topic_;
+
+  bool      lateral_removal_;
+  bool      vertical_removal_;
+
+  double       left_distance_;
+  double       right_distance_;
+  double       below_distance_;
+  double       above_distance_;
+
+  void VelodyneCallback(const sensor_msgs::PointCloud2::Ptr& in_sensor_cloud_ptr);
+  void KeepLanes(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
+              pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+              float in_left_lane_threshold,
+              float in_right_lane_threshold);
+  void ClipCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
+              pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+              float in_min_height,
+              float in_max_height);
+};
+
+SpaceFilter::SpaceFilter() :
+    node_handle_("~")
+{
+
+  node_handle_.param<std::string>("subscribe_topic",  subscribe_topic_,  "/points_raw");
+
+  node_handle_.param("lateral_removal",  lateral_removal_,  true);
+  node_handle_.param("left_distance",  left_distance_,  5.0);
+  node_handle_.param("right_distance",  right_distance_,  5.0);
+
+  node_handle_.param("vertical_removal",  vertical_removal_,  true);
+  node_handle_.param("below_distance",  below_distance_,  -1.5);
+  node_handle_.param("above_distance",  above_distance_,  0.5);
+
+  cloud_sub_ = node_handle_.subscribe(subscribe_topic_, 10, &SpaceFilter::VelodyneCallback, this);
+  cloud_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>( "/points_clipped", 10);
+}
+
+void SpaceFilter::KeepLanes(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
+    pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+    float in_left_lane_threshold,
+    float in_right_lane_threshold)
+{
+  pcl::PointIndices::Ptr far_indices (new pcl::PointIndices);
+  for(unsigned int i=0; i< in_cloud_ptr->points.size(); i++)
+  {
+    pcl::PointXYZ current_point;
+    current_point.x=in_cloud_ptr->points[i].x;
+    current_point.y=in_cloud_ptr->points[i].y;
+    current_point.z=in_cloud_ptr->points[i].z;
+
+    if (
+      current_point.y > (in_left_lane_threshold) || current_point.y < -1.0*in_right_lane_threshold
+    )
+    {
+      far_indices->indices.push_back(i);
+    }
+  }
+  out_cloud_ptr->points.clear();
+  pcl::ExtractIndices<pcl::PointXYZ> extract;
+  extract.setInputCloud (in_cloud_ptr);
+  extract.setIndices(far_indices);
+  extract.setNegative(true);//true removes the indices, false leaves only the indices
+  extract.filter(*out_cloud_ptr);
+}
+
+void SpaceFilter::ClipCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
+    pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+    float in_min_height,
+    float in_max_height)
+{
+  out_cloud_ptr->points.clear();
+  for (unsigned int i=0; i<in_cloud_ptr->points.size(); i++)
+  {
+    if (in_cloud_ptr->points[i].z >= in_min_height &&
+        in_cloud_ptr->points[i].z <= in_max_height)
+    {
+      out_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
+    }
+  }
+}
+
+void SpaceFilter::VelodyneCallback(const sensor_msgs::PointCloud2::Ptr& in_sensor_cloud_ptr)
+{
+  pcl::PointCloud<pcl::PointXYZ>::Ptr current_sensor_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr inlanes_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr clipped_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
+
+  pcl::fromROSMsg(*in_sensor_cloud_ptr, *current_sensor_cloud_ptr);
+
+  if (lateral_removal_)
+  {
+    KeepLanes(current_sensor_cloud_ptr, inlanes_cloud_ptr, left_distance_, right_distance_);
+  }
+  else
+  {
+    inlanes_cloud_ptr = current_sensor_cloud_ptr;
+  }
+  if (vertical_removal_)
+  {
+    ClipCloud(inlanes_cloud_ptr, clipped_cloud_ptr, below_distance_, above_distance_);
+  }
+  else
+  {
+    clipped_cloud_ptr = inlanes_cloud_ptr;
+  }
+
+  sensor_msgs::PointCloud2 cloud_msg;
+
+  pcl::toROSMsg(*clipped_cloud_ptr, cloud_msg);
+
+  cloud_msg.header=in_sensor_cloud_ptr->header;
+  cloud_pub_.publish(cloud_msg);
+}
+
+int main(int argc, char **argv)
+{
+
+  ros::init(argc, argv, "space_filter");
+  SpaceFilter node;
+  ros::spin();
+
+  return 0;
+}
+
+
+
+

src/ros/catkin/src/points_preprocessor/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>points_preprocessor</name>
+  <version>1.12.0</version>
+  <description>The points_preprocessor package</description>
+  <maintainer email="abrahammonrroy@yahoo.com">amc-nu</maintainer>
+  <author email="n.patiphon@gmail.com">n-patiphon</author>
+  <author email="akihito.ohsato@tier4.jp">aohsato</author>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roslint</depend>
+  <depend>autoware_config_msgs</depend>
+  <depend>autoware_health_checker</depend>
+  <depend>cv_bridge</depend>
+  <depend>gtest</depend>
+  <depend>message_filters</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>roscpp</depend>
+  <depend>rostest</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_eigen</depend>
+  <depend>velodyne_pointcloud</depend>
+  <depend>yaml-cpp</depend>
+</package>

src/ros/catkin/src/points_preprocessor/test/include/test_ray_groundfilter.h

@@ -0,0 +1,55 @@
+
+#include <cmath>
+
+#include <ros/ros.h>
+
+#include "ray_ground_filter.h"
+
+// test fixtures are necessary to use friend classes
+TEST(RayGroundFilter, clipCloud)
+{
+  char* argv = "test_points_preprocessor";
+  int argc = 1;
+  ros::init(argc, &argv, "test_raygroundfilter_clipcloud");
+  pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+  pcl::PointCloud<pcl::PointXYZI>::Ptr out_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+
+  RayGroundFilter rgfilter;
+
+  // add a few test points
+  pcl::PointXYZI pt; 
+  pt.x = 0.0F; pt.y = 0.0F; pt.z = 1.0F;
+  in_cloud_ptr->push_back(pt);
+  pt.x = 0.0F; pt.y = 1.0F; pt.z = 1.3F;
+  in_cloud_ptr->push_back(pt);
+  pt.x = 0.0F; pt.y = 2.0F; pt.z = 1.5F;
+  in_cloud_ptr->push_back(pt);
+  pt.x = 0.0F; pt.y = 3.0F; pt.z = 1.7F;
+  in_cloud_ptr->push_back(pt);
+  pt.x = 0.0F; pt.y = 4.0F; pt.z = 1.9F;
+  in_cloud_ptr->push_back(pt);
+  pt.x = 0.0F; pt.y = 5.0F; pt.z = 1.7F;
+  in_cloud_ptr->push_back(pt);
+  pt.x = 0.0F; pt.y = 6.0F; pt.z = 1.5F;
+  in_cloud_ptr->push_back(pt);
+
+  // clip cloud
+  const float CLIP_HEIGHT = 1.5F;
+  rgfilter.ClipCloud(in_cloud_ptr, CLIP_HEIGHT, out_cloud_ptr);
+
+  // make sure everything worked correctly
+  ASSERT_EQ(out_cloud_ptr->points.size(), 4);
+  const float TOL = 1.0E-6F;
+  ASSERT_LT(fabsf(out_cloud_ptr->points[0].x), TOL);
+  ASSERT_LT(fabsf(out_cloud_ptr->points[0].y), TOL);
+  ASSERT_LT(fabsf(out_cloud_ptr->points[0].z - 1.0F), TOL);
+  ASSERT_LT(fabsf(out_cloud_ptr->points[1].x), TOL);
+  ASSERT_LT(fabsf(out_cloud_ptr->points[1].y - 1.0F), TOL);
+  ASSERT_LT(fabsf(out_cloud_ptr->points[1].z - 1.3F), TOL);
+  ASSERT_LT(fabsf(out_cloud_ptr->points[2].x), TOL);
+  ASSERT_LT(fabsf(out_cloud_ptr->points[2].y - 2.0F), TOL);
+  ASSERT_LT(fabsf(out_cloud_ptr->points[2].z - 1.5F), TOL);
+  ASSERT_LT(fabsf(out_cloud_ptr->points[3].x), TOL);
+  ASSERT_LT(fabsf(out_cloud_ptr->points[3].y - 6.0F), TOL);
+  ASSERT_LT(fabsf(out_cloud_ptr->points[3].z - 1.5F), TOL);
+}

src/ros/catkin/src/points_preprocessor/test/src/test_points_preprocessor.cpp

@@ -0,0 +1,10 @@
+
+#include "gtest/gtest.h"
+
+#include "test_ray_groundfilter.h"
+
+int32_t main(int32_t argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/points_preprocessor/test/test_points_preprocessor.test

@@ -0,0 +1,17 @@
+<!-- -*- mode: XML -*- -->
+<!-- rostest of the velodyne_laserscan system -->
+
+<launch>
+
+  <!-- Select log or screen output -->
+  <arg name="output" default="log"/> <!-- screen/log -->
+
+  <!-- Start the laserscan node -->
+  <node pkg="points_preprocessor" type="ray_ground_filter" name="ray_ground_filter" output="$(arg output)" />
+
+  <!-- Start the rostest -->
+  <test test-name="test_points_preprocessor" pkg="points_preprocessor"
+        type="test_raygroundfilter_clipcloud" name="test_ray_ground_filter">
+  </test>
+
+</launch>

src/ros/catkin/src/waypoint_planner/CHANGELOG.rst

@@ -0,0 +1,266 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package waypoint_planner
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* [Feature] Improve Hybrid A* planner (`#1594 <https://github.com/CPFL/Autoware/issues/1594>`_)
+  * Delete obstacle_sim from astar_planner package, replaced to lidar_fake_perception
+  * Modify package name, astar_planner -> waypoint_planner, and create astar_planner library package
+  * Delete obstacle_avoid/astar* and modify its dependency to astar_planner library
+  * Fix astar_navi with astar_planner library
+  * Refactor astar_navi by separating HAstar library and fixing coodinate system
+  * Rename obstacle_avoid -> astar_avoid and under refactoring
+  * Fix cost function and configures
+  * Fix backward search and refactor configurations
+  * Apply clang-format
+  * Refactor include
+  * Fix typo and so on
+  * Improve astar_avoid by incremental goal search
+  * Apply clang-format
+  * Revert package names
+  * Fix package/code names
+  * Update runtime_manager
+  * Improve astar_avoid to execute avoidance behavior by state transition (by rebuild decision maker)
+  * Fix PascalCase message names by `#1408 <https://github.com/CPFL/Autoware/issues/1408>`_
+  * Remove obstacle_avoid directory
+  * Fix default parameter for costmap topic
+  * Fix warning and initialize condition
+  * Remove use_avoidance_state mode (TODO: after merging rebuild decision maker)
+  * Improve astar_avoid behavior by simple state transition and multi-threading
+  * Apply clang-format
+  * Fix replan_interval in astar_avoid
+  * Add descriptions for paramters
+  * Rename pkg name, astar_planner -> waypoint_planner
+  * Fix param name
+  * Fix avoid waypoints height
+  * Fix parameter and formalize code
+  * Add README for freespace/waypoint_planner
+  * Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Fix CHANGELOG
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  Add License terms
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  * Fix astar_navi/README.md
+  * Add License terms
+  * Fix const pointer
+  * Added unit test base
+  * Restructured folders
+  * Fix bug by adding AstarSearch reset
+  * Fix WaveFrontNode initialization
+  Co-Authored-By: aohsato <aohsato@gmail.com>
+  * Fix variable name
+  * Refactor threading
+  * Re-adding lidar_fake_perception
+  * Fix the condition to judge reaching goal
+  * Add 'use_decision state' mode to transit avoidance state by decision_maker
+  * Fix calcDiffOfRadian (if diff > 2pi)
+  * Feature/test astar planner (`#1753 <https://github.com/CPFL/Autoware/issues/1753>`_)
+  * Restructured folders
+  * Added unit test base
+  * Removed remaining folder
+  * Test WIP
+  * Added astar_util tests and base file for astar_search tests
+  * Updated to ROS Cpp Style guidelines
+  * Added test for SimpleNode constructor
+  * Updated Copyright date
+  * Added tests for astar algorithm
+  * Added default constructor to WaveFront struct
+  * Revert use_state_decision mode (94af7b6)
+  * Fix costmap topic names by merging costmap_generator
+* Contributors: Akihito Ohsato
+
+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)
+------------------
+* 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
+* Add 'EControl::STOPLINE' state and detected object type 'EObstacleType' to change speed planning
+* Separate configration for speed planning against obstacle/stopline (Note: no logics changed)
+* Fix checking input for detection in velocity_set::pointsDetection
+* VelocitySet support to stop by stopline
+* Contributors: Akihito Ohsato, Kosuke Murakami, Yusuke FUJII
+
+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
+* Update velocity_set.launch
+  set value to false
+* fix video settings
+* launch file missing parameter setting.
+  missing parameter setting, when launch motion plan, report error :enablePlannerDynamicSwitch arg to be set.
+  see :  https://github.com/CPFL/Autoware/issues/871
+* Contributors: Yamato ANDO, asimay, hironari.yashiro
+
+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 web ui
+* Change parameter for obstacle avoid
+* Change color of a simulated obstacle
+* fix a planner selector
+  - lane select got to be able to change topicname for planner selector
+* Add changing topic name option for the planner selector.
+* fix segv
+* Add feature to put simulated obstacles in astar planner
+* R.I.P.
+* apply clang-format
+* add multiple crosswalk detection
+* Change the method to decide stop point
+* Fix indentation
+* Add parameter to ignore points nearby the vehicle
+* Contributors: TomohitoAndo, Yusuke FUJII, hironari.yashiro
+
+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)
+------------------
+* fix build issues due to autoware_msgs
+* Apply clang-formt
+* Add obstacle avoid feature in waypoint_planner
+* convert to autoware_msgs
+* Contributors: TomohitoAndo, YamatoAndo, Yusuke FUJII
+
+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)
+------------------------
+* Fix incorrect check for the waypoint index
+* Contributors: TomohitoAndo
+
+1.1.0 (2017-02-24)
+------------------
+* Use enum class instead of enum
+* improve deceleratiion for obstacles
+* Make function names more concise
+* Decide the number of zero velocity from the position of the obstacle
+* Avoid sudden acceleration after changing waypoints for deceleration
+* Remove unnecessary calcalation
+* Add get size method for new waypoints
+* Fix typo
+* improve acceleration
+* Use integer size with temporal waypoints
+* Avoid sudden aceleration after changing waypoints
+* Remove unnecessary comments
+* Remove unnecessary include
+* Remove unnecessary comment
+* Comment out publishing of the obstacle marker
+* Make constans all capitals
+* Make the function more concise
+* Use local variables for publishers
+* Implement callbacks in class
+* Use local variables instead of global variables
+* Remove the dependency of libvelocity_set
+* Use constexpr for constant values
+* Make obstacle detection function more concise
+* Modify variable names
+* Remove ignore range
+* Don't use call by reference with primitive data types
+* Remove unused variables
+* Fix dependencies
+* Remove unused function
+* Format comments
+* Split class into separate files
+* Subscribe closest waypoint
+* Contributors: TomohitoAndo
+
+1.0.1 (2017-01-14)
+------------------
+
+1.0.0 (2016-12-22)
+------------------
+* Ignore close points
+* Stop publishing obstacle marker
+* Use the result of lidar_tracker
+* Change launch file to output log
+* Fix license
+* Remove needless dependencies
+* Remove comments
+* Separate motion planning package
+* Contributors: TomohitoAndo, pdsljp

src/ros/catkin/src/waypoint_planner/CMakeLists.txt

@@ -0,0 +1,101 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(waypoint_planner)
+
+set(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
+
+find_package(autoware_build_flags REQUIRED)
+
+find_package(
+  catkin REQUIRED COMPONENTS
+    astar_search
+    autoware_config_msgs
+    autoware_health_checker
+    autoware_msgs
+    lanelet2_extension
+    libwaypoint_follower
+    pcl_conversions
+    pcl_ros
+    roscpp
+    roslint
+    std_msgs
+    tf
+    vector_map
+)
+
+catkin_package()
+
+# TODO add all codes to roslint
+roslint_cpp(src/velocity_set_lanelet2/velocity_set_lanelet2.cpp)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(
+  astar_avoid
+  src/astar_avoid/astar_avoid.cpp
+  src/astar_avoid/astar_avoid_node.cpp
+)
+
+target_link_libraries(
+  astar_avoid
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(
+  astar_avoid
+  ${catkin_EXPORTED_TARGETS}
+)
+
+add_executable(
+  velocity_set
+  src/velocity_set/velocity_set.cpp
+  src/velocity_set/velocity_set_path.cpp
+  src/velocity_set/velocity_set_info.cpp
+  src/velocity_set/libvelocity_set.cpp
+)
+
+target_link_libraries(
+  velocity_set
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(
+  velocity_set
+  ${catkin_EXPORTED_TARGETS}
+)
+
+add_executable(
+  velocity_set_lanelet2
+  src/velocity_set_lanelet2/velocity_set_lanelet2.cpp
+  src/velocity_set/velocity_set_path.cpp
+  src/velocity_set/velocity_set_info.cpp
+  src/velocity_set/libvelocity_set.cpp
+)
+
+target_link_libraries(
+  velocity_set_lanelet2
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(
+  velocity_set_lanelet2
+  ${catkin_EXPORTED_TARGETS}
+)
+
+install(
+  TARGETS astar_avoid velocity_set velocity_set_lanelet2
+  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)
+  roslint_add_test()
+endif()

src/ros/catkin/src/waypoint_planner/README.md

@@ -0,0 +1,73 @@
+# Waypoint Planner
+
+Waypoint planner package provides the local planner nodes that dynamically plan avoidance behavior, velocity on waypoints, and so on.
+
+## Waypoint Planner - Astar Avoid
+
+`astar_avoid` node has two mode, Relay mode and Avoidance mode. You can switch these modes by `enable_avoidance` parameter.
+
+- Relay mode: Not avoid planning and just publishing waypoints from self pose.
+- Avoidance mode: Avoiding obstacles by Hybrid-A* search algorithm in `astar_search` package with internal state transition
+
+*NOTE* : If you have `wayarea` in your ADAS map, it's possible to limit search area and realize more safety planning by enabling `Use Wayarea` in `costmap_generator` node. Please see the results in below demo videos.
+
+Please see also: mission/packages/freespace_planner/README.md
+
+### How to launch
+
+* From Runtime Manager:
+
+Computing -> Motion Planning -> waypoint_planner -> astar_avoid
+
+* From CLI:
+
+`$ roslaunch waypoint_planner astar_avoid.launch`
+
+### Parameters
+
+Parameters can be set in both Launch file and Runtime manager:
+
+| Parameter in RM | Parameter in Launch | Type | Description | Default |
+| --- | --- | --- | --- | --- |
+| `Enable Avoidance` | `enable_avoidance` | *Bool* | Enable avoidance mode | `false` |
+| `Costmap Topic` | `costmap_topic` | *String* | Costmap topic for Hybrid-A* search | `semantics/costmap_generator/occupancy_grid` |
+| `Waypoint Velocity` | `avoid_waypoints_velocity` | *Double* | Constant velocity on planned waypoints [km/h] | `10.0` |
+| `Avoidance Start Velocity` | `avoid_start_velocity` | *Double* | Self velocity for staring avoidance behavior [km/h] | `5.0` |
+| `Replan Interval` | `replan_interval` | *Double* | Replan interval for avoidance planning [Hz] | `2.0` |
+| - | `safety_waypoints_size` | *Int* | Output waypoint size [-] | `100` |
+| - | `update_rate` | *Double* | Publishing rate [Hz] | `10.0` |
+| - | `search_waypoints_size` | *Int* | Range of waypoints for incremental search [-] | `50` |
+| - | `search_waypoints_delta` | *Int* | Skipped waypoints for incremental search [-] | `2` |
+
+### Subscriptions/Publications
+
+```
+Node [/astar_avoid]
+Publications:
+ * /safety_waypoints [autoware_msgs/Lane]
+
+Subscriptions:
+ * /base_waypoints [autoware_msgs/Lane]
+ * /closest_waypoint [std_msgs/Int32]
+ * /current_pose [geometry_msgs/PoseStamped]
+ * /current_velocity [geometry_msgs/TwistStamped]
+ * /semantics/costmap_generator/occupancy_grid [nav_msgs/OccupancyGrid]
+ * /obstacle_waypoint [std_msgs/Int32]
+ * /tf [tf2_msgs/TFMessage]
+ * /tf_static [tf2_msgs/TFMessage]
+```
+
+### Demo videos
+
+#### Dynamically avoiding (senario 1)
+[![Hybrid A*, dynamically avoiding (scenario 1)](https://img.youtube.com/vi/o_WXfPh9JKA/sddefault.jpg)](https://youtu.be/o_WXfPh9JKA)
+
+#### Dynamically avoiding (senario 2)
+[![Hybrid A*, dynamically avoiding (scenario 12](https://img.youtube.com/vi/fqXIlWuMuDk/sddefault.jpg)](https://youtu.be/fqXIlWuMuDk)
+
+#### Statically avoiding by big re-routing
+[![Hybrid A*, statically avoiding by big re-routing](https://img.youtube.com/vi/J-3J-EiBP38/sddefault.jpg)](https://youtu.be/J-3J-EiBP38)
+
+## Waypoint Planner - Velocity Set
+
+// TODO

src/ros/catkin/src/waypoint_planner/include/waypoint_planner/astar_avoid/astar_avoid.h

@@ -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.
+ */
+
+#ifndef ASTAR_AVOID_H
+#define ASTAR_AVOID_H
+
+#include <iostream>
+#include <vector>
+#include <thread>
+#include <mutex>
+
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <autoware_msgs/LaneArray.h>
+
+#include "libwaypoint_follower/libwaypoint_follower.h"
+#include "astar_search/astar_search.h"
+
+class AstarAvoid
+{
+public:
+  typedef enum STATE
+  {
+    INITIALIZING = -1,
+    RELAYING = 0,
+    STOPPING = 1,
+    PLANNING = 2,
+    AVOIDING = 3
+  } State;
+
+  AstarAvoid();
+  ~AstarAvoid();
+  void run();
+
+private:
+  // ros
+  ros::NodeHandle nh_, private_nh_;
+  ros::Publisher safety_waypoints_pub_;
+  ros::Subscriber costmap_sub_;
+  ros::Subscriber current_pose_sub_;
+  ros::Subscriber current_velocity_sub_;
+  ros::Subscriber base_waypoints_sub_;
+  ros::Subscriber closest_waypoint_sub_;
+  ros::Subscriber obstacle_waypoint_sub_;
+  ros::Subscriber state_sub_;
+  ros::Rate *rate_;
+  tf::TransformListener tf_listener_;
+
+  // params
+  int safety_waypoints_size_;   // output waypoint size [-]
+  double update_rate_;          // publishing rate [Hz]
+
+  bool enable_avoidance_;           // enable avoidance mode
+  double avoid_waypoints_velocity_; // constant velocity on planned waypoints [km/h]
+  double avoid_start_velocity_;     // self velocity for staring avoidance behavior [km/h]
+  double replan_interval_;          // replan interval for avoidance planning [Hz]
+  int search_waypoints_size_;       // range of waypoints for incremental search [-]
+  int search_waypoints_delta_;      // skipped waypoints for incremental search [-]
+  int closest_search_size_;         // search closest waypoint around your car [-]
+
+  // classes
+  AstarSearch astar_;
+  State state_;
+
+  // threads
+  std::thread publish_thread_;
+  std::mutex mutex_;
+
+  // variables
+  bool terminate_thread_;
+  bool found_avoid_path_;
+  int closest_waypoint_index_;
+  int obstacle_waypoint_index_;
+  int closest_local_index_;
+  nav_msgs::OccupancyGrid costmap_;
+  autoware_msgs::Lane base_waypoints_;
+  autoware_msgs::Lane avoid_waypoints_;
+  autoware_msgs::Lane safety_waypoints_;
+  geometry_msgs::PoseStamped current_pose_local_, current_pose_global_;
+  geometry_msgs::PoseStamped goal_pose_local_, goal_pose_global_;
+  geometry_msgs::TwistStamped current_velocity_;
+  tf::Transform local2costmap_;  // local frame (e.g. velodyne) -> costmap origin
+
+  bool costmap_initialized_;
+  bool current_pose_initialized_;
+  bool current_velocity_initialized_;
+  bool base_waypoints_initialized_;
+  bool closest_waypoint_initialized_;
+
+  // functions, callback
+  void costmapCallback(const nav_msgs::OccupancyGrid& msg);
+  void currentPoseCallback(const geometry_msgs::PoseStamped& msg);
+  void currentVelocityCallback(const geometry_msgs::TwistStamped& msg);
+  void baseWaypointsCallback(const autoware_msgs::Lane& msg);
+  void closestWaypointCallback(const std_msgs::Int32& msg);
+  void obstacleWaypointCallback(const std_msgs::Int32& msg);
+
+  // functions
+  bool checkInitialized();
+  bool planAvoidWaypoints(int& end_of_avoid_index);
+  void mergeAvoidWaypoints(const nav_msgs::Path& path, int& end_of_avoid_index);
+  void publishWaypoints();
+  tf::Transform getTransform(const std::string& from, const std::string& to);
+  int getLocalClosestWaypoint(const autoware_msgs::Lane& waypoints, const geometry_msgs::Pose& pose, const int& search_size);
+};
+
+#endif

src/ros/catkin/src/waypoint_planner/include/waypoint_planner/velocity_set/libvelocity_set.h

@@ -0,0 +1,206 @@
+#ifndef _VELOCITY_SET_H
+#define _VELOCITY_SET_H
+
+#include <math.h>
+#include <iostream>
+#include <map>
+#include <unordered_map>
+#include <vector>
+
+#include <geometry_msgs/Point.h>
+#include <ros/ros.h>
+#include <vector_map/vector_map.h>
+
+#include <libwaypoint_follower/libwaypoint_follower.h>
+
+enum class EControl
+{
+  KEEP = -1,
+  STOP = 1,
+  STOPLINE = 2,
+  DECELERATE = 3,
+  OTHERS = 4,
+};
+
+enum class EObstacleType
+{
+  NONE = -1,
+  ON_WAYPOINTS = 1,
+  ON_CROSSWALK = 2,
+  STOPLINE = 3,
+};
+
+struct CrossWalkPoints
+{
+  std::vector<geometry_msgs::Point> points;
+  geometry_msgs::Point center;
+  double width;
+};
+
+class CrossWalk
+{
+private:
+  // detection_points_[bdID] has information of each crosswalk
+  std::unordered_map<int, CrossWalkPoints> detection_points_;
+  int detection_waypoint_;
+  int detection_crosswalk_id_;
+  std::vector<geometry_msgs::Point> obstacle_points_;
+  std::vector<int> bdID_;
+
+  bool enable_multiple_crosswalk_detection_;
+  std::vector<int> detection_crosswalk_array_;
+
+public:
+  bool loaded_crosswalk;
+  bool loaded_area;
+  bool loaded_line;
+  bool loaded_point;
+  bool loaded_all;
+  bool set_points;
+
+  vector_map::CrossWalkArray crosswalk_;
+  vector_map::AreaArray area_;
+  vector_map::LineArray line_;
+  vector_map::PointArray point_;
+
+  void crossWalkCallback(const vector_map::CrossWalkArray &msg);
+  void areaCallback(const vector_map::AreaArray &msg);
+  void lineCallback(const vector_map::LineArray &msg);
+  void pointCallback(const vector_map::PointArray &msg);
+
+  int countAreaSize() const;
+  void getAID(std::unordered_map<int, std::vector<int>> &aid_crosswalk) const;
+  void calcDetectionArea(const std::unordered_map<int, std::vector<int>> &bdid2aid_map);
+  geometry_msgs::Point calcCenterofGravity(const int &aid) const;
+  double calcCrossWalkWidth(const int &aid) const;
+  geometry_msgs::Point getPoint(const int &pid) const;
+  void calcCenterPoints();
+  void setCrossWalkPoints();
+  int findClosestCrosswalk(const int closest_waypoint, const autoware_msgs::Lane &lane, const int search_distance);
+  int getSize() const
+  {
+    return detection_points_.size();
+  }
+  std::vector<int> getBDID() const
+  {
+    return bdID_;
+  }
+  CrossWalkPoints getDetectionPoints(const int &id) const
+  {
+    return detection_points_.at(id);
+  }
+  void setDetectionWaypoint(const int &num)
+  {
+    detection_waypoint_ = num;
+  }
+  int getDetectionWaypoint() const
+  {
+    return detection_waypoint_;
+  }
+  void setDetectionCrossWalkID(const int &id)
+  {
+    detection_crosswalk_id_ = id;
+  }
+  int getDetectionCrossWalkID() const
+  {
+    return detection_crosswalk_id_;
+  }
+
+  void initDetectionCrossWalkIDs()
+  {
+    return detection_crosswalk_array_.clear();
+  }
+  void addDetectionCrossWalkIDs(const int &id)
+  {
+    auto itr = std::find(detection_crosswalk_array_.begin(), detection_crosswalk_array_.end(), id);
+    if (detection_crosswalk_array_.empty() || itr == detection_crosswalk_array_.end())
+    {
+      detection_crosswalk_array_.push_back(id);
+    }
+  }
+  std::vector<int> getDetectionCrossWalkIDs() const
+  {
+    return detection_crosswalk_array_;
+  }
+  void setMultipleDetectionFlag(const bool _multiple_flag)
+  {
+    enable_multiple_crosswalk_detection_ = _multiple_flag;
+  }
+  bool isMultipleDetection() const
+  {
+    return enable_multiple_crosswalk_detection_;
+  }
+
+  CrossWalk()
+    : detection_waypoint_(-1)
+    , detection_crosswalk_id_(-1)
+    , loaded_crosswalk(false)
+    , loaded_area(false)
+    , loaded_line(false)
+    , loaded_point(false)
+    , loaded_all(false)
+    , set_points(false)
+  {
+  }
+};
+
+//////////////////////////////////////
+// for visualization of obstacles
+//////////////////////////////////////
+class ObstaclePoints
+{
+private:
+  std::vector<geometry_msgs::Point> stop_points_;
+  std::vector<geometry_msgs::Point> decelerate_points_;
+  geometry_msgs::Point previous_detection_;
+
+public:
+  void setStopPoint(const geometry_msgs::Point &p)
+  {
+    stop_points_.push_back(p);
+  }
+  void setDeceleratePoint(const geometry_msgs::Point &p)
+  {
+    decelerate_points_.push_back(p);
+  }
+  geometry_msgs::Point getObstaclePoint(const EControl &kind) const;
+  void clearStopPoints()
+  {
+    stop_points_.clear();
+  }
+  void clearDeceleratePoints()
+  {
+    decelerate_points_.clear();
+  }
+
+  ObstaclePoints() : stop_points_(0), decelerate_points_(0)
+  {
+  }
+};
+
+inline double calcSquareOfLength(const geometry_msgs::Point &p1, const geometry_msgs::Point &p2)
+{
+  return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
+}
+
+// Calculate waypoint index corresponding to distance from begin_waypoint
+inline int calcWaypointIndexReverse(const autoware_msgs::Lane &lane, const int begin_waypoint, const double distance)
+{
+  double dist_sum = 0;
+  for (int i = begin_waypoint; i > 0; i--)
+  {
+    tf::Vector3 v1(lane.waypoints[i].pose.pose.position.x, lane.waypoints[i].pose.pose.position.y, 0);
+
+    tf::Vector3 v2(lane.waypoints[i - 1].pose.pose.position.x, lane.waypoints[i - 1].pose.pose.position.y, 0);
+
+    dist_sum += tf::tfDistance(v1, v2);
+
+    if (dist_sum > distance)
+      return i;
+  }
+
+  // reach the first waypoint
+  return 0;
+}
+
+#endif /* _VELOCITY_SET_H */

src/ros/catkin/src/waypoint_planner/include/waypoint_planner/velocity_set/velocity_set_info.h

@@ -0,0 +1,152 @@
+/*
+ * 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 VELOCITY_SET_INFO_H
+#define VELOCITY_SET_INFO_H
+
+#include <ros/ros.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/Int32.h>
+
+#include <autoware_config_msgs/ConfigVelocitySet.h>
+
+#include <autoware_health_checker/health_checker/health_checker.h>
+#include <memory>
+
+class VelocitySetInfo
+{
+ private:
+  // parameters
+  double stop_range_;               // if obstacle is in this range, stop
+  double deceleration_range_;       // if obstacle is in this range, decelerate
+  int points_threshold_;            // points threshold to find obstacles
+  double detection_height_top_;     // from sensor
+  double detection_height_bottom_;  // from sensor
+  double stop_distance_obstacle_;   // (meter) stopping distance from obstacles
+  double stop_distance_stopline_;   // (meter) stopping distance from stoplines
+  double deceleration_obstacle_;    // (m/s^2) deceleration for obstacles
+  double deceleration_stopline_;    // (m/s^2) deceleration for stopline
+  double velocity_change_limit_;    // (m/s)
+  double temporal_waypoints_size_;  // (meter)
+  int  wpidx_detectionResultByOtherNodes_; // waypoints index@finalwaypoints
+
+  // ROS param
+  double remove_points_upto_;
+
+  pcl::PointCloud<pcl::PointXYZ> points_;
+  geometry_msgs::PoseStamped localizer_pose_;  // pose of sensor
+  geometry_msgs::PoseStamped control_pose_;    // pose of base_link
+  bool set_pose_;
+
+  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
+
+ public:
+  VelocitySetInfo();
+  ~VelocitySetInfo();
+
+  // ROS Callback
+  void configCallback(const autoware_config_msgs::ConfigVelocitySetConstPtr &msg);
+  void pointsCallback(const sensor_msgs::PointCloud2ConstPtr &msg);
+  void controlPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg);
+  void localizerPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg);
+  void detectionCallback(const std_msgs::Int32 &msg);
+
+  void clearPoints();
+
+
+  int getDetectionResultByOtherNodes() const
+  {
+    return wpidx_detectionResultByOtherNodes_;
+  }
+
+  double getStopRange() const
+  {
+    return stop_range_;
+  }
+
+  double getDecelerationRange() const
+  {
+    return deceleration_range_;
+  }
+
+  int getPointsThreshold() const
+  {
+    return points_threshold_;
+  }
+
+  int getDetectionHeightTop() const
+  {
+    return detection_height_top_;
+  }
+
+  int getDetectionHeightBottom() const
+  {
+    return detection_height_bottom_;
+  }
+
+  int getStopDistanceObstacle() const
+  {
+    return stop_distance_obstacle_;
+  }
+
+  int getStopDistanceStopline() const
+  {
+    return stop_distance_stopline_;
+  }
+
+  double getDecelerationObstacle() const
+  {
+    return deceleration_obstacle_;
+  }
+
+  double getDecelerationStopline() const
+  {
+    return deceleration_stopline_;
+  }
+
+  double getVelocityChangeLimit() const
+  {
+    return velocity_change_limit_;
+  }
+
+  double getTemporalWaypointsSize() const
+  {
+    return temporal_waypoints_size_;
+  }
+
+  pcl::PointCloud<pcl::PointXYZ> getPoints() const
+  {
+    return points_;
+  }
+
+  geometry_msgs::PoseStamped getControlPose() const
+  {
+    return control_pose_;
+  }
+
+  geometry_msgs::PoseStamped getLocalizerPose() const
+  {
+    return localizer_pose_;
+  }
+
+  bool getSetPose() const
+  {
+    return set_pose_;
+  }
+};
+
+#endif // VELOCITY_SET_INFO_H

src/ros/catkin/src/waypoint_planner/include/waypoint_planner/velocity_set/velocity_set_path.h

@@ -0,0 +1,93 @@
+/*
+ * 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 VELOCITY_SET_PATH_H
+#define VELOCITY_SET_PATH_H
+
+#include <autoware_msgs/Lane.h>
+#include <libwaypoint_follower/libwaypoint_follower.h>
+
+class VelocitySetPath
+{
+ private:
+  autoware_msgs::Lane prev_waypoints_;
+  autoware_msgs::Lane new_waypoints_;
+  autoware_msgs::Lane temporal_waypoints_;
+  bool set_path_;
+  double current_vel_;
+
+  // ROS param
+  double velocity_offset_; // m/s
+  double decelerate_vel_min_; // m/s
+
+  bool checkWaypoint(int num, const char *name) const;
+
+ public:
+  VelocitySetPath();
+  ~VelocitySetPath();
+
+  double calcChangedVelocity(const double& current_vel, const double& accel, const std::array<int, 2>& range) const;
+  void changeWaypointsForStopping(int stop_waypoint, int obstacle_waypoint, int closest_waypoint, double deceleration);
+  void avoidSuddenDeceleration(double velocity_change_limit, double deceleration, int closest_waypoint);
+  void avoidSuddenAcceleration(double decelerationint, int closest_waypoint);
+  void changeWaypointsForDeceleration(double deceleration, int closest_waypoint, int obstacle_waypoint);
+  void setTemporalWaypoints(int temporal_waypoints_size, int closest_waypoint, geometry_msgs::PoseStamped control_pose);
+  void initializeNewWaypoints();
+  void resetFlag();
+
+  // ROS Callbacks
+  void waypointsCallback(const autoware_msgs::LaneConstPtr& msg);
+  void currentVelocityCallback(const geometry_msgs::TwistStampedConstPtr& msg);
+
+  double calcInterval(const int begin, const int end) const;
+
+  autoware_msgs::Lane getPrevWaypoints() const
+  {
+    return prev_waypoints_;
+  }
+
+  autoware_msgs::Lane getNewWaypoints() const
+  {
+    return new_waypoints_;
+  }
+
+  autoware_msgs::Lane getTemporalWaypoints() const
+  {
+    return temporal_waypoints_;
+  }
+
+  bool getSetPath() const
+  {
+    return set_path_;
+  }
+
+  double getCurrentVelocity() const
+  {
+    return current_vel_;
+  }
+
+  int getPrevWaypointsSize() const
+  {
+    return prev_waypoints_.waypoints.size();
+  }
+
+  int getNewWaypointsSize() const
+  {
+    return new_waypoints_.waypoints.size();
+  }
+};
+
+#endif // VELOCITY_SET_PATH_H

src/ros/catkin/src/waypoint_planner/interface.yaml

@@ -0,0 +1,7 @@
+- name: /obstacle_avoid
+  publish: [/astar_path, /safety_waypoints, /astar_debug_poses, /astar_footprint]
+  subscribe: [/grid_map_visualization/distance_transform, /current_pose, /move_base_simple/goal]
+- name: /velocity_set
+  publish: [/detection_range, /obstacle, /obstacle_waypoint, /stopline_waypoint, /final_waypoints]
+  subscribe: [/safety_waypoints, /current_velocity, /config/velocity_set, /points_no_ground,
+    /localizer_pose, /current_pose, /state/stopline_wpid, /vector_map_info/*]

src/ros/catkin/src/waypoint_planner/launch/astar_avoid.launch

@@ -0,0 +1,68 @@
+<launch>
+
+  <!-- Relay behavior configurations -->
+  <arg name="safety_waypoints_size" default="100" />
+  <arg name="update_rate" default="10" />
+
+  <!-- Avoidance behavior configurations -->
+  <arg name="costmap_topic" default="semantics/costmap_generator/occupancy_grid" />
+  <arg name="enable_avoidance" default="false" />
+  <arg name="avoid_waypoints_velocity" default="10.0" />
+  <arg name="avoid_start_velocity" default="3.0" />
+  <arg name="replan_interval" default="2.0" />
+  <arg name="search_waypoints_size" default="50" />
+  <arg name="search_waypoints_delta" default="2" />
+  <arg name="closest_search_size" default="30" />
+
+  <!-- A* search configurations -->
+  <arg name="use_back" default="false" />
+  <arg name="use_potential_heuristic" default="true" />
+  <arg name="use_wavefront_heuristic" default="false" />
+  <arg name="time_limit" default="1000.0" />
+  <arg name="robot_length" default="4.5" />
+  <arg name="robot_width" default="1.75" />
+  <arg name="robot_base2back" default="1.0" />
+  <arg name="minimum_turning_radius" default="6.0" />
+  <arg name="theta_size" default="48" />
+  <arg name="curve_weight" default="1.2" />
+  <arg name="reverse_weight" default="2.00" />
+  <arg name="lateral_goal_range" default="0.5" />
+  <arg name="longitudinal_goal_range" default="2.0" />
+  <arg name="angle_goal_range" default="6.0" />
+  <arg name="obstacle_threshold" default="100" />
+  <arg name="potential_weight" default="10.0" />
+  <arg name="distance_heuristic_weight" default="1.0" />
+
+  <node pkg="waypoint_planner" type="astar_avoid" name="astar_avoid" output="screen">
+    <param name="safety_waypoints_size" value="$(arg safety_waypoints_size)" />
+    <param name="update_rate" value="$(arg update_rate)" />
+
+    <remap from="costmap" to="$(arg costmap_topic)" />
+    <param name="enable_avoidance" value="$(arg enable_avoidance)" />
+    <param name="search_waypoints_size" value="$(arg search_waypoints_size)" />
+    <param name="search_waypoints_delta" value="$(arg search_waypoints_delta)" />
+    <param name="closest_search_size" value="$(arg closest_search_size)" />
+    <param name="avoid_waypoints_velocity" value="$(arg avoid_waypoints_velocity)" />
+    <param name="avoid_start_velocity" value="$(arg avoid_start_velocity)" />
+    <param name="replan_interval" value="$(arg replan_interval)" />
+
+    <param name="use_back" value="$(arg use_back)" />
+    <param name="use_potential_heuristic" value="$(arg use_potential_heuristic)" />
+    <param name="use_wavefront_heuristic" value="$(arg use_wavefront_heuristic)" />
+    <param name="time_limit" value="$(arg time_limit)" />
+    <param name="robot_length" value="$(arg robot_length)" />
+    <param name="robot_width" value="$(arg robot_width)" />
+    <param name="robot_base2back" value="$(arg robot_base2back)" />
+    <param name="minimum_turning_radius" value="$(arg minimum_turning_radius)" />
+    <param name="theta_size" value="$(arg theta_size)" />
+    <param name="angle_goal_range" value="$(arg angle_goal_range)" />
+    <param name="curve_weight" value="$(arg curve_weight)" />
+    <param name="reverse_weight" value="$(arg reverse_weight)" />
+    <param name="lateral_goal_range" value="$(arg lateral_goal_range)" />
+    <param name="longitudinal_goal_range" value="$(arg longitudinal_goal_range)" />
+    <param name="obstacle_threshold" value="$(arg obstacle_threshold)" />
+    <param name="potential_weight" value="$(arg potential_weight)" />
+    <param name="distance_heuristic_weight" value="$(arg distance_heuristic_weight)" />
+  </node>
+
+</launch>

src/ros/catkin/src/waypoint_planner/launch/velocity_set.launch

@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="use_crosswalk_detection" default="true" />
+  <arg name="points_topic" default="points_no_ground" />
+  <arg name="velocity_offset" default="1.2" />
+  <arg name="decelerate_vel_min" default="1.3" />
+  <arg name="remove_points_upto" default="2.3" />
+  <arg name="enable_multiple_crosswalk_detection" default="true" />
+  <arg name="stop_distance_obstacle" default="10.0" />
+  <arg name="stop_distance_stopline" default="5.0" />
+  <arg name="detection_range" default="1.3" />
+  <arg name="points_threshold" default="10" />
+  <arg name="detection_height_top" default="0.2" />
+  <arg name="detection_height_bottom" default="-1.7" />
+  <arg name="deceleration_obstacle" default="0.8" />
+  <arg name="deceleration_stopline" default="0.6" />
+  <arg name="velocity_change_limit" default="9.972" />
+  <arg name="deceleration_range" default="0" />
+  <arg name="temporal_waypoints_size" default="100" />
+
+  <node pkg="waypoint_planner" type="velocity_set" name="velocity_set" output="screen">
+    <param name="use_crosswalk_detection" value="$(arg use_crosswalk_detection)" />
+    <param name="enable_multiple_crosswalk_detection" value="$(arg enable_multiple_crosswalk_detection)" />
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="velocity_offset" value="$(arg velocity_offset)" />
+    <param name="decelerate_vel_min" value="$(arg decelerate_vel_min)" />
+    <param name="remove_points_upto" value="$(arg remove_points_upto)" />
+    <param name="stop_distance_obstacle" value="$(arg stop_distance_obstacle)" />
+    <param name="stop_distance_stopline" value="$(arg stop_distance_stopline)" />
+    <param name="detection_range" value="$(arg detection_range)" />
+    <param name="points_threshold" value="$(arg points_threshold)" />
+    <param name="detection_height_top" value="$(arg detection_height_top)" />
+    <param name="detection_height_bottom" value="$(arg detection_height_bottom)" />
+    <param name="deceleration_obstacle" value="$(arg deceleration_obstacle)" />
+    <param name="deceleration_stopline" value="$(arg deceleration_stopline)" />
+    <param name="velocity_change_limit" value="$(arg velocity_change_limit)" />
+    <param name="deceleration_range" value="$(arg deceleration_range)" />
+    <param name="temporal_waypoints_size" value="$(arg temporal_waypoints_size)" />
+  </node>
+
+</launch>

src/ros/catkin/src/waypoint_planner/launch/velocity_set_lanelet2.launch

@@ -0,0 +1,41 @@
+<!-- -->
+<launch>
+  <arg name="use_crosswalk_detection" default="true" />
+  <arg name="points_topic" default="points_no_ground" />
+  <arg name="velocity_offset" default="1.2" />
+  <arg name="decelerate_vel_min" default="1.3" />
+  <arg name="remove_points_upto" default="2.3" />
+  <arg name="enable_multiple_crosswalk_detection" default="true" />
+  <arg name="stop_distance_obstacle" default="10.0" />
+  <arg name="stop_distance_stopline" default="5.0" />
+  <arg name="detection_range" default="1.3" />
+  <arg name="points_threshold" default="10" />
+  <arg name="detection_height_top" default="0.2" />
+  <arg name="detection_height_bottom" default="-1.7" />
+  <arg name="deceleration_obstacle" default="0.8" />
+  <arg name="deceleration_stopline" default="0.6" />
+  <arg name="velocity_change_limit" default="9.972" />
+  <arg name="deceleration_range" default="0" />
+  <arg name="temporal_waypoints_size" default="100" />
+
+  <node pkg="waypoint_planner" type="velocity_set_lanelet2" name="velocity_set" output="screen">
+    <param name="use_crosswalk_detection" value="$(arg use_crosswalk_detection)" />
+    <param name="enable_multiple_crosswalk_detection" value="$(arg enable_multiple_crosswalk_detection)" />
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="velocity_offset" value="$(arg velocity_offset)" />
+    <param name="decelerate_vel_min" value="$(arg decelerate_vel_min)" />
+    <param name="remove_points_upto" value="$(arg remove_points_upto)" />
+    <param name="stop_distance_obstacle" value="$(arg stop_distance_obstacle)" />
+    <param name="stop_distance_stopline" value="$(arg stop_distance_stopline)" />
+    <param name="detection_range" value="$(arg detection_range)" />
+    <param name="points_threshold" value="$(arg points_threshold)" />
+    <param name="detection_height_top" value="$(arg detection_height_top)" />
+    <param name="detection_height_bottom" value="$(arg detection_height_bottom)" />
+    <param name="deceleration_obstacle" value="$(arg deceleration_obstacle)" />
+    <param name="deceleration_stopline" value="$(arg deceleration_stopline)" />
+    <param name="velocity_change_limit" value="$(arg velocity_change_limit)" />
+    <param name="deceleration_range" value="$(arg deceleration_range)" />
+    <param name="temporal_waypoints_size" value="$(arg temporal_waypoints_size)" />
+  </node>
+
+</launch>

src/ros/catkin/src/waypoint_planner/launch/velocity_set_option.launch

@@ -0,0 +1,68 @@
+<!-- -->
+<launch>
+  <arg name="use_crosswalk_detection" default="true" />
+  <arg name="points_topic" default="points_no_ground" />
+  <arg name="velocity_offset" default="1.2" />
+  <arg name="decelerate_vel_min" default="1.3" />
+  <arg name="remove_points_upto" default="2.3" />
+  <arg name="enable_multiple_crosswalk_detection" default="true" />
+  <arg name="stop_distance_obstacle" default="10.0" />
+  <arg name="stop_distance_stopline" default="5.0" />
+  <arg name="detection_range" default="1.3" />
+  <arg name="points_threshold" default="10" />
+  <arg name="detection_height_top" default="0.2" />
+  <arg name="detection_height_bottom" default="-1.7" />
+  <arg name="deceleration_obstacle" default="0.8" />
+  <arg name="deceleration_stopline" default="0.6" />
+  <arg name="velocity_change_limit" default="9.972" />
+  <arg name="deceleration_range" default="0" />
+  <arg name="temporal_waypoints_size" default="100" />
+  <arg name="use_ll2" default="false" />
+
+<group if="$(arg use_ll2)">
+
+  <node pkg="waypoint_planner" type="velocity_set_lanelet2" name="velocity_set" output="screen">
+    <param name="use_crosswalk_detection" value="$(arg use_crosswalk_detection)" />
+    <param name="enable_multiple_crosswalk_detection" value="$(arg enable_multiple_crosswalk_detection)" />
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="velocity_offset" value="$(arg velocity_offset)" />
+    <param name="decelerate_vel_min" value="$(arg decelerate_vel_min)" />
+    <param name="remove_points_upto" value="$(arg remove_points_upto)" />
+    <param name="stop_distance_obstacle" value="$(arg stop_distance_obstacle)" />
+    <param name="stop_distance_stopline" value="$(arg stop_distance_stopline)" />
+    <param name="detection_range" value="$(arg detection_range)" />
+    <param name="points_threshold" value="$(arg points_threshold)" />
+    <param name="detection_height_top" value="$(arg detection_height_top)" />
+    <param name="detection_height_bottom" value="$(arg detection_height_bottom)" />
+    <param name="deceleration_obstacle" value="$(arg deceleration_obstacle)" />
+    <param name="deceleration_stopline" value="$(arg deceleration_stopline)" />
+    <param name="velocity_change_limit" value="$(arg velocity_change_limit)" />
+    <param name="deceleration_range" value="$(arg deceleration_range)" />
+    <param name="temporal_waypoints_size" value="$(arg temporal_waypoints_size)" />
+  </node>
+</group>
+
+<group unless="$(arg use_ll2)">
+
+  <node pkg="waypoint_planner" type="velocity_set" name="velocity_set" output="screen">
+    <param name="use_crosswalk_detection" value="$(arg use_crosswalk_detection)" />
+    <param name="enable_multiple_crosswalk_detection" value="$(arg enable_multiple_crosswalk_detection)" />
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="velocity_offset" value="$(arg velocity_offset)" />
+    <param name="decelerate_vel_min" value="$(arg decelerate_vel_min)" />
+    <param name="remove_points_upto" value="$(arg remove_points_upto)" />
+    <param name="stop_distance_obstacle" value="$(arg stop_distance_obstacle)" />
+    <param name="stop_distance_stopline" value="$(arg stop_distance_stopline)" />
+    <param name="detection_range" value="$(arg detection_range)" />
+    <param name="points_threshold" value="$(arg points_threshold)" />
+    <param name="detection_height_top" value="$(arg detection_height_top)" />
+    <param name="detection_height_bottom" value="$(arg detection_height_bottom)" />
+    <param name="deceleration_obstacle" value="$(arg deceleration_obstacle)" />
+    <param name="deceleration_stopline" value="$(arg deceleration_stopline)" />
+    <param name="velocity_change_limit" value="$(arg velocity_change_limit)" />
+    <param name="deceleration_range" value="$(arg deceleration_range)" />
+    <param name="temporal_waypoints_size" value="$(arg temporal_waypoints_size)" />
+  </node>
+</group>
+
+</launch>

src/ros/catkin/src/waypoint_planner/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>waypoint_planner</name>
+  <version>1.12.0</version>
+  <description>The waypoint_planner package</description>
+  <maintainer email="aohsato@gmail.com">Akihito OHSATO</maintainer>
+  <author email="aohsato@gmail.com">Akihito OHSATO</author>
+  <author email="antm678@ertl.jp">ando</author>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>astar_search</depend>
+  <depend>autoware_config_msgs</depend>
+  <depend>autoware_health_checker</depend>
+  <depend>autoware_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>libwaypoint_follower</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>roscpp</depend>
+  <depend>roslint</depend>
+  <depend>std_msgs</depend>
+  <depend>tf</depend>
+  <depend>vector_map</depend>
+
+</package>

src/ros/catkin/src/waypoint_planner/src/astar_avoid/astar_avoid.cpp

@@ -0,0 +1,443 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "waypoint_planner/astar_avoid/astar_avoid.h"
+
+AstarAvoid::AstarAvoid()
+  : nh_()
+  , private_nh_("~")
+  , closest_waypoint_index_(-1)
+  , obstacle_waypoint_index_(-1)
+  , closest_local_index_(-1)
+  , costmap_initialized_(false)
+  , current_pose_initialized_(false)
+  , current_velocity_initialized_(false)
+  , base_waypoints_initialized_(false)
+  , closest_waypoint_initialized_(false)
+  , terminate_thread_(false)
+{
+  private_nh_.param<int>("safety_waypoints_size", safety_waypoints_size_, 100);
+  private_nh_.param<double>("update_rate", update_rate_, 10.0);
+
+  private_nh_.param<bool>("enable_avoidance", enable_avoidance_, false);
+  private_nh_.param<double>("avoid_waypoints_velocity", avoid_waypoints_velocity_, 10.0);
+  private_nh_.param<double>("avoid_start_velocity", avoid_start_velocity_, 5.0);
+  private_nh_.param<double>("replan_interval", replan_interval_, 2.0);
+  private_nh_.param<int>("search_waypoints_size", search_waypoints_size_, 50);
+  private_nh_.param<int>("search_waypoints_delta", search_waypoints_delta_, 2);
+  private_nh_.param<int>("closest_search_size", closest_search_size_, 30);
+
+  safety_waypoints_pub_ = nh_.advertise<autoware_msgs::Lane>("safety_waypoints", 1, true);
+  costmap_sub_ = nh_.subscribe("costmap", 1, &AstarAvoid::costmapCallback, this);
+  current_pose_sub_ = nh_.subscribe("current_pose", 1, &AstarAvoid::currentPoseCallback, this);
+  current_velocity_sub_ = nh_.subscribe("current_velocity", 1, &AstarAvoid::currentVelocityCallback, this);
+  base_waypoints_sub_ = nh_.subscribe("base_waypoints", 1, &AstarAvoid::baseWaypointsCallback, this);
+  closest_waypoint_sub_ = nh_.subscribe("closest_waypoint", 1, &AstarAvoid::closestWaypointCallback, this);
+  obstacle_waypoint_sub_ = nh_.subscribe("obstacle_waypoint", 1, &AstarAvoid::obstacleWaypointCallback, this);
+
+  rate_ = new ros::Rate(update_rate_);
+}
+
+AstarAvoid::~AstarAvoid()
+{
+  publish_thread_.join();
+}
+
+void AstarAvoid::costmapCallback(const nav_msgs::OccupancyGrid& msg)
+{
+  costmap_ = msg;
+  tf::poseMsgToTF(costmap_.info.origin, local2costmap_);
+  costmap_initialized_ = true;
+}
+
+void AstarAvoid::currentPoseCallback(const geometry_msgs::PoseStamped& msg)
+{
+  current_pose_global_ = msg;
+
+  if (!enable_avoidance_)
+  {
+    current_pose_initialized_ = true;
+  }
+  else
+  {
+    current_pose_local_.pose = transformPose(
+        current_pose_global_.pose, getTransform(costmap_.header.frame_id, current_pose_global_.header.frame_id));
+    current_pose_local_.header.frame_id = costmap_.header.frame_id;
+    current_pose_local_.header.stamp = current_pose_global_.header.stamp;
+    current_pose_initialized_ = true;
+  }
+}
+
+void AstarAvoid::currentVelocityCallback(const geometry_msgs::TwistStamped& msg)
+{
+  current_velocity_ = msg;
+  current_velocity_initialized_ = true;
+}
+
+void AstarAvoid::baseWaypointsCallback(const autoware_msgs::Lane& msg)
+{
+  static autoware_msgs::Lane prev_base_waypoints;
+  base_waypoints_ = msg;
+
+  if (base_waypoints_initialized_)
+  {
+    // detect waypoint change by timestamp update
+    ros::Time t1 = prev_base_waypoints.header.stamp;
+    ros::Time t2 = base_waypoints_.header.stamp;
+    if ((t2 - t1).toSec() > 1e-3)
+    {
+      ROS_INFO("Receive new /base_waypoints, reset waypoint index.");
+      closest_local_index_ = -1; // reset local closest waypoint
+      prev_base_waypoints = base_waypoints_;
+    }
+  }
+  else
+  {
+    prev_base_waypoints = base_waypoints_;
+  }
+
+  base_waypoints_initialized_ = true;
+}
+
+void AstarAvoid::closestWaypointCallback(const std_msgs::Int32& msg)
+{
+  closest_waypoint_index_ = msg.data;
+
+  if (closest_waypoint_index_ == -1)
+  {
+    closest_local_index_ = -1; // reset local closest waypoint
+  }
+
+  closest_waypoint_initialized_ = true;
+}
+
+void AstarAvoid::obstacleWaypointCallback(const std_msgs::Int32& msg)
+{
+  obstacle_waypoint_index_ = msg.data;
+}
+
+void AstarAvoid::run()
+{
+  // check topics
+  state_ = AstarAvoid::STATE::INITIALIZING;
+
+  while (ros::ok())
+  {
+    ros::spinOnce();
+    if (checkInitialized())
+    {
+      break;
+    }
+    ROS_WARN("Waiting for subscribing topics...");
+    ros::Duration(1.0).sleep();
+  }
+
+  // main loop
+  int end_of_avoid_index = -1;
+  ros::WallTime start_plan_time = ros::WallTime::now();
+  ros::WallTime start_avoid_time = ros::WallTime::now();
+
+  // reset obstacle index
+  obstacle_waypoint_index_ = -1;
+
+  // relaying mode at startup
+  state_ = AstarAvoid::STATE::RELAYING;
+
+  // start publish thread
+  publish_thread_ = std::thread(&AstarAvoid::publishWaypoints, this);
+
+  while (ros::ok())
+  {
+    ros::spinOnce();
+
+    // relay mode
+    if (!enable_avoidance_)
+    {
+      rate_->sleep();
+      continue;
+    }
+
+    // avoidance mode
+    bool found_obstacle = (obstacle_waypoint_index_ >= 0);
+    bool avoid_velocity = (current_velocity_.twist.linear.x < avoid_start_velocity_ / 3.6);
+
+    // update state
+    if (state_ == AstarAvoid::STATE::RELAYING)
+    {
+      avoid_waypoints_ = base_waypoints_;
+
+      if (found_obstacle)
+      {
+        ROS_INFO("RELAYING -> STOPPING, Decelerate for stopping");
+        state_ = AstarAvoid::STATE::STOPPING;
+      }
+    }
+    else if (state_ == AstarAvoid::STATE::STOPPING)
+    {
+      bool replan = ((ros::WallTime::now() - start_plan_time).toSec() > replan_interval_);
+
+      if (!found_obstacle)
+      {
+        ROS_INFO("STOPPING -> RELAYING, Obstacle disappers");
+        state_ = AstarAvoid::STATE::RELAYING;
+      }
+      else if (replan && avoid_velocity)
+      {
+        ROS_INFO("STOPPING -> PLANNING, Start A* planning");
+        state_ = AstarAvoid::STATE::PLANNING;
+      }
+    }
+    else if (state_ == AstarAvoid::STATE::PLANNING)
+    {
+      start_plan_time = ros::WallTime::now();
+
+      if (planAvoidWaypoints(end_of_avoid_index))
+      {
+        ROS_INFO("PLANNING -> AVOIDING, Found path");
+        state_ = AstarAvoid::STATE::AVOIDING;
+        start_avoid_time = ros::WallTime::now();
+      }
+      else
+      {
+        ROS_INFO("PLANNING -> STOPPING, Cannot find path");
+        state_ = AstarAvoid::STATE::STOPPING;
+      }
+    }
+    else if (state_ == AstarAvoid::STATE::AVOIDING)
+    {
+      bool reached = (getLocalClosestWaypoint(avoid_waypoints_, current_pose_global_.pose, closest_search_size_) > end_of_avoid_index);
+      if (reached)
+      {
+        ROS_INFO("AVOIDING -> RELAYING, Reached goal");
+        state_ = AstarAvoid::STATE::RELAYING;
+      }
+      else if (found_obstacle && avoid_velocity)
+      {
+        bool replan = ((ros::WallTime::now() - start_avoid_time).toSec() > replan_interval_);
+        if (replan)
+        {
+          ROS_INFO("AVOIDING -> STOPPING, Abort avoiding");
+          state_ = AstarAvoid::STATE::STOPPING;
+        }
+      }
+    }
+
+    rate_->sleep();
+  }
+
+  terminate_thread_ = true;
+}
+
+bool AstarAvoid::checkInitialized()
+{
+  bool initialized = false;
+
+  // check for relay mode
+  initialized = (current_pose_initialized_ && closest_waypoint_initialized_ && base_waypoints_initialized_ &&
+                 (closest_waypoint_index_ >= 0));
+
+  // check for avoidance mode, additionally
+  if (enable_avoidance_)
+  {
+    initialized = (initialized && (current_velocity_initialized_ && costmap_initialized_));
+  }
+
+  return initialized;
+}
+
+bool AstarAvoid::planAvoidWaypoints(int& end_of_avoid_index)
+{
+  bool found_path = false;
+  int closest_waypoint_index = getLocalClosestWaypoint(avoid_waypoints_, current_pose_global_.pose, closest_search_size_);
+
+  // update goal pose incrementally and execute A* search
+  for (int i = search_waypoints_delta_; i < static_cast<int>(search_waypoints_size_); i += search_waypoints_delta_)
+  {
+    // update goal index
+    int goal_waypoint_index = closest_waypoint_index + obstacle_waypoint_index_ + i;
+    if (goal_waypoint_index >= static_cast<int>(avoid_waypoints_.waypoints.size()))
+    {
+      break;
+    }
+
+    // update goal pose
+    goal_pose_global_ = avoid_waypoints_.waypoints[goal_waypoint_index].pose;
+    goal_pose_local_.header = costmap_.header;
+    goal_pose_local_.pose = transformPose(goal_pose_global_.pose,
+                                          getTransform(costmap_.header.frame_id, goal_pose_global_.header.frame_id));
+
+    // initialize costmap for A* search
+    astar_.initialize(costmap_);
+
+    // execute astar search
+    // ros::WallTime start = ros::WallTime::now();
+    found_path = astar_.makePlan(current_pose_local_.pose, goal_pose_local_.pose);
+    // ros::WallTime end = ros::WallTime::now();
+
+    static ros::Publisher pub = nh_.advertise<nav_msgs::Path>("debug", 1, true);
+
+    // ROS_INFO("Astar planning: %f [s], at index = %d", (end - start).toSec(), goal_waypoint_index);
+
+    if (found_path)
+    {
+      pub.publish(astar_.getPath());
+      end_of_avoid_index = goal_waypoint_index;
+      mergeAvoidWaypoints(astar_.getPath(), end_of_avoid_index);
+      if (avoid_waypoints_.waypoints.size() > 0)
+      {
+        ROS_INFO("Found GOAL at index = %d", goal_waypoint_index);
+        astar_.reset();
+        return true;
+      }
+      else
+      {
+        found_path = false;
+      }
+    }
+    astar_.reset();
+  }
+
+  ROS_ERROR("Can't find goal...");
+  return false;
+}
+
+void AstarAvoid::mergeAvoidWaypoints(const nav_msgs::Path& path, int& end_of_avoid_index)
+{
+  autoware_msgs::Lane current_waypoints = avoid_waypoints_;
+
+  // reset
+  std::lock_guard<std::mutex> lock(mutex_);
+  avoid_waypoints_.waypoints.clear();
+
+  // add waypoints before start index
+  int closest_waypoint_index = getLocalClosestWaypoint(current_waypoints, current_pose_global_.pose, closest_search_size_);
+  for (int i = 0; i < closest_waypoint_index; ++i)
+  {
+    avoid_waypoints_.waypoints.push_back(current_waypoints.waypoints.at(i));
+  }
+
+  // set waypoints for avoiding
+  for (const auto& pose : path.poses)
+  {
+    autoware_msgs::Waypoint wp;
+    wp.pose.header = avoid_waypoints_.header;
+    wp.pose.pose = transformPose(pose.pose, getTransform(avoid_waypoints_.header.frame_id, pose.header.frame_id));
+    wp.pose.pose.position.z = current_pose_global_.pose.position.z;  // height = const
+    wp.twist.twist.linear.x = avoid_waypoints_velocity_ / 3.6;       // velocity = const
+    avoid_waypoints_.waypoints.push_back(wp);
+  }
+
+  // add waypoints after goal index
+  for (int i = end_of_avoid_index; i < static_cast<int>(current_waypoints.waypoints.size()); ++i)
+  {
+    avoid_waypoints_.waypoints.push_back(current_waypoints.waypoints.at(i));
+  }
+
+  // update index for merged waypoints
+  end_of_avoid_index = closest_waypoint_index + path.poses.size();
+}
+
+void AstarAvoid::publishWaypoints()
+{
+  autoware_msgs::Lane current_waypoints;
+
+  while (!terminate_thread_)
+  {
+    // select waypoints
+    switch (state_)
+    {
+      case AstarAvoid::STATE::RELAYING:
+        current_waypoints = base_waypoints_;
+        break;
+      case AstarAvoid::STATE::STOPPING:
+        // do nothing, keep current waypoints
+        break;
+      case AstarAvoid::STATE::PLANNING:
+        // do nothing, keep current waypoints
+        break;
+      case AstarAvoid::STATE::AVOIDING:
+        current_waypoints = avoid_waypoints_;
+        break;
+      default:
+        current_waypoints = base_waypoints_;
+        break;
+    }
+
+    autoware_msgs::Lane safety_waypoints;
+    safety_waypoints.header = current_waypoints.header;
+    safety_waypoints.increment = current_waypoints.increment;
+
+    // push waypoints from closest index
+    for (int i = 0; i < safety_waypoints_size_; ++i)
+    {
+      int index = getLocalClosestWaypoint(current_waypoints, current_pose_global_.pose, closest_search_size_) + i;
+      if (index < 0 || static_cast<int>(current_waypoints.waypoints.size()) <= index)
+      {
+        break;
+      }
+      const autoware_msgs::Waypoint& wp = current_waypoints.waypoints[index];
+      safety_waypoints.waypoints.push_back(wp);
+    }
+
+    if (safety_waypoints.waypoints.size() > 0)
+    {
+      safety_waypoints_pub_.publish(safety_waypoints);
+    }
+
+    rate_->sleep();
+  }
+}
+
+tf::Transform AstarAvoid::getTransform(const std::string& from, const std::string& to)
+{
+  tf::StampedTransform stf;
+  try
+  {
+    tf_listener_.lookupTransform(from, to, ros::Time(0), stf);
+  }
+  catch (tf::TransformException ex)
+  {
+    ROS_ERROR("%s", ex.what());
+  }
+  return stf;
+}
+
+int AstarAvoid::getLocalClosestWaypoint(const autoware_msgs::Lane& waypoints, const geometry_msgs::Pose& pose, const int& search_size)
+{
+  static autoware_msgs::Lane local_waypoints;  // around self-vehicle
+  const int prev_index = closest_local_index_;
+
+  // search in all waypoints if lane_select judges you're not on waypoints
+  if (closest_local_index_ == -1)
+  {
+    closest_local_index_ = getClosestWaypoint(waypoints, pose);
+  }
+  // search in limited area based on prev_index
+  else
+  {
+    // get neighborhood waypoints around prev_index
+    int start_index = std::max(0, prev_index - search_size / 2);
+    int end_index = std::min(prev_index + search_size / 2, (int)waypoints.waypoints.size());
+    auto start_itr = waypoints.waypoints.begin() + start_index;
+    auto end_itr = waypoints.waypoints.begin() + end_index;
+    local_waypoints.waypoints = std::vector<autoware_msgs::Waypoint>(start_itr, end_itr);
+
+    // get closest waypoint in neighborhood waypoints
+    closest_local_index_ = start_index + getClosestWaypoint(local_waypoints, pose);
+  }
+
+  return closest_local_index_;
+}

src/ros/catkin/src/waypoint_planner/src/astar_avoid/astar_avoid_node.cpp

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

src/ros/catkin/src/waypoint_planner/src/velocity_set/libvelocity_set.cpp

@@ -0,0 +1,338 @@
+#include <waypoint_planner/velocity_set/libvelocity_set.h>
+
+// extract edge points from zebra zone
+std::vector<geometry_msgs::Point> removeNeedlessPoints(std::vector<geometry_msgs::Point> &area_points)
+{
+  area_points.push_back(area_points.front());
+  std::map<double, int> length_index;
+  for (unsigned int i = 0; i < area_points.size() - 1; i++)
+    length_index[calcSquareOfLength(area_points[i], area_points[i + 1])] = i;
+
+  std::vector<geometry_msgs::Point> new_points;
+  auto it = length_index.end();
+  int first = (--it)->second;
+  int second = (--it)->second;
+  new_points.push_back(area_points[first]);
+  new_points.push_back(area_points[first + 1]);
+  new_points.push_back(area_points[second]);
+  new_points.push_back(area_points[second + 1]);
+
+  return new_points;
+}
+
+void CrossWalk::crossWalkCallback(const vector_map::CrossWalkArray &msg)
+{
+  crosswalk_ = msg;
+
+  loaded_crosswalk = true;
+  if (loaded_crosswalk && loaded_area && loaded_line && loaded_point)
+  {
+    loaded_all = true;
+    ROS_INFO("All VectorMap loaded");
+  }
+}
+
+void CrossWalk::areaCallback(const vector_map::AreaArray &msg)
+{
+  area_ = msg;
+
+  loaded_area = true;
+  if (loaded_crosswalk && loaded_area && loaded_line && loaded_point)
+  {
+    loaded_all = true;
+    ROS_INFO("All VectorMap loaded");
+  }
+}
+
+void CrossWalk::lineCallback(const vector_map::LineArray &msg)
+{
+  line_ = msg;
+
+  loaded_line = true;
+  if (loaded_crosswalk && loaded_area && loaded_line && loaded_point)
+  {
+    loaded_all = true;
+    ROS_INFO("All VectorMap loaded");
+  }
+}
+
+void CrossWalk::pointCallback(const vector_map::PointArray &msg)
+{
+  point_ = msg;
+
+  loaded_point = true;
+  if (loaded_crosswalk && loaded_area && loaded_line && loaded_point)
+  {
+    loaded_all = true;
+    ROS_INFO("All VectorMap loaded");
+  }
+}
+
+geometry_msgs::Point CrossWalk::getPoint(const int &pid) const
+{
+  geometry_msgs::Point point;
+  for (const auto &p : point_.data)
+  {
+    if (p.pid == pid)
+    {
+      point.x = p.ly;
+      point.y = p.bx;
+      point.z = p.h;
+      return point;
+    }
+  }
+
+  ROS_ERROR("can't find a point of pid %d", pid);
+  return point;
+}
+
+geometry_msgs::Point CrossWalk::calcCenterofGravity(const int &aid) const
+{
+  int search_lid = -1;
+  for (const auto &area : area_.data)
+    if (area.aid == aid)
+    {
+      search_lid = area.slid;
+      break;
+    }
+
+  std::vector<geometry_msgs::Point> area_points;
+  while (search_lid)
+  {
+    for (const auto &line : line_.data)
+    {
+      if (line.lid == search_lid)
+      {
+        area_points.push_back(getPoint(line.bpid));
+        search_lid = line.flid;
+      }
+    }
+  }
+
+  geometry_msgs::Point point;
+  point.x = 0.0;
+  point.y = 0.0;
+  point.z = 0.0;
+  if (area_points.size() > 4)
+  {
+    std::vector<geometry_msgs::Point> filterd_points = removeNeedlessPoints(area_points);
+    for (const auto &p : filterd_points)
+    {
+      point.x += p.x;
+      point.y += p.y;
+      point.z += p.z;
+    }
+  }
+  else
+  {
+    for (const auto &p : area_points)
+    {
+      point.x += p.x;
+      point.y += p.y;
+      point.z += p.z;
+    }
+  }
+
+  point.x /= 4;
+  point.y /= 4;
+  point.z /= 4;
+  return point;
+}
+
+double CrossWalk::calcCrossWalkWidth(const int &aid) const
+{
+  int search_lid = -1;
+  for (const auto &area : area_.data)
+    if (area.aid == aid)
+    {
+      search_lid = area.slid;
+      break;
+    }
+
+  std::vector<geometry_msgs::Point> area_points;
+  while (search_lid)
+  {
+    for (const auto &line : line_.data)
+    {
+      if (line.lid == search_lid)
+      {
+        area_points.push_back(getPoint(line.bpid));
+        //_points.push_back(area_points.back());///
+        search_lid = line.flid;
+      }
+    }
+  }
+
+  area_points.push_back(area_points.front());
+  double max_length = calcSquareOfLength(area_points[0], area_points[1]);
+  for (unsigned int i = 1; i < area_points.size() - 1; i++)
+  {
+    if (calcSquareOfLength(area_points[i], area_points[i + 1]) > max_length)
+      max_length = calcSquareOfLength(area_points[i], area_points[i + 1]);
+  }
+
+  return sqrt(max_length);
+}
+
+// count the number of crosswalks
+int CrossWalk::countAreaSize() const
+{
+  int count = 0;
+  for (const auto &x : crosswalk_.data)
+    if (x.type == 0)  // type:0 -> outer frame of crosswalks
+      count++;
+
+  return count;
+}
+
+void CrossWalk::getAID(std::unordered_map<int, std::vector<int>> &bdid2aid_map) const
+{
+  for (const auto &x : crosswalk_.data)
+    if (x.type == 1)
+    {                                         // if it is zebra
+      bdid2aid_map[x.bdid].push_back(x.aid);  // save area id
+    }
+}
+
+void CrossWalk::calcDetectionArea(const std::unordered_map<int, std::vector<int>> &bdid2aid_map)
+{
+  for (const auto &crosswalk_aids : bdid2aid_map)
+  {
+    int bdid = crosswalk_aids.first;
+    double width = 0.0;
+    for (const auto &aid : crosswalk_aids.second)
+    {
+      detection_points_[bdid].points.push_back(calcCenterofGravity(aid));
+      width += calcCrossWalkWidth(aid);
+    }
+    width /= crosswalk_aids.second.size();
+    detection_points_[bdid].width = width;
+  }
+}
+
+void CrossWalk::calcCenterPoints()
+{
+  for (const auto &i : bdID_)
+  {
+    geometry_msgs::Point center;
+    center.x = 0.0;
+    center.y = 0.0;
+    center.z = 0.0;
+    for (const auto &p : detection_points_[i].points)
+    {
+      center.x += p.x;
+      center.y += p.y;
+      center.z += p.z;
+    }
+    center.x /= detection_points_[i].points.size();
+    center.y /= detection_points_[i].points.size();
+    center.z /= detection_points_[i].points.size();
+    detection_points_[i].center = center;
+  }
+}
+
+void CrossWalk::setCrossWalkPoints()
+{
+  // bdid2aid_map[BDID] has AIDs of its zebra zone
+  std::unordered_map<int, std::vector<int>> bdid2aid_map;
+  getAID(bdid2aid_map);
+
+  // Save key values
+  for (const auto &bdid2aid : bdid2aid_map)
+    bdID_.push_back(bdid2aid.first);
+
+  calcDetectionArea(bdid2aid_map);
+  calcCenterPoints();
+
+  ROS_INFO("Set cross walk detection points");
+  set_points = true;
+}
+
+int CrossWalk::findClosestCrosswalk(const int closest_waypoint, const autoware_msgs::Lane &lane,
+                                    const int search_distance)
+{
+  if (!set_points || closest_waypoint < 0)
+    return -1;
+
+  double find_distance = 2.0 * 2.0;      // meter
+  double ignore_distance = 20.0 * 20.0;  // meter
+  static std::vector<int> bdid = getBDID();
+
+  int _return_val = 0;
+
+  initDetectionCrossWalkIDs();  // for multiple
+
+  // Find near cross walk
+  for (int num = closest_waypoint; num < closest_waypoint + search_distance && num < (int)lane.waypoints.size(); num++)
+  {
+    geometry_msgs::Point waypoint = lane.waypoints[num].pose.pose.position;
+    waypoint.z = 0.0;  // ignore Z axis
+    for (const auto &i : bdid)
+    {
+      // ignore far crosswalk
+      geometry_msgs::Point crosswalk_center = getDetectionPoints(i).center;
+      crosswalk_center.z = 0.0;
+      if (calcSquareOfLength(crosswalk_center, waypoint) > ignore_distance)
+        continue;
+
+      for (auto p : getDetectionPoints(i).points)
+      {
+        p.z = waypoint.z;
+        if (calcSquareOfLength(p, waypoint) < find_distance)
+        {
+          addDetectionCrossWalkIDs(i);
+          if (!this->isMultipleDetection())
+          {
+            setDetectionCrossWalkID(i);
+            return num;
+          }
+          else if (!_return_val)
+          {
+            setDetectionCrossWalkID(i);
+            _return_val = num;
+          }
+        }
+      }
+    }
+  }
+
+  if (_return_val)
+    return _return_val;
+
+  setDetectionCrossWalkID(-1);
+  return -1;  // no near crosswalk
+}
+
+geometry_msgs::Point ObstaclePoints::getObstaclePoint(const EControl &kind) const
+{
+  geometry_msgs::Point point;
+
+  if (kind == EControl::STOP || kind == EControl::STOPLINE)
+  {
+    for (const auto &p : stop_points_)
+    {
+      point.x += p.x;
+      point.y += p.y;
+      point.z += p.z;
+    }
+    point.x /= stop_points_.size();
+    point.y /= stop_points_.size();
+    point.z /= stop_points_.size();
+
+    return point;
+  }
+  else  // kind == DECELERATE
+  {
+    for (const auto &p : decelerate_points_)
+    {
+      point.x += p.x;
+      point.y += p.y;
+      point.z += p.z;
+    }
+    point.x /= decelerate_points_.size();
+    point.y /= decelerate_points_.size();
+    point.z /= decelerate_points_.size();
+
+    return point;
+  }
+}

src/ros/catkin/src/waypoint_planner/src/velocity_set/velocity_set.cpp

@@ -0,0 +1,639 @@
+/*
+ * 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 <visualization_msgs/MarkerArray.h>
+#include <std_msgs/ColorRGBA.h>
+#include <iostream>
+
+#include <waypoint_planner/velocity_set/libvelocity_set.h>
+#include <waypoint_planner/velocity_set/velocity_set_info.h>
+#include <waypoint_planner/velocity_set/velocity_set_path.h>
+
+namespace
+{
+constexpr int LOOP_RATE = 10;
+constexpr double DECELERATION_SEARCH_DISTANCE = 30;
+constexpr double STOP_SEARCH_DISTANCE = 60;
+
+void obstacleColorByKind(const EControl kind, std_msgs::ColorRGBA &color, const double alpha=0.5)
+{
+  if (kind == EControl::STOP)
+  {
+    color.r = 1.0; color.g = 0.0; color.b = 0.0; color.a = alpha;  // red
+  }
+  else if (kind == EControl::STOPLINE)
+  {
+    color.r = 0.0; color.g = 0.0; color.b = 1.0; color.a = alpha;  // blue
+  }
+  else if (kind == EControl::DECELERATE)
+  {
+    color.r = 1.0; color.g = 1.0; color.b = 0.0; color.a = alpha;  // yellow
+  }
+  else
+  {
+    color.r = 1.0; color.g = 1.0; color.b = 1.0; color.a = alpha;  // white
+  }
+}
+
+// Display a detected obstacle
+void displayObstacle(const EControl& kind, const ObstaclePoints& obstacle_points, const ros::Publisher& obstacle_pub)
+{
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "/map";
+  marker.header.stamp = ros::Time();
+  marker.ns = "my_namespace";
+  marker.id = 0;
+  marker.type = visualization_msgs::Marker::CUBE;
+  marker.action = visualization_msgs::Marker::ADD;
+
+  static geometry_msgs::Point prev_obstacle_point;
+  if (kind == EControl::STOP || kind == EControl::STOPLINE || kind == EControl::DECELERATE)
+  {
+    marker.pose.position = obstacle_points.getObstaclePoint(kind);
+    prev_obstacle_point = marker.pose.position;
+  }
+  else  // kind == OTHERS
+  {
+    marker.pose.position = prev_obstacle_point;
+  }
+  geometry_msgs::Quaternion quat;
+  marker.pose.orientation = quat;
+
+  marker.scale.x = 1.0;
+  marker.scale.y = 1.0;
+  marker.scale.z = 2.0;
+  marker.lifetime = ros::Duration(0.1);
+  marker.frame_locked = true;
+  obstacleColorByKind(kind, marker.color, 0.7);
+
+  obstacle_pub.publish(marker);
+}
+
+void displayDetectionRange(const autoware_msgs::Lane& lane, const CrossWalk& crosswalk, const int closest_waypoint,
+                           const EControl& kind, const int obstacle_waypoint, const double stop_range,
+                           const double deceleration_range, const ros::Publisher& detection_range_pub)
+{
+  // set up for marker array
+  visualization_msgs::MarkerArray marker_array;
+  visualization_msgs::Marker crosswalk_marker;
+  visualization_msgs::Marker waypoint_marker_stop;
+  visualization_msgs::Marker waypoint_marker_decelerate;
+  visualization_msgs::Marker stop_line;
+  crosswalk_marker.header.frame_id = "/map";
+  crosswalk_marker.header.stamp = ros::Time();
+  crosswalk_marker.id = 0;
+  crosswalk_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+  crosswalk_marker.action = visualization_msgs::Marker::ADD;
+  waypoint_marker_stop = crosswalk_marker;
+  waypoint_marker_decelerate = crosswalk_marker;
+  stop_line = crosswalk_marker;
+  stop_line.type = visualization_msgs::Marker::CUBE;
+
+  // set each namespace
+  crosswalk_marker.ns = "Crosswalk Detection";
+  waypoint_marker_stop.ns = "Stop Detection";
+  waypoint_marker_decelerate.ns = "Decelerate Detection";
+  stop_line.ns = "Stop Line";
+
+  // set scale and color
+  double scale = 2 * stop_range;
+  waypoint_marker_stop.scale.x = scale;
+  waypoint_marker_stop.scale.y = scale;
+  waypoint_marker_stop.scale.z = scale;
+  waypoint_marker_stop.color.a = 0.2;
+  waypoint_marker_stop.color.r = 0.0;
+  waypoint_marker_stop.color.g = 1.0;
+  waypoint_marker_stop.color.b = 0.0;
+  waypoint_marker_stop.frame_locked = true;
+
+  scale = 2 * (stop_range + deceleration_range);
+  waypoint_marker_decelerate.scale.x = scale;
+  waypoint_marker_decelerate.scale.y = scale;
+  waypoint_marker_decelerate.scale.z = scale;
+  waypoint_marker_decelerate.color.a = 0.15;
+  waypoint_marker_decelerate.color.r = 1.0;
+  waypoint_marker_decelerate.color.g = 1.0;
+  waypoint_marker_decelerate.color.b = 0.0;
+  waypoint_marker_decelerate.frame_locked = true;
+
+  if (obstacle_waypoint > -1)
+  {
+    stop_line.pose.position = lane.waypoints[obstacle_waypoint].pose.pose.position;
+    stop_line.pose.orientation = lane.waypoints[obstacle_waypoint].pose.pose.orientation;
+  }
+  stop_line.pose.position.z += 1.0;
+  stop_line.scale.x = 0.1;
+  stop_line.scale.y = 15.0;
+  stop_line.scale.z = 2.0;
+  stop_line.lifetime = ros::Duration(0.1);
+  stop_line.frame_locked = true;
+  obstacleColorByKind(kind, stop_line.color, 0.3);
+
+  int crosswalk_id = crosswalk.getDetectionCrossWalkID();
+  if (crosswalk_id > 0)
+    scale = crosswalk.getDetectionPoints(crosswalk_id).width;
+  crosswalk_marker.scale.x = scale;
+  crosswalk_marker.scale.y = scale;
+  crosswalk_marker.scale.z = scale;
+  crosswalk_marker.color.a = 0.5;
+  crosswalk_marker.color.r = 0.0;
+  crosswalk_marker.color.g = 1.0;
+  crosswalk_marker.color.b = 0.0;
+  crosswalk_marker.frame_locked = true;
+
+  // set marker points coordinate
+  for (int i = 0; i < STOP_SEARCH_DISTANCE; i++)
+  {
+    if (closest_waypoint < 0 || i + closest_waypoint > static_cast<int>(lane.waypoints.size()) - 1)
+      break;
+
+    geometry_msgs::Point point;
+    point = lane.waypoints[closest_waypoint + i].pose.pose.position;
+
+    waypoint_marker_stop.points.push_back(point);
+
+    if (i > DECELERATION_SEARCH_DISTANCE)
+      continue;
+    waypoint_marker_decelerate.points.push_back(point);
+  }
+
+  if (crosswalk_id > 0)
+  {
+    if (!crosswalk.isMultipleDetection())
+    {
+      for (const auto& p : crosswalk.getDetectionPoints(crosswalk_id).points)
+        crosswalk_marker.points.push_back(p);
+    }
+    else
+    {
+      for (const auto& c_id : crosswalk.getDetectionCrossWalkIDs())
+      {
+        for (const auto& p : crosswalk.getDetectionPoints(c_id).points)
+        {
+          scale = crosswalk.getDetectionPoints(c_id).width;
+          crosswalk_marker.points.push_back(p);
+        }
+      }
+    }
+  }
+  // publish marker
+  marker_array.markers.push_back(crosswalk_marker);
+  marker_array.markers.push_back(waypoint_marker_stop);
+  marker_array.markers.push_back(waypoint_marker_decelerate);
+  if (kind != EControl::KEEP)
+    marker_array.markers.push_back(stop_line);
+  detection_range_pub.publish(marker_array);
+  marker_array.markers.clear();
+}
+
+// obstacle detection for crosswalk
+EControl crossWalkDetection(const pcl::PointCloud<pcl::PointXYZ>& points, const CrossWalk& crosswalk,
+                            const geometry_msgs::PoseStamped& localizer_pose, const int points_threshold,
+                            ObstaclePoints* obstacle_points)
+{
+  int crosswalk_id = crosswalk.getDetectionCrossWalkID();
+  double search_radius = crosswalk.getDetectionPoints(crosswalk_id).width / 2;
+  // std::vector<int> crosswalk_ids crosswalk.getDetectionCrossWalkIDs();
+
+  // Search each calculated points in the crosswalk
+  for (const auto& c_id : crosswalk.getDetectionCrossWalkIDs())
+  {
+    for (const auto& p : crosswalk.getDetectionPoints(c_id).points)
+    {
+      geometry_msgs::Point detection_point = calcRelativeCoordinate(p, localizer_pose.pose);
+      tf::Vector3 detection_vector = point2vector(detection_point);
+      detection_vector.setZ(0.0);
+
+      int stop_count = 0;  // the number of points in the detection area
+      for (const auto& p : points)
+      {
+        tf::Vector3 point_vector(p.x, p.y, 0.0);
+        double distance = tf::tfDistance(point_vector, detection_vector);
+        if (distance < search_radius)
+        {
+          stop_count++;
+          geometry_msgs::Point point_temp;
+          point_temp.x = p.x;
+          point_temp.y = p.y;
+          point_temp.z = p.z;
+          obstacle_points->setStopPoint(calcAbsoluteCoordinate(point_temp, localizer_pose.pose));
+        }
+        if (stop_count > points_threshold)
+          return EControl::STOP;
+      }
+    }
+
+    obstacle_points->clearStopPoints();
+    if (!crosswalk.isMultipleDetection())
+      break;
+  }
+  return EControl::KEEP;  // find no obstacles
+}
+
+int detectStopObstacle(const pcl::PointCloud<pcl::PointXYZ>& points, const int closest_waypoint,
+                       const autoware_msgs::Lane& lane, const CrossWalk& crosswalk, double stop_range,
+                       double points_threshold, const geometry_msgs::PoseStamped& localizer_pose,
+                       ObstaclePoints* obstacle_points, EObstacleType* obstacle_type,
+                       const int wpidx_detection_result_by_other_nodes)
+{
+  int stop_obstacle_waypoint = -1;
+  *obstacle_type = EObstacleType::NONE;
+  std::cout<<" close "<<closest_waypoint<<std::endl<<" search distance: "<<STOP_SEARCH_DISTANCE<<std::endl;
+  // start search from the closest waypoint
+  for (int i = closest_waypoint; i < closest_waypoint + STOP_SEARCH_DISTANCE; i++)
+  {
+    // reach the end of waypoints
+    if (i >= static_cast<int>(lane.waypoints.size()))
+      break;
+
+    // detection another nodes
+    if (wpidx_detection_result_by_other_nodes >= 0 &&
+        lane.waypoints.at(i).gid == wpidx_detection_result_by_other_nodes)
+    {
+      stop_obstacle_waypoint = i;
+      *obstacle_type = EObstacleType::STOPLINE;
+      obstacle_points->setStopPoint(lane.waypoints.at(i).pose.pose.position); // for vizuialization
+      break;
+    }
+
+    // Detection for cross walk
+    if (i == crosswalk.getDetectionWaypoint())
+    {
+      // found an obstacle in the cross walk
+      if (crossWalkDetection(points, crosswalk, localizer_pose, points_threshold, obstacle_points) == EControl::STOP)
+      {
+        stop_obstacle_waypoint = i;
+        *obstacle_type = EObstacleType::ON_CROSSWALK;
+        break;
+      }
+    }
+
+    // waypoint seen by localizer
+    geometry_msgs::Point waypoint = calcRelativeCoordinate(lane.waypoints[i].pose.pose.position, localizer_pose.pose);
+    tf::Vector3 tf_waypoint = point2vector(waypoint);
+    tf_waypoint.setZ(0);
+
+    int stop_point_count = 0;
+    for (const auto& p : points)
+    {
+      tf::Vector3 point_vector(p.x, p.y, 0);
+
+      // 2D distance between waypoint and points (obstacle)
+      double dt = tf::tfDistance(point_vector, tf_waypoint);
+      if (dt < stop_range)
+      {
+        stop_point_count++;
+        geometry_msgs::Point point_temp;
+        point_temp.x = p.x;
+        point_temp.y = p.y;
+        point_temp.z = p.z;
+        obstacle_points->setStopPoint(calcAbsoluteCoordinate(point_temp, localizer_pose.pose));
+      }
+    }
+
+    // there is an obstacle if the number of points exceeded the threshold
+    if (stop_point_count > points_threshold)
+    {
+      stop_obstacle_waypoint = i;
+      *obstacle_type = EObstacleType::ON_WAYPOINTS;
+      break;
+    }
+
+    obstacle_points->clearStopPoints();
+
+    // check next waypoint...
+  }
+
+  return stop_obstacle_waypoint;
+}
+
+int detectDecelerateObstacle(const pcl::PointCloud<pcl::PointXYZ>& points, const int closest_waypoint,
+                             const autoware_msgs::Lane& lane, const double stop_range, const double deceleration_range,
+                             const double points_threshold, const geometry_msgs::PoseStamped& localizer_pose,
+                             ObstaclePoints* obstacle_points)
+{
+  int decelerate_obstacle_waypoint = -1;
+  // start search from the closest waypoint
+  for (int i = closest_waypoint; i < closest_waypoint + DECELERATION_SEARCH_DISTANCE; i++)
+  {
+    // reach the end of waypoints
+    if (i >= static_cast<int>(lane.waypoints.size()))
+      break;
+
+    // waypoint seen by localizer
+    geometry_msgs::Point waypoint = calcRelativeCoordinate(lane.waypoints[i].pose.pose.position, localizer_pose.pose);
+    tf::Vector3 tf_waypoint = point2vector(waypoint);
+    tf_waypoint.setZ(0);
+
+    int decelerate_point_count = 0;
+    for (const auto& p : points)
+    {
+      tf::Vector3 point_vector(p.x, p.y, 0);
+
+      // 2D distance between waypoint and points (obstacle)
+      double dt = tf::tfDistance(point_vector, tf_waypoint);
+      if (dt > stop_range && dt < stop_range + deceleration_range)
+      {
+        decelerate_point_count++;
+        geometry_msgs::Point point_temp;
+        point_temp.x = p.x;
+        point_temp.y = p.y;
+        point_temp.z = p.z;
+        obstacle_points->setDeceleratePoint(calcAbsoluteCoordinate(point_temp, localizer_pose.pose));
+      }
+    }
+
+    // there is an obstacle if the number of points exceeded the threshold
+    if (decelerate_point_count > points_threshold)
+    {
+      decelerate_obstacle_waypoint = i;
+      break;
+    }
+
+    obstacle_points->clearDeceleratePoints();
+
+    // check next waypoint...
+  }
+
+  return decelerate_obstacle_waypoint;
+}
+
+// Detect an obstacle by using pointcloud
+EControl pointsDetection(const pcl::PointCloud<pcl::PointXYZ>& points, const int closest_waypoint,
+                         const autoware_msgs::Lane& lane, const CrossWalk& crosswalk, const VelocitySetInfo& vs_info,
+                         int* obstacle_waypoint, ObstaclePoints* obstacle_points)
+{
+  std::cout<<"points callback "<<std::endl;
+  // no input for detection || no closest waypoint
+  if ((points.empty() == true && vs_info.getDetectionResultByOtherNodes() == -1) || closest_waypoint < 0)
+    return EControl::KEEP;
+
+
+  EObstacleType obstacle_type = EObstacleType::NONE;
+  int stop_obstacle_waypoint =
+      detectStopObstacle(points, closest_waypoint, lane, crosswalk, vs_info.getStopRange(),
+                         vs_info.getPointsThreshold(), vs_info.getLocalizerPose(),
+                         obstacle_points, &obstacle_type, vs_info.getDetectionResultByOtherNodes());
+
+  // skip searching deceleration range
+  if (vs_info.getDecelerationRange() < 0.01)
+  {
+    *obstacle_waypoint = stop_obstacle_waypoint;
+    if (stop_obstacle_waypoint < 0)
+      return EControl::KEEP;
+    else if (obstacle_type == EObstacleType::ON_WAYPOINTS || obstacle_type == EObstacleType::ON_CROSSWALK)
+      return EControl::STOP;
+    else if (obstacle_type == EObstacleType::STOPLINE)
+      return EControl::STOPLINE;
+    else
+      return EControl::OTHERS;
+  }
+
+  int decelerate_obstacle_waypoint =
+      detectDecelerateObstacle(points, closest_waypoint, lane, vs_info.getStopRange(), vs_info.getDecelerationRange(),
+                               vs_info.getPointsThreshold(), vs_info.getLocalizerPose(), obstacle_points);
+
+  // stop obstacle was not found
+  if (stop_obstacle_waypoint < 0)
+  {
+    *obstacle_waypoint = decelerate_obstacle_waypoint;
+    return decelerate_obstacle_waypoint < 0 ? EControl::KEEP : EControl::DECELERATE;
+  }
+
+  // stop obstacle was found but decelerate obstacle was not found
+  if (decelerate_obstacle_waypoint < 0)
+  {
+    *obstacle_waypoint = stop_obstacle_waypoint;
+    return EControl::STOP;
+  }
+
+  // about 5.0 meter
+  double waypoint_interval =
+      getPlaneDistance(lane.waypoints[0].pose.pose.position, lane.waypoints[1].pose.pose.position);
+  int stop_decelerate_threshold = 5 / waypoint_interval;
+
+  // both were found
+  if (stop_obstacle_waypoint - decelerate_obstacle_waypoint > stop_decelerate_threshold)
+  {
+    *obstacle_waypoint = decelerate_obstacle_waypoint;
+    return EControl::DECELERATE;
+  }
+  else
+  {
+    *obstacle_waypoint = stop_obstacle_waypoint;
+    return EControl::STOP;
+  }
+}
+
+EControl obstacleDetection(int closest_waypoint, const autoware_msgs::Lane& lane, const CrossWalk& crosswalk,
+                           const VelocitySetInfo vs_info, const ros::Publisher& detection_range_pub,
+                           const ros::Publisher& obstacle_pub, int* obstacle_waypoint)
+{
+  ObstaclePoints obstacle_points;
+  EControl detection_result = pointsDetection(vs_info.getPoints(), closest_waypoint, lane, crosswalk, vs_info,
+                                              obstacle_waypoint, &obstacle_points);
+  displayDetectionRange(lane, crosswalk, closest_waypoint, detection_result, *obstacle_waypoint, vs_info.getStopRange(),
+                        vs_info.getDecelerationRange(), detection_range_pub);
+
+  static int false_count = 0;
+  static EControl prev_detection = EControl::KEEP;
+  static int prev_obstacle_waypoint = -1;
+
+  // stop or decelerate because we found obstacles
+  if (detection_result == EControl::STOP || detection_result == EControl::STOPLINE || detection_result == EControl::DECELERATE)
+  {
+    displayObstacle(detection_result, obstacle_points, obstacle_pub);
+    prev_detection = detection_result;
+    false_count = 0;
+    prev_obstacle_waypoint = *obstacle_waypoint;
+    return detection_result;
+  }
+
+  // there are no obstacles, but wait a little for safety
+  if (prev_detection == EControl::STOP || prev_detection == EControl::STOPLINE || prev_detection == EControl::DECELERATE)
+  {
+    false_count++;
+
+    if (false_count < LOOP_RATE / 2)
+    {
+      *obstacle_waypoint = prev_obstacle_waypoint;
+      displayObstacle(EControl::OTHERS, obstacle_points, obstacle_pub);
+      return prev_detection;
+    }
+  }
+
+  // there are no obstacles, so we move forward
+  *obstacle_waypoint = -1;
+  false_count = 0;
+  prev_detection = EControl::KEEP;
+  return detection_result;
+}
+
+void changeWaypoints(const VelocitySetInfo& vs_info, const EControl& detection_result, int closest_waypoint,
+                     int obstacle_waypoint, const ros::Publisher& final_waypoints_pub, VelocitySetPath* vs_path)
+{
+  if (detection_result == EControl::STOP || detection_result == EControl::STOPLINE)
+  {  // STOP for obstacle/stopline
+    // stop_waypoint is about stop_distance meter away from obstacles/stoplines
+    int stop_distance = (detection_result == EControl::STOP)
+      ? vs_info.getStopDistanceObstacle() : vs_info.getStopDistanceStopline();
+    double deceleration = (detection_result == EControl::STOP)
+      ? vs_info.getDecelerationObstacle() : vs_info.getDecelerationStopline();
+    int stop_waypoint =
+        calcWaypointIndexReverse(vs_path->getPrevWaypoints(), obstacle_waypoint, stop_distance);
+    // change waypoints to stop by the stop_waypoint
+    vs_path->changeWaypointsForStopping(stop_waypoint, obstacle_waypoint, closest_waypoint, deceleration);
+    vs_path->avoidSuddenAcceleration(deceleration, closest_waypoint);
+    vs_path->avoidSuddenDeceleration(vs_info.getVelocityChangeLimit(), deceleration, closest_waypoint);
+    vs_path->setTemporalWaypoints(vs_info.getTemporalWaypointsSize(), closest_waypoint, vs_info.getControlPose());
+    final_waypoints_pub.publish(vs_path->getTemporalWaypoints());
+  }
+  else if (detection_result == EControl::DECELERATE)
+  {  // DECELERATE for obstacles
+    vs_path->initializeNewWaypoints();
+    vs_path->changeWaypointsForDeceleration(vs_info.getDecelerationObstacle(), closest_waypoint, obstacle_waypoint);
+    vs_path->avoidSuddenDeceleration(vs_info.getVelocityChangeLimit(), vs_info.getDecelerationObstacle(), closest_waypoint);
+    vs_path->avoidSuddenAcceleration(vs_info.getDecelerationObstacle(), closest_waypoint);
+    vs_path->setTemporalWaypoints(vs_info.getTemporalWaypointsSize(), closest_waypoint, vs_info.getControlPose());
+    final_waypoints_pub.publish(vs_path->getTemporalWaypoints());
+  }
+  else
+  {  // ACCELERATE or KEEP
+    vs_path->initializeNewWaypoints();
+    vs_path->avoidSuddenAcceleration(vs_info.getDecelerationObstacle(), closest_waypoint);
+    vs_path->avoidSuddenDeceleration(vs_info.getVelocityChangeLimit(), vs_info.getDecelerationObstacle(), closest_waypoint);
+    vs_path->setTemporalWaypoints(vs_info.getTemporalWaypointsSize(), closest_waypoint, vs_info.getControlPose());
+    final_waypoints_pub.publish(vs_path->getTemporalWaypoints());
+  }
+}
+
+}  // end namespace
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "velocity_set");
+
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  bool use_crosswalk_detection;
+  bool enable_multiple_crosswalk_detection;
+
+  std::string points_topic;
+  private_nh.param<bool>("use_crosswalk_detection", use_crosswalk_detection, true);
+  private_nh.param<bool>("enable_multiple_crosswalk_detection", enable_multiple_crosswalk_detection, true);
+
+
+  private_nh.param<std::string>("points_topic", points_topic, "points_lanes");
+
+  // class
+  CrossWalk crosswalk;
+  VelocitySetPath vs_path;
+  VelocitySetInfo vs_info;
+
+  
+
+  // velocity set subscriber
+  ros::Subscriber waypoints_sub = nh.subscribe("safety_waypoints", 1, &VelocitySetPath::waypointsCallback, &vs_path);
+  ros::Subscriber current_vel_sub =
+      nh.subscribe("current_velocity", 1, &VelocitySetPath::currentVelocityCallback, &vs_path);
+
+
+
+  // velocity set info subscriber
+  ros::Subscriber config_sub = nh.subscribe("config/velocity_set", 1, &VelocitySetInfo::configCallback, &vs_info);
+  ros::Subscriber points_sub = nh.subscribe(points_topic, 1, &VelocitySetInfo::pointsCallback, &vs_info);
+  ros::Subscriber localizer_sub = nh.subscribe("localizer_pose", 1, &VelocitySetInfo::localizerPoseCallback, &vs_info);
+  ros::Subscriber control_pose_sub = nh.subscribe("current_pose", 1, &VelocitySetInfo::controlPoseCallback, &vs_info);
+  ros::Subscriber detectionresult_sub = nh.subscribe("/state/stopline_wpidx", 1, &VelocitySetInfo::detectionCallback, &vs_info);
+
+
+
+  // vector map subscriber
+  ros::Subscriber sub_dtlane = nh.subscribe("vector_map_info/cross_walk", 1, &CrossWalk::crossWalkCallback, &crosswalk);
+  ros::Subscriber sub_area = nh.subscribe("vector_map_info/area", 1, &CrossWalk::areaCallback, &crosswalk);
+  ros::Subscriber sub_line = nh.subscribe("vector_map_info/line", 1, &CrossWalk::lineCallback, &crosswalk);
+  ros::Subscriber sub_point = nh.subscribe("vector_map_info/point", 1, &CrossWalk::pointCallback, &crosswalk);
+
+  // publisher
+  ros::Publisher detection_range_pub = nh.advertise<visualization_msgs::MarkerArray>("detection_range", 1);
+  ros::Publisher obstacle_pub = nh.advertise<visualization_msgs::Marker>("obstacle", 1);
+  ros::Publisher obstacle_waypoint_pub = nh.advertise<std_msgs::Int32>("obstacle_waypoint", 1, true);
+  ros::Publisher stopline_waypoint_pub = nh.advertise<std_msgs::Int32>("stopline_waypoint", 1, true);
+
+  ros::Publisher final_waypoints_pub;
+  final_waypoints_pub = nh.advertise<autoware_msgs::Lane>("final_waypoints", 1, true);
+
+  ros::Rate loop_rate(LOOP_RATE);
+  while (ros::ok())
+  {
+    ros::spinOnce();
+
+    int closest_waypoint = 0;
+
+    if (crosswalk.loaded_all && !crosswalk.set_points)
+      crosswalk.setCrossWalkPoints();
+
+    if (!vs_info.getSetPose() || !vs_path.getSetPath() || vs_path.getPrevWaypointsSize() == 0)
+    {
+      loop_rate.sleep();
+      continue;
+    }
+
+    crosswalk.setMultipleDetectionFlag(enable_multiple_crosswalk_detection);
+
+    if (use_crosswalk_detection)
+      crosswalk.setDetectionWaypoint(
+          crosswalk.findClosestCrosswalk(closest_waypoint, vs_path.getPrevWaypoints(), STOP_SEARCH_DISTANCE));
+
+    int obstacle_waypoint = -1;
+    EControl detection_result = obstacleDetection(closest_waypoint, vs_path.getPrevWaypoints(), crosswalk, vs_info,
+                                                  detection_range_pub, obstacle_pub, &obstacle_waypoint);
+
+    changeWaypoints(vs_info, detection_result, closest_waypoint,
+                    obstacle_waypoint, final_waypoints_pub, &vs_path);
+
+    vs_info.clearPoints();
+
+    // publish obstacle waypoint index
+    std_msgs::Int32 obstacle_waypoint_index;
+    std_msgs::Int32 stopline_waypoint_index;
+    if (detection_result == EControl::STOP)
+    {
+      obstacle_waypoint_index.data = obstacle_waypoint;
+      stopline_waypoint_index.data = -1;
+    }
+    else if (detection_result == EControl::STOPLINE)
+    {
+      obstacle_waypoint_index.data = -1;
+      stopline_waypoint_index.data = obstacle_waypoint;
+    }
+    else
+    {
+      obstacle_waypoint_index.data = -1;
+      stopline_waypoint_index.data = -1;
+    }
+    obstacle_waypoint_pub.publish(obstacle_waypoint_index);
+    stopline_waypoint_pub.publish(stopline_waypoint_index);
+
+    vs_path.resetFlag();
+
+    loop_rate.sleep();
+  }
+
+  return 0;
+}

src/ros/catkin/src/waypoint_planner/src/velocity_set/velocity_set_info.cpp

@@ -0,0 +1,131 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <waypoint_planner/velocity_set/velocity_set_info.h>
+
+void joinPoints(const pcl::PointCloud<pcl::PointXYZ>& points1, pcl::PointCloud<pcl::PointXYZ>* points2)
+{
+  for (const auto& p : points1)
+  {
+    points2->push_back(p);
+  }
+}
+
+VelocitySetInfo::VelocitySetInfo()
+  : stop_range_(1.3),
+    deceleration_range_(0),
+    points_threshold_(10),
+    detection_height_top_(0.2),
+    detection_height_bottom_(-1.7),
+    stop_distance_obstacle_(10),
+    stop_distance_stopline_(5),
+    deceleration_obstacle_(0.8),
+    deceleration_stopline_(0.6),
+    velocity_change_limit_(2.77),
+    temporal_waypoints_size_(100),
+    set_pose_(false),
+    wpidx_detectionResultByOtherNodes_(-1)
+{
+  ros::NodeHandle private_nh_("~");
+  ros::NodeHandle nh;
+
+  double vel_change_limit_kph = 9.972;
+  private_nh_.param<double>("remove_points_upto", remove_points_upto_, 2.3);
+  private_nh_.param<double>("stop_distance_obstacle", stop_distance_obstacle_, 10.0);
+  private_nh_.param<double>("stop_distance_stopline", stop_distance_stopline_, 5.0);
+  private_nh_.param<double>("detection_range", stop_range_, 1.3);
+  private_nh_.param<int>("points_threshold", points_threshold_, 10);
+  private_nh_.param<double>("detection_height_top", detection_height_top_, 0.2);
+  private_nh_.param<double>("detection_height_bottom", detection_height_bottom_, -1.7);
+  private_nh_.param<double>("deceleration_obstacle", deceleration_obstacle_, 0.8);
+  private_nh_.param<double>("deceleration_stopline", deceleration_stopline_, 0.6);
+  private_nh_.param<double>("velocity_change_limit", vel_change_limit_kph, 9.972);
+  private_nh_.param<double>("deceleration_range", deceleration_range_, 0);
+  private_nh_.param<double>("temporal_waypoints_size", temporal_waypoints_size_, 100.0);
+
+  velocity_change_limit_ = vel_change_limit_kph / 3.6;  // kph -> mps
+
+  health_checker_ptr_ = std::make_shared<autoware_health_checker::HealthChecker>(nh,private_nh_);
+  health_checker_ptr_->ENABLE();
+}
+
+VelocitySetInfo::~VelocitySetInfo()
+{
+}
+
+void VelocitySetInfo::clearPoints()
+{
+  points_.clear();
+}
+
+void VelocitySetInfo::configCallback(const autoware_config_msgs::ConfigVelocitySetConstPtr &config)
+{
+  stop_distance_obstacle_ = config->stop_distance_obstacle;
+  stop_distance_stopline_ = config->stop_distance_stopline;
+  stop_range_ = config->detection_range;
+  points_threshold_ = config->threshold_points;
+  detection_height_top_ = config->detection_height_top;
+  detection_height_bottom_ = config->detection_height_bottom;
+  deceleration_obstacle_ = config->deceleration_obstacle;
+  deceleration_stopline_ = config->deceleration_stopline;
+  velocity_change_limit_ = config->velocity_change_limit / 3.6; // kmph -> mps
+  deceleration_range_ = config->deceleration_range;
+  temporal_waypoints_size_ = config->temporal_waypoints_size;
+}
+
+void VelocitySetInfo::pointsCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
+{
+  health_checker_ptr_->CHECK_RATE("topic_rate_points_no_ground_slow", 8, 5, 1, "topic points_no_ground subscribe rate slow.");
+  pcl::PointCloud<pcl::PointXYZ> sub_points;
+  pcl::fromROSMsg(*msg, sub_points);
+
+  points_.clear();
+  for (const auto &v : sub_points)
+  {
+    if (v.x == 0 && v.y == 0)
+      continue;
+
+    if (v.z > detection_height_top_ || v.z < detection_height_bottom_)
+      continue;
+
+    // ignore points nearby the vehicle
+    if (v.x * v.x + v.y * v.y < remove_points_upto_ * remove_points_upto_)
+      continue;
+
+    points_.push_back(v);
+  }
+
+}
+
+void VelocitySetInfo::detectionCallback(const std_msgs::Int32 &msg)
+{
+    wpidx_detectionResultByOtherNodes_ = msg.data;
+}
+
+void VelocitySetInfo::controlPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg)
+{
+  control_pose_ = *msg;
+
+  if (!set_pose_)
+    set_pose_ = true;
+}
+
+void VelocitySetInfo::localizerPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg)
+{
+  health_checker_ptr_->NODE_ACTIVATE();
+  health_checker_ptr_->CHECK_RATE("topic_rate_localizer_pose_slow", 8, 5, 1, "topic localizer_pose subscribe rate slow.");
+  localizer_pose_ = *msg;
+}

src/ros/catkin/src/waypoint_planner/src/velocity_set/velocity_set_path.cpp

@@ -0,0 +1,239 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <waypoint_planner/velocity_set/velocity_set_path.h>
+
+VelocitySetPath::VelocitySetPath()
+  : set_path_(false),
+    current_vel_(0)
+{
+  ros::NodeHandle private_nh_("~");
+  private_nh_.param<double>("velocity_offset", velocity_offset_, 1.2);
+  private_nh_.param<double>("decelerate_vel_min", decelerate_vel_min_, 1.3);
+}
+
+VelocitySetPath::~VelocitySetPath()
+{
+}
+
+// check if waypoint number is valid
+bool VelocitySetPath::checkWaypoint(int num, const char *name) const
+{
+  if (num < 0 || num >= getPrevWaypointsSize())
+  {
+    return false;
+  }
+  return true;
+}
+
+// set about '_temporal_waypoints_size' meter waypoints from closest waypoint
+void VelocitySetPath::setTemporalWaypoints(int temporal_waypoints_size, int closest_waypoint, geometry_msgs::PoseStamped control_pose)
+{
+  if (closest_waypoint < 0)
+    return;
+
+  temporal_waypoints_.waypoints.clear();
+  temporal_waypoints_.header = new_waypoints_.header;
+  temporal_waypoints_.increment = new_waypoints_.increment;
+
+  // push current pose
+  autoware_msgs::Waypoint current_point;
+  current_point.pose = control_pose;
+  current_point.twist = new_waypoints_.waypoints[closest_waypoint].twist;
+  current_point.dtlane = new_waypoints_.waypoints[closest_waypoint].dtlane;
+  temporal_waypoints_.waypoints.push_back(current_point);
+
+  for (int i = 0; i < temporal_waypoints_size; i++)
+  {
+    if (closest_waypoint + i >= getNewWaypointsSize())
+      return;
+
+    temporal_waypoints_.waypoints.push_back(new_waypoints_.waypoints[closest_waypoint + i]);
+  }
+
+  return;
+}
+
+double VelocitySetPath::calcChangedVelocity(const double& current_vel, const double& accel, const std::array<int, 2>& range) const
+{
+  static double current_velocity = current_vel;
+  static double square_vel = current_vel * current_vel;
+  if (current_velocity != current_vel)
+  {
+    current_velocity = current_vel;
+    square_vel = current_vel * current_vel;
+  }
+  return std::sqrt(square_vel + 2.0 * accel * calcInterval(range.at(0), range.at(1)));
+}
+
+void VelocitySetPath::changeWaypointsForDeceleration(double deceleration, int closest_waypoint, int obstacle_waypoint)
+{
+  int extra = 4; // for safety
+
+  // decelerate with constant deceleration
+  for (int index = obstacle_waypoint + extra; index >= closest_waypoint; index--)
+  {
+    if (!checkWaypoint(index, __FUNCTION__))
+      continue;
+
+    // v = sqrt( (v0)^2 + 2ax )
+    std::array<int, 2> range = {index, obstacle_waypoint};
+    double changed_vel = calcChangedVelocity(decelerate_vel_min_, deceleration, range);
+
+    double prev_vel = prev_waypoints_.waypoints[index].twist.twist.linear.x;
+    const int sgn = (prev_vel < 0) ? -1 : 1;
+    new_waypoints_.waypoints[index].twist.twist.linear.x = sgn * std::min(std::abs(prev_vel), changed_vel);
+  }
+
+}
+
+void VelocitySetPath::avoidSuddenAcceleration(double deceleration, int closest_waypoint)
+{
+  for (int i = 0;; i++)
+  {
+    if (!checkWaypoint(closest_waypoint + i, __FUNCTION__))
+      return;
+
+    // accelerate with constant acceleration
+    // v = root((v0)^2 + 2ax)
+    std::array<int, 2> range = {closest_waypoint, closest_waypoint + i};
+    double changed_vel = calcChangedVelocity(current_vel_, deceleration, range) + velocity_offset_;
+
+    const double& target_vel = new_waypoints_.waypoints[closest_waypoint + i].twist.twist.linear.x;
+    // Don't exceed original velocity
+    if (changed_vel > std::abs(target_vel))
+      return;
+
+    const int sgn = (target_vel < 0) ? -1 : 1;
+    new_waypoints_.waypoints[closest_waypoint + i].twist.twist.linear.x = sgn * changed_vel;
+  }
+
+  return;
+}
+
+void VelocitySetPath::avoidSuddenDeceleration(double velocity_change_limit, double deceleration, int closest_waypoint)
+{
+  if (closest_waypoint < 0)
+    return;
+
+  const double& closest_vel = new_waypoints_.waypoints[closest_waypoint].twist.twist.linear.x;
+
+  // if accelerating, do not modify the speed profile.
+  if ((current_vel_ >= 0.0 && current_vel_ <= closest_vel) || (current_vel_ < 0.0 && current_vel_ > closest_vel))
+    return;
+
+  // not avoid braking
+  if (std::abs(current_vel_ - closest_vel) < velocity_change_limit)
+    return;
+
+  //std::cout << "avoid sudden braking!" << std::endl;
+  for (int i = 0;; i++)
+  {
+    if (!checkWaypoint(closest_waypoint + i, __FUNCTION__))
+      return;
+
+    // sqrt(v^2 - 2ax)
+    std::array<int, 2> range = {closest_waypoint, closest_waypoint + i};
+    double changed_vel = calcChangedVelocity(std::abs(current_vel_) - velocity_change_limit, -deceleration, range);
+    const double& target_vel = new_waypoints_.waypoints[closest_waypoint + i].twist.twist.linear.x;
+
+    if (std::isnan(changed_vel))
+    {
+      break;
+    }
+    const int sgn = (target_vel < 0) ? -1 : 1;
+    new_waypoints_.waypoints[closest_waypoint + i].twist.twist.linear.x = sgn * changed_vel;
+  }
+
+}
+
+void VelocitySetPath::changeWaypointsForStopping(int stop_waypoint, int obstacle_waypoint, int closest_waypoint, double deceleration)
+{
+  if (closest_waypoint < 0)
+    return;
+
+  // decelerate with constant deceleration
+  for (int index = stop_waypoint; index >= closest_waypoint; index--)
+  {
+    if (!checkWaypoint(index, __FUNCTION__))
+      continue;
+
+    // v = (v0)^2 + 2ax, and v0 = 0
+    std::array<int, 2> range = {index, stop_waypoint};
+    double changed_vel = calcChangedVelocity(0.0, deceleration, range);
+
+    double prev_vel = prev_waypoints_.waypoints[index].twist.twist.linear.x;
+    const int sgn = (prev_vel < 0) ? -1 : 1;
+    new_waypoints_.waypoints[index].twist.twist.linear.x = sgn * std::min(std::abs(prev_vel), changed_vel);
+  }
+
+  // fill velocity with 0 for stopping
+  for (int i = stop_waypoint; i <= obstacle_waypoint; i++)
+  {
+    new_waypoints_.waypoints[i].twist.twist.linear.x = 0;
+  }
+
+}
+
+void VelocitySetPath::initializeNewWaypoints()
+{
+  new_waypoints_ = prev_waypoints_;
+}
+
+double VelocitySetPath::calcInterval(const int begin, const int end) const
+{
+  // check index
+  if (begin < 0 || begin >= getPrevWaypointsSize() || end < 0 || end >= getPrevWaypointsSize())
+  {
+    ROS_WARN("Invalid index");
+    return 0;
+  }
+
+  // Calculate the inteval of waypoints
+  double dist_sum = 0;
+  for (int i = begin; i < end; i++)
+  {
+    tf::Vector3 v1(prev_waypoints_.waypoints[i].pose.pose.position.x,
+                   prev_waypoints_.waypoints[i].pose.pose.position.y, 0);
+
+    tf::Vector3 v2(prev_waypoints_.waypoints[i + 1].pose.pose.position.x,
+                   prev_waypoints_.waypoints[i + 1].pose.pose.position.y, 0);
+
+    dist_sum += tf::tfDistance(v1, v2);
+  }
+
+  return dist_sum;
+}
+
+void VelocitySetPath::resetFlag()
+{
+  set_path_ = false;
+}
+
+
+void VelocitySetPath::waypointsCallback(const autoware_msgs::LaneConstPtr& msg)
+{
+  prev_waypoints_ = *msg;
+  // temporary, edit waypoints velocity later
+  new_waypoints_ = *msg;
+
+  set_path_ = true;
+}
+
+void VelocitySetPath::currentVelocityCallback(const geometry_msgs::TwistStampedConstPtr& msg)
+{
+  current_vel_ = msg->twist.linear.x;
+}

src/ros/catkin/src/waypoint_planner/src/velocity_set_lanelet2/velocity_set_lanelet2.cpp

@@ -0,0 +1,740 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include <tf/transform_listener.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/geometry/Point.h>
+
+#include <autoware_lanelet2_msgs/MapBin.h>
+#include <lanelet2_extension/utility/message_conversion.h>
+#include <lanelet2_extension/utility/query.h>
+#include <lanelet2_extension/visualization/visualization.h>
+
+#include <limits>
+#include <string>
+#include <vector>
+
+#include <waypoint_planner/velocity_set/libvelocity_set.h>
+#include <waypoint_planner/velocity_set/velocity_set_info.h>
+#include <waypoint_planner/velocity_set/velocity_set_path.h>
+
+constexpr int LOOP_RATE = 10;
+constexpr double DECELERATION_SEARCH_DISTANCE = 30;
+constexpr double STOP_SEARCH_DISTANCE = 60;
+
+static lanelet::LaneletMapPtr g_lanelet_map;
+static bool g_loaded_lanelet_map;
+lanelet::ConstLanelets g_crosswalk_lanelets;
+
+// set color according to given obstacle
+void obstacleColorByKind(const EControl kind, std_msgs::ColorRGBA* color, const double alpha = 0.5)
+{
+  if (kind == EControl::STOP)
+  {
+    // red
+    color->r = 1.0;
+    color->g = 0.0;
+    color->b = 0.0;
+    color->a = alpha;
+  }
+  else if (kind == EControl::STOPLINE)
+  {
+    // blue
+    color->r = 0.0;
+    color->g = 0.0;
+    color->b = 1.0;
+    color->a = alpha;
+  }
+  else if (kind == EControl::DECELERATE)
+  {
+    // yellow
+    color->r = 1.0;
+    color->g = 1.0;
+    color->b = 0.0;
+    color->a = alpha;
+  }
+  else
+  {
+    // white
+    color->r = 1.0;
+    color->g = 1.0;
+    color->b = 1.0;
+    color->a = alpha;
+  }
+}
+
+// Display a detected obstacle
+void displayObstacle(const EControl& kind, const ObstaclePoints& obstacle_points, const ros::Publisher& obstacle_pub)
+{
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "/map";
+  marker.header.stamp = ros::Time();
+  marker.ns = "my_namespace";
+  marker.id = 0;
+  marker.type = visualization_msgs::Marker::CUBE;
+  marker.action = visualization_msgs::Marker::ADD;
+
+  static geometry_msgs::Point prev_obstacle_point;
+  if (kind == EControl::STOP || kind == EControl::STOPLINE || kind == EControl::DECELERATE)
+  {
+    marker.pose.position = obstacle_points.getObstaclePoint(kind);
+    prev_obstacle_point = marker.pose.position;
+  }
+  else  // kind == OTHERS
+  {
+    marker.pose.position = prev_obstacle_point;
+  }
+  geometry_msgs::Quaternion quat;
+  marker.pose.orientation = quat;
+
+  marker.scale.x = 1.0;
+  marker.scale.y = 1.0;
+  marker.scale.z = 2.0;
+  marker.lifetime = ros::Duration(0.1);
+  marker.frame_locked = true;
+  obstacleColorByKind(kind, &marker.color, 0.7);
+
+  obstacle_pub.publish(marker);
+}
+
+// returns index of waypoint first crosswalk is detected (within 2 meters) from (if any -1 otherwise)
+int findClosestCrosswalk(const lanelet::ConstLanelets& crosswalks, const int closest_waypoint,
+                         const autoware_msgs::Lane& lane_msg, const int search_distance,
+                         lanelet::ConstLanelets* closest_crosswalks, bool multiple_crosswalk_detection)
+{
+  int wp_near_crosswalk = -1;
+
+  double find_distance = 2.0;  // meter
+
+  // find near crosswalk
+  for (int wpi = closest_waypoint;
+       wpi < closest_waypoint + search_distance && wpi < static_cast<int>(lane_msg.waypoints.size()); wpi++)
+  {
+    geometry_msgs::Point waypoint = lane_msg.waypoints[wpi].pose.pose.position;
+    lanelet::BasicPoint2d wp2d(waypoint.x, waypoint.y);
+    waypoint.z = 0.0;
+
+    for (const auto& ll : crosswalks)
+    {
+      if (ll.hasAttribute(lanelet::AttributeName::Subtype))
+      {
+        lanelet::Attribute attr = ll.attribute(lanelet::AttributeName::Subtype);
+
+        if (attr.value() == lanelet::AttributeValueString::Crosswalk)
+        {
+          double d = lanelet::geometry::distance2d(ll, wp2d);
+          if (d < find_distance)
+          {
+            if (std::find(closest_crosswalks->begin(), closest_crosswalks->end(), ll) == closest_crosswalks->end())
+            {
+              closest_crosswalks->push_back(ll);
+            }
+            if (!multiple_crosswalk_detection)
+            {
+              wp_near_crosswalk = wpi;
+
+              return wp_near_crosswalk;
+            }
+            if (wp_near_crosswalk == -1)
+              wp_near_crosswalk = wpi;
+          }
+        }
+      }
+    }
+  }
+
+  return wp_near_crosswalk;
+}
+
+// obstacle detection for crosswalk
+// return EControl::STOP when there are lidar points in crosswalk
+// return EControl::Keep otherwise
+EControl crossWalkDetection(const pcl::PointCloud<pcl::PointXYZ>& points,
+                            const lanelet::ConstLanelets& closest_crosswalks,
+                            const geometry_msgs::PoseStamped& localizer_pose, const int points_threshold,
+                            ObstaclePoints* obstacle_points)
+{
+  for (auto lli = closest_crosswalks.begin(); lli != closest_crosswalks.end(); lli++)
+  {
+    // get polygon in lidar frame
+    lanelet::BasicPolygon2d transformed_poly2d;
+    lanelet::BasicPolygon3d poly3d = lli->polygon3d().basicPolygon();
+
+    for (const auto& point : poly3d)
+    {
+      geometry_msgs::Point point_geom, transformed_point_geom;
+      lanelet::utils::conversion::toGeomMsgPt(point, &point_geom);
+      transformed_point_geom = calcRelativeCoordinate(point_geom, localizer_pose.pose);
+      lanelet::BasicPoint2d transformed_point2d(transformed_point_geom.x, transformed_point_geom.y);
+      transformed_poly2d.push_back(transformed_point2d);
+    }
+
+    int stop_count = 0;  // number of points in the detection area
+    for (const auto& p : points)
+    {
+      lanelet::BasicPoint2d p2d(p.x, p.y);
+      double distance = lanelet::geometry::distance(transformed_poly2d, p2d);
+
+      if (distance < std::numeric_limits<double>::epsilon())
+      {
+        stop_count++;
+        geometry_msgs::Point point_temp;
+        point_temp.x = p.x;
+        point_temp.y = p.y;
+        point_temp.z = p.z;
+        obstacle_points->setStopPoint(calcAbsoluteCoordinate(point_temp, localizer_pose.pose));
+      }
+      if (stop_count > points_threshold)
+      {
+        return EControl::STOP;
+      }
+    }
+
+    obstacle_points->clearStopPoints();
+  }
+
+  return EControl::KEEP;  // find no obstacles
+}
+
+// same as velocity_set.cpp - except for no reference to vector maps or crosswalk
+int detectStopObstacle(const pcl::PointCloud<pcl::PointXYZ>& points, const int closest_waypoint, int detection_waypoint,
+                       const autoware_msgs::Lane& lane, const lanelet::ConstLanelets& closest_crosswalks,
+                       double stop_range, double points_threshold, const geometry_msgs::PoseStamped& localizer_pose,
+                       ObstaclePoints* obstacle_points, EObstacleType* obstacle_type,
+                       const int wpidx_detection_result_by_other_nodes)
+{
+  int stop_obstacle_waypoint = -1;
+  *obstacle_type = EObstacleType::NONE;
+  // start search from the closest waypoint
+  for (int i = closest_waypoint; i < closest_waypoint + STOP_SEARCH_DISTANCE; i++)
+  {
+    // reach the end of waypoints
+    if (i >= static_cast<int>(lane.waypoints.size()))
+      break;
+
+    // detection from other nodes
+    if (wpidx_detection_result_by_other_nodes >= 0 && lane.waypoints.at(i).gid == wpidx_detection_result_by_other_nodes)
+    {
+      stop_obstacle_waypoint = i;
+      *obstacle_type = EObstacleType::STOPLINE;
+      obstacle_points->setStopPoint(lane.waypoints.at(i).pose.pose.position);  // for vizuialization
+      break;
+    }
+
+    // Detection for cross walk
+    if (i == detection_waypoint)
+    {
+      // found an obstacle in the cross walk
+      if (crossWalkDetection(points, closest_crosswalks, localizer_pose, points_threshold, obstacle_points) ==
+          EControl::STOP)
+      {
+        stop_obstacle_waypoint = i;
+        *obstacle_type = EObstacleType::ON_CROSSWALK;
+        break;
+      }
+    }
+
+    // waypoint seen by localizer
+    geometry_msgs::Point waypoint = calcRelativeCoordinate(lane.waypoints[i].pose.pose.position, localizer_pose.pose);
+    tf::Vector3 tf_waypoint = point2vector(waypoint);
+    tf_waypoint.setZ(0);
+
+    int stop_point_count = 0;
+    for (const auto& p : points)
+    {
+      tf::Vector3 point_vector(p.x, p.y, 0);
+
+      // 2D distance between waypoint and points (obstacle)
+      double dt = tf::tfDistance(point_vector, tf_waypoint);
+      if (dt < stop_range)
+      {
+        stop_point_count++;
+        geometry_msgs::Point point_temp;
+        point_temp.x = p.x;
+        point_temp.y = p.y;
+        point_temp.z = p.z;
+        obstacle_points->setStopPoint(calcAbsoluteCoordinate(point_temp, localizer_pose.pose));
+      }
+    }
+
+    // there is an obstacle if the number of points exceeded the threshold
+    if (stop_point_count > points_threshold)
+    {
+      stop_obstacle_waypoint = i;
+      *obstacle_type = EObstacleType::ON_WAYPOINTS;
+      break;
+    }
+
+    obstacle_points->clearStopPoints();
+
+    // check next waypoint...
+  }
+
+  return stop_obstacle_waypoint;
+}
+
+//  same as velocity_set.cpp - expect for no reference to vector maps
+int detectDecelerateObstacle(const pcl::PointCloud<pcl::PointXYZ>& points, const int closest_waypoint,
+                             const autoware_msgs::Lane& lane, const double stop_range, const double deceleration_range,
+                             const double points_threshold, const geometry_msgs::PoseStamped& localizer_pose,
+                             ObstaclePoints* obstacle_points)
+{
+  int decelerate_obstacle_waypoint = -1;
+  // start search from the closest waypoint
+  for (int i = closest_waypoint; i < closest_waypoint + DECELERATION_SEARCH_DISTANCE; i++)
+  {
+    // reach the end of waypoints
+    if (i >= static_cast<int>(lane.waypoints.size()))
+      break;
+
+    // waypoint seen by localizer
+    geometry_msgs::Point waypoint = calcRelativeCoordinate(lane.waypoints[i].pose.pose.position, localizer_pose.pose);
+    tf::Vector3 tf_waypoint = point2vector(waypoint);
+    tf_waypoint.setZ(0);
+
+    int decelerate_point_count = 0;
+    for (const auto& p : points)
+    {
+      tf::Vector3 point_vector(p.x, p.y, 0);
+
+      // 2D distance between waypoint and points (obstacle)
+      double dt = tf::tfDistance(point_vector, tf_waypoint);
+      if (dt > stop_range && dt < stop_range + deceleration_range)
+      {
+        decelerate_point_count++;
+        geometry_msgs::Point point_temp;
+        point_temp.x = p.x;
+        point_temp.y = p.y;
+        point_temp.z = p.z;
+        obstacle_points->setDeceleratePoint(calcAbsoluteCoordinate(point_temp, localizer_pose.pose));
+      }
+    }
+
+    // there is an obstacle if the number of points exceeded the threshold
+    if (decelerate_point_count > points_threshold)
+    {
+      decelerate_obstacle_waypoint = i;
+      break;
+    }
+
+    obstacle_points->clearDeceleratePoints();
+
+    // check next waypoint...
+  }
+
+  return decelerate_obstacle_waypoint;
+}
+
+//-------------------------------------------------------------------------
+//  Detect an obstacle by using pointcloud
+//  same as velocity_set.cpp - no reference to vector maps or crosswalk
+//-------------------------------------------------------------------------
+EControl pointsDetection(const pcl::PointCloud<pcl::PointXYZ>& points, const int closest_waypoint,
+                         const int detection_waypoint, const autoware_msgs::Lane& lane,
+                         const lanelet::ConstLanelets& closest_crosswalks, const VelocitySetInfo& vs_info,
+                         int* obstacle_waypoint, ObstaclePoints* obstacle_points)
+{
+  // no input for detection || no closest waypoint
+  if ((points.empty() == true && vs_info.getDetectionResultByOtherNodes() == -1) || closest_waypoint < 0)
+    return EControl::KEEP;
+
+  EObstacleType obstacle_type = EObstacleType::NONE;
+  int stop_obstacle_waypoint =
+      detectStopObstacle(points, closest_waypoint, detection_waypoint, lane, closest_crosswalks, vs_info.getStopRange(),
+                         vs_info.getPointsThreshold(), vs_info.getLocalizerPose(), obstacle_points, &obstacle_type,
+                         vs_info.getDetectionResultByOtherNodes());
+
+  // skip searching deceleration range
+  if (vs_info.getDecelerationRange() < 0.01)
+  {
+    *obstacle_waypoint = stop_obstacle_waypoint;
+    if (stop_obstacle_waypoint < 0)
+      return EControl::KEEP;
+    else if (obstacle_type == EObstacleType::ON_WAYPOINTS || obstacle_type == EObstacleType::ON_CROSSWALK)
+      return EControl::STOP;
+    else if (obstacle_type == EObstacleType::STOPLINE)
+      return EControl::STOPLINE;
+    else
+      return EControl::OTHERS;
+  }
+
+  int decelerate_obstacle_waypoint =
+      detectDecelerateObstacle(points, closest_waypoint, lane, vs_info.getStopRange(), vs_info.getDecelerationRange(),
+                               vs_info.getPointsThreshold(), vs_info.getLocalizerPose(), obstacle_points);
+
+  // stop obstacle was not found
+  if (stop_obstacle_waypoint < 0)
+  {
+    *obstacle_waypoint = decelerate_obstacle_waypoint;
+    return decelerate_obstacle_waypoint < 0 ? EControl::KEEP : EControl::DECELERATE;
+  }
+
+  // stop obstacle was found but decelerate obstacle was not found
+  if (decelerate_obstacle_waypoint < 0)
+  {
+    *obstacle_waypoint = stop_obstacle_waypoint;
+    return EControl::STOP;
+  }
+
+  // about 5.0 meter
+  double waypoint_interval =
+      getPlaneDistance(lane.waypoints[0].pose.pose.position, lane.waypoints[1].pose.pose.position);
+  int stop_decelerate_threshold = 5 / waypoint_interval;
+
+  // both were found
+  if (stop_obstacle_waypoint - decelerate_obstacle_waypoint > stop_decelerate_threshold)
+  {
+    *obstacle_waypoint = decelerate_obstacle_waypoint;
+    return EControl::DECELERATE;
+  }
+  else
+  {
+    *obstacle_waypoint = stop_obstacle_waypoint;
+    return EControl::STOP;
+  }
+}
+
+// visualization of stoplines, crosswalks, and detection range
+void displayDetectionRange(const autoware_msgs::Lane& lane, const lanelet::ConstLanelets& closest_crosswalks,
+                           const int closest_waypoint, const int detection_waypoint, const EControl& kind,
+                           const int obstacle_waypoint, const double stop_range, const double deceleration_range,
+                           const ros::Publisher& detection_range_pub)
+{
+  // set up for marker array
+  visualization_msgs::MarkerArray marker_array;
+  visualization_msgs::Marker crosswalk_marker;
+  visualization_msgs::Marker waypoint_marker_stop;
+  visualization_msgs::Marker waypoint_marker_decelerate;
+  visualization_msgs::Marker stop_line;
+  crosswalk_marker.header.frame_id = "/map";
+  crosswalk_marker.header.stamp = ros::Time();
+  crosswalk_marker.id = 0;
+  crosswalk_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+  crosswalk_marker.action = visualization_msgs::Marker::ADD;
+  waypoint_marker_stop = crosswalk_marker;
+  waypoint_marker_decelerate = crosswalk_marker;
+  stop_line = crosswalk_marker;
+  stop_line.type = visualization_msgs::Marker::CUBE;
+
+  // set each namespace
+  crosswalk_marker.ns = "Crosswalk Detection";
+  waypoint_marker_stop.ns = "Stop Detection";
+  waypoint_marker_decelerate.ns = "Decelerate Detection";
+  stop_line.ns = "Stop Line";
+
+  // set scale and color
+  double scale = 2 * stop_range;
+  waypoint_marker_stop.scale.x = scale;
+  waypoint_marker_stop.scale.y = scale;
+  waypoint_marker_stop.scale.z = scale;
+  waypoint_marker_stop.color.a = 0.2;
+  waypoint_marker_stop.color.r = 0.0;
+  waypoint_marker_stop.color.g = 1.0;
+  waypoint_marker_stop.color.b = 0.0;
+  waypoint_marker_stop.frame_locked = true;
+
+  scale = 2 * (stop_range + deceleration_range);
+  waypoint_marker_decelerate.scale.x = scale;
+  waypoint_marker_decelerate.scale.y = scale;
+  waypoint_marker_decelerate.scale.z = scale;
+  waypoint_marker_decelerate.color.a = 0.15;
+  waypoint_marker_decelerate.color.r = 1.0;
+  waypoint_marker_decelerate.color.g = 1.0;
+  waypoint_marker_decelerate.color.b = 0.0;
+  waypoint_marker_decelerate.frame_locked = true;
+
+  if (obstacle_waypoint > -1)
+  {
+    stop_line.pose.position = lane.waypoints[obstacle_waypoint].pose.pose.position;
+    stop_line.pose.orientation = lane.waypoints[obstacle_waypoint].pose.pose.orientation;
+  }
+  stop_line.pose.position.z += 1.0;
+  stop_line.scale.x = 0.1;
+  stop_line.scale.y = 15.0;
+  stop_line.scale.z = 2.0;
+  stop_line.lifetime = ros::Duration(0.1);
+  stop_line.frame_locked = true;
+  obstacleColorByKind(kind, &stop_line.color, 0.3);
+
+  // int crosswalk_id = crosswalk.getDetectionCrossWalkID();
+  crosswalk_marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
+  crosswalk_marker.scale.x = 1.0;
+  crosswalk_marker.scale.y = 1.0;
+  crosswalk_marker.scale.z = 1.0;
+  crosswalk_marker.color.a = 0.5;
+  crosswalk_marker.color.r = 0.0;
+  crosswalk_marker.color.g = 1.0;
+  crosswalk_marker.color.b = 0.0;
+
+  for (const auto crosswalk_ll : closest_crosswalks)
+  {
+    std::vector<geometry_msgs::Polygon> triangles;
+    lanelet::visualization::lanelet2Triangle(crosswalk_ll, &triangles);
+    for (const auto t : triangles)
+    {
+      for (const auto p_32 : t.points)
+      {
+        geometry_msgs::Point pt;
+        lanelet::utils::conversion::toGeomMsgPt(p_32, &pt);
+        crosswalk_marker.points.push_back(pt);
+      }
+    }
+  }
+
+  // inverse to correct direction of triangle
+  std::reverse(crosswalk_marker.points.begin(), crosswalk_marker.points.end());
+  crosswalk_marker.frame_locked = true;
+
+  // set marker points coordinate
+  for (int i = 0; i < STOP_SEARCH_DISTANCE; i++)
+  {
+    if (closest_waypoint < 0 || i + closest_waypoint > static_cast<int>(lane.waypoints.size()) - 1)
+      break;
+
+    geometry_msgs::Point point;
+    point = lane.waypoints[closest_waypoint + i].pose.pose.position;
+
+    waypoint_marker_stop.points.push_back(point);
+
+    if (i > DECELERATION_SEARCH_DISTANCE)
+      continue;
+    waypoint_marker_decelerate.points.push_back(point);
+  }
+
+  marker_array.markers.push_back(crosswalk_marker);
+  marker_array.markers.push_back(waypoint_marker_stop);
+  marker_array.markers.push_back(waypoint_marker_decelerate);
+  if (kind != EControl::KEEP)
+    marker_array.markers.push_back(stop_line);
+  detection_range_pub.publish(marker_array);
+  marker_array.markers.clear();
+}
+
+// same as velocity_set.cpp - except for no reference to vector maps
+EControl obstacleDetection(int closest_waypoint, int detection_waypoint, const autoware_msgs::Lane& lane,
+                           const lanelet::ConstLanelets& closest_crosswalks, const VelocitySetInfo vs_info,
+                           const ros::Publisher& detection_range_pub, const ros::Publisher& obstacle_pub,
+                           int* obstacle_waypoint)
+{
+  ObstaclePoints obstacle_points;
+
+  EControl detection_result = pointsDetection(vs_info.getPoints(), closest_waypoint, detection_waypoint, lane,
+                                              closest_crosswalks,  // crosswalk,
+                                              vs_info, obstacle_waypoint, &obstacle_points);
+
+  displayDetectionRange(lane, closest_crosswalks, closest_waypoint, detection_waypoint, detection_result,
+                        *obstacle_waypoint, vs_info.getStopRange(), vs_info.getDecelerationRange(),
+                        detection_range_pub);
+
+  static int false_count = 0;
+  static EControl prev_detection = EControl::KEEP;
+  static int prev_obstacle_waypoint = -1;
+
+  // stop or decelerate because we found obstacles
+  if (detection_result == EControl::STOP || detection_result == EControl::STOPLINE ||
+      detection_result == EControl::DECELERATE)
+  {
+    displayObstacle(detection_result, obstacle_points, obstacle_pub);
+    prev_detection = detection_result;
+    false_count = 0;
+    prev_obstacle_waypoint = *obstacle_waypoint;
+    return detection_result;
+  }
+
+  // there are no obstacles, but wait a little for safety
+  if (prev_detection == EControl::STOP || prev_detection == EControl::STOPLINE ||
+      prev_detection == EControl::DECELERATE)
+  {
+    false_count++;
+
+    if (false_count < LOOP_RATE / 2)
+    {
+      *obstacle_waypoint = prev_obstacle_waypoint;
+      displayObstacle(EControl::OTHERS, obstacle_points, obstacle_pub);
+      return prev_detection;
+    }
+  }
+
+  // there are no obstacles, so we move forward
+  *obstacle_waypoint = -1;
+  false_count = 0;
+  prev_detection = EControl::KEEP;
+
+  return detection_result;
+}
+
+// change velocity according to detected obstacles and stoplines
+// same as velocity_set.cpp
+void changeWaypoints(const VelocitySetInfo& vs_info, const EControl& detection_result, int closest_waypoint,
+                     int obstacle_waypoint, const ros::Publisher& final_waypoints_pub, VelocitySetPath* vs_path)
+{
+  int stop_distance =
+      (detection_result == EControl::STOPLINE) ? vs_info.getStopDistanceStopline() : vs_info.getStopDistanceObstacle();
+  double deceleration =
+      (detection_result == EControl::STOPLINE) ? vs_info.getDecelerationStopline() : vs_info.getDecelerationObstacle();
+  int stop_waypoint = calcWaypointIndexReverse(vs_path->getPrevWaypoints(), obstacle_waypoint, stop_distance);
+
+  if (detection_result == EControl::STOP || detection_result == EControl::STOPLINE)  // STOP for obstacle/stopline
+  {  // stop_waypoint is about stop_distance meter away from obstacles/stoplines
+    // change waypoints to stop by the stop_waypoint
+    vs_path->changeWaypointsForStopping(stop_waypoint, obstacle_waypoint, closest_waypoint, deceleration);
+  }
+  else if (detection_result == EControl::DECELERATE)  // DECELERATE for obstacles
+  {
+    vs_path->initializeNewWaypoints();
+    vs_path->changeWaypointsForDeceleration(vs_info.getDecelerationObstacle(), closest_waypoint, obstacle_waypoint);
+  }
+  else
+  {  // ACCELERATE or KEEP
+    vs_path->initializeNewWaypoints();
+  }
+
+  vs_path->avoidSuddenAcceleration(deceleration, closest_waypoint);
+  vs_path->avoidSuddenDeceleration(vs_info.getVelocityChangeLimit(), deceleration, closest_waypoint);
+  vs_path->setTemporalWaypoints(vs_info.getTemporalWaypointsSize(), closest_waypoint, vs_info.getControlPose());
+  final_waypoints_pub.publish(vs_path->getTemporalWaypoints());
+}
+
+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;
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(g_lanelet_map);
+  g_crosswalk_lanelets = lanelet::utils::query::crosswalkLanelets(all_lanelets);
+  ROS_INFO("velocity_set_lanelet2: lanelet map loaded\n");
+}
+
+int main(int argc, char** argv)
+{
+  g_loaded_lanelet_map = false;
+
+  ros::init(argc, argv, "velocity_set");
+  ros::NodeHandle rosnode;
+
+  ros::NodeHandle private_rosnode("~");
+
+  // parameters from ros param
+  bool use_crosswalk_detection;
+  bool enable_multiple_crosswalk_detection;
+  bool enablePlannerDynamicSwitch;
+  std::string points_topic;
+
+  private_rosnode.param<bool>("use_crosswalk_detection", use_crosswalk_detection, true);
+  private_rosnode.param<bool>("enable_multiple_crosswalk_detection", enable_multiple_crosswalk_detection, true);
+  private_rosnode.param<bool>("enablePlannerDynamicSwitch", enablePlannerDynamicSwitch, false);
+  private_rosnode.param<std::string>("points_topic", points_topic, "points_lanes");
+
+  VelocitySetPath vs_path;
+  VelocitySetInfo vs_info;
+
+  // map subscriber
+  ros::Subscriber bin_map_sub = rosnode.subscribe("lanelet_map_bin", 1, binMapCallback);
+
+  // velocity set path subscriber
+  ros::Subscriber waypoints_sub =
+      rosnode.subscribe("safety_waypoints", 1, &VelocitySetPath::waypointsCallback, &vs_path);
+  ros::Subscriber current_vel_sub =
+      rosnode.subscribe("current_velocity", 1, &VelocitySetPath::currentVelocityCallback, &vs_path);
+
+  // velocity set info subscriber
+  ros::Subscriber config_sub = rosnode.subscribe("config/velocity_set", 1, &VelocitySetInfo::configCallback, &vs_info);
+  ros::Subscriber points_sub = rosnode.subscribe(points_topic, 1, &VelocitySetInfo::pointsCallback, &vs_info);
+  ros::Subscriber localizer_sub =
+      rosnode.subscribe("localizer_pose", 1, &VelocitySetInfo::localizerPoseCallback, &vs_info);
+  ros::Subscriber control_pose_sub =
+      rosnode.subscribe("current_pose", 1, &VelocitySetInfo::controlPoseCallback, &vs_info);
+  ros::Subscriber detectionresult_sub =
+      rosnode.subscribe("state/stopline_wpidx", 1, &VelocitySetInfo::detectionCallback, &vs_info);
+
+  // publisher
+  ros::Publisher detection_range_pub = rosnode.advertise<visualization_msgs::MarkerArray>("detection_range", 1);
+  ros::Publisher obstacle_pub = rosnode.advertise<visualization_msgs::Marker>("obstacle", 1);
+  ros::Publisher obstacle_waypoint_pub = rosnode.advertise<std_msgs::Int32>("obstacle_waypoint", 1, true);
+  ros::Publisher stopline_waypoint_pub = rosnode.advertise<std_msgs::Int32>("stopline_waypoint", 1, true);
+
+  ros::Publisher final_waypoints_pub;
+  final_waypoints_pub = rosnode.advertise<autoware_msgs::Lane>("final_waypoints", 1, true);
+
+  ros::Rate loop_rate(LOOP_RATE);
+  while (ros::ok())
+  {
+    ros::spinOnce();
+
+    int closest_waypoint = 0;
+
+    if (!vs_info.getSetPose() || !vs_path.getSetPath())
+    {
+      loop_rate.sleep();
+      continue;
+    }
+
+    int detection_waypoint = -1;
+    lanelet::ConstLanelets closest_crosswalks;
+    if (use_crosswalk_detection)
+    {
+      if (!g_loaded_lanelet_map)
+      {
+        ROS_WARN("use_crosswalk_detection is true, but lanelet map is not loaded!");
+      }
+      detection_waypoint =
+          findClosestCrosswalk(g_crosswalk_lanelets, closest_waypoint, vs_path.getPrevWaypoints(), STOP_SEARCH_DISTANCE,
+                               &closest_crosswalks, enable_multiple_crosswalk_detection);
+    }
+
+    int obstacle_waypoint = -1;
+    EControl detection_result =
+        obstacleDetection(closest_waypoint, detection_waypoint, vs_path.getPrevWaypoints(), closest_crosswalks, vs_info,
+                          detection_range_pub, obstacle_pub, &obstacle_waypoint);
+
+    changeWaypoints(vs_info, detection_result, closest_waypoint, obstacle_waypoint, final_waypoints_pub, &vs_path);
+
+    vs_info.clearPoints();
+
+    // publish obstacle waypoint index
+    std_msgs::Int32 obstacle_waypoint_index;
+    std_msgs::Int32 stopline_waypoint_index;
+    if (detection_result == EControl::STOP)
+    {
+      obstacle_waypoint_index.data = obstacle_waypoint;
+      stopline_waypoint_index.data = -1;
+    }
+    else if (detection_result == EControl::STOPLINE)
+    {
+      obstacle_waypoint_index.data = -1;
+      stopline_waypoint_index.data = obstacle_waypoint;
+    }
+    else
+    {
+      obstacle_waypoint_index.data = -1;
+      stopline_waypoint_index.data = -1;
+    }
+    obstacle_waypoint_pub.publish(obstacle_waypoint_index);
+    stopline_waypoint_pub.publish(stopline_waypoint_index);
+
+    vs_path.resetFlag();
+    loop_rate.sleep();
+  }
+
+  return 0;
+}