add some autoware project to catkin folder,test a star, ok.

src/ros/catkin/src/adc_transform/CMakeLists.txt

@@ -0,0 +1,93 @@
+cmake_minimum_required(VERSION 2.8.11)
+project(adc_transform)
+
+
+
+
+
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  pluginlib
+  sensor_msgs
+  tf
+)
+find_package(Boost REQUIRED)
+find_package(Protobuf REQUIRED)
+
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_AUTORCC ON)
+
+
+if(CMAKE_COMPILER_IS_GNUCXX)
+    set(CMAKE_CXX_FLAGS "-fPIC ${CMAKE_CXX_FLAGS}")
+    message(STATUS "optional:-fPIC")   
+endif(CMAKE_COMPILER_IS_GNUCXX)
+
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+#find_package(Qt5Core  REQUIRED)
+#qt5_wrap_cpp(MOC src/qt_ros_test.h)
+#qt5_wrap_ui(UIC src/qt_ros_test.ui)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+##   INCLUDE_DIRS include
+#  LIBRARIES client_plugin
+   CATKIN_DEPENDS 
+    roscpp std_msgs 
+    sensor_msgs pluginlib
+  DEPENDS
+    Boost
+    Protobuf
+    tf
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${catkin_INCLUDE_DIRS} include
+  ${Boost_INCLUDE_DIR}
+  ${autoware_msgs_INCLUDE_DIRS}
+)
+
+## Declare a C++ executable
+add_executable(adc_transform src/main.cpp  )
+target_link_libraries(adc_transform ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES} )
+
+
+install(
+  TARGETS adc_transform
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

src/ros/catkin/src/adc_transform/package.xml

@@ -0,0 +1,63 @@
+<?xml version="1.0"?>
+<package>
+  <name>adc_transform</name>
+  <version>0.0.0</version>
+  <description>The adc_transform package for trasform</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="hans@todo.todo">hans</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/client_plugin</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+  <build_depend>pluginlib</build_depend>
+  <run_depend>pluginlib</run_depend>
+
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+  <build_depend>tf</build_depend>
+  <run_depend>tf</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/ros/catkin/src/adc_transform/src/main.cpp

@@ -0,0 +1,71 @@
+#include "ros/ros.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+
+#include <thread>
+
+
+static std::string _pose_topic = "/current_pose";
+static std::string _twist_topic = "/current_velocity";
+
+
+ros::Publisher pose_pub;
+ros::Publisher twist_pub;
+ros::Publisher localizer_pub;
+
+ros::Subscriber pose_sub;
+
+void currentPoseCallback(const geometry_msgs::PoseStamped& msg)
+{
+    static tf::TransformBroadcaster br;
+    
+    std::cout<<"receive pos "<<std::endl;
+    tf::Transform transform;
+    tf::Quaternion current_q;
+    current_q.setW(msg.pose.orientation.w);
+    current_q.setX(msg.pose.orientation.x);
+    current_q.setY(msg.pose.orientation.y);
+    current_q.setZ(msg.pose.orientation.z);
+
+    transform.setOrigin(tf::Vector3(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z));
+    // transform.setOrigin(tf::Vector3(2.0*sin(ros::Time::now().toSec()),2.0*cos(ros::Time::now().toSec()),0.0));
+    transform.setRotation(current_q);
+
+    ros::Time current_scan_time = ros::Time::now();
+    br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/velodyne"));
+}
+
+
+int main(int argc, char *argv[])
+{
+    ros::init(argc, argv, "adc_transform");
+    //	ros::NodeHandle n;
+    ros::NodeHandle private_nh("~");
+    ros::Rate loop_rate(100);
+
+    private_nh.getParam("pose_topic", _pose_topic);
+    std::cout << "pose_topic  is " << _pose_topic << std::endl;
+
+    pose_sub = private_nh.subscribe(_pose_topic, 1, currentPoseCallback);
+
+    while(ros::ok())
+    {
+        
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+}

src/ros/catkin/src/costmap_generator/CHANGELOG.rst

@@ -0,0 +1,106 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package costmap_generator
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* [Feature] costmap generator (`#1774 <https://github.com/CPFL/Autoware/issues/1774>`_)
+  * * Initial commit for visualization package
+  * Removal of all visualization messages from perception nodes
+  * Visualization dependency removal
+  * Launch file modification
+  * * Fixes to visualization
+  * Error on Clustering CPU
+  * Reduce verbosity on markers
+  * Publish acceleration and velocity from ukf tracker
+  * Remove hardcoded path
+  * Updated README
+  * updated prototype
+  * Prototype update for header and usage
+  * Removed unknown label from being reported
+  * Updated publishing orientation to match develop
+  * * Published all the trackers
+  * Added valid field for visualization and future compatibility with ADAS ROI filtering
+  * Add simple functions
+  * * Reversed back UKF node to develop
+  * Formatted speed
+  * Refactor codes
+  * Make tracking visualization work
+  * Relay class info in tracker node
+  * Remove dependency to jskbbox and rosmarker in ukf tracker
+  * apply rosclang to ukf tracker
+  * Refactor codes
+  * Revert "apply rosclang to ukf tracker"
+  * Revert "Remove dependency to jskbbox and rosmarker in ukf tracker"
+  * Revert "Relay class info in tracker node"
+  * delete dependency to jsk and remove pointcloud_frame
+  * get direction nis
+  * set velocity_reliable true in tracker node
+  * Add divided function
+  * add function
+  * Sanity checks
+  * Relay all the data from input DetectedObject
+  * Divided function work both for immukf and sukf
+  * Add comment
+  * Refactor codes
+  * Pass immukf test
+  * make direction assisted tracking work
+  * Visualization fixes
+  * Refactor codes
+  * Tracker Merging step added
+  * Added launch file support for merging phase
+  * lane assisted with sukf
+  * * change only static objects
+  * keep label of the oldest tracker
+  * Static Object discrimination
+  * Non rotating bouding box
+  * no disappear if detector works
+  * Modify removeRedundant a bit
+  * initial commit for costmap generator
+  * add vague stucture
+  * add brief structure fot points2costmap
+  * refactor codes
+  * add sensor points costmap
+  * add waypoint costmap
+  * bug fix for wayarea2costmap
+  * add simple structure for objects2costmap
+  * add vague structure for waypoints2costmap
+  * Replacement of JSK visualization for RViz Native Markers
+  * add occupancy grid visualization
+  * add objects costmap
+  * add minimum height threshold for pointcloud
+  * debug computing.yaml from objects_topic to objects_input
+  * Add blurred costmap
+  * Add comment on computing.yml
+  * visualizing bug fix
+  * Make sure object's cost is 100 and blurred outside of objects
+  * add costmap_generator package
+  * add unit tests
+  * delete test launch file
+  * Apply autoware ros clang
+  * Add README
+  * sync develop's readme
+  * sync develop's code
+  * add convex hull costmap
+  * Relay ros header appropriately
+  * change interaface for generating costmap from points
+  * add test for points2costmap
+  * Modify for how to pick up convex-hull points
+  * add test
+  * add test for objects2costmap
+  * Added missing include
+  * Added missing grid_map_ros dependency
+  * Updated include
+  * Re-strutured include folders
+  * Generic folder name
+  * Fix/costmap generator (`#2077 <https://github.com/CPFL/Autoware/issues/2077>`_)
+  * segmentation fault in  CheckAssignPoints2GridCell
+  * Remove redundant codes in test
+  * Add some variables in SetUp
+  * rename class
+  * rename files
+  * modify tests
+  * Add scription in SetUp
+  * Remove unnecesary in_object
+  * Refactor test codes
+* Contributors: Kosuke Murakami

src/ros/catkin/src/costmap_generator/CMakeLists.txt

@@ -0,0 +1,102 @@
+cmake_minimum_required(VERSION 2.8.12)
+
+project(costmap_generator)
+
+find_package(autoware_build_flags REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+  autoware_msgs
+  grid_map_ros
+  object_map
+  pcl_ros
+  roscpp
+  tf
+  vector_map
+  lanelet2_extension
+)
+
+catkin_package()
+
+set(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+### costmap_generator ###
+add_library(
+  costmap_generator_lib
+  nodes/costmap_generator/costmap_generator_node.cpp
+  nodes/costmap_generator/costmap_generator.cpp
+  nodes/costmap_generator/points_to_costmap.cpp
+  nodes/costmap_generator/objects_to_costmap.cpp
+)
+
+target_link_libraries(
+  costmap_generator_lib
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(
+  costmap_generator_lib
+  ${catkin_EXPORTED_TARGETS}
+)
+
+add_executable(
+  costmap_generator
+  nodes/costmap_generator/costmap_generator_node.cpp
+  nodes/costmap_generator/costmap_generator.cpp
+  nodes/costmap_generator/points_to_costmap.cpp
+  nodes/costmap_generator/objects_to_costmap.cpp
+)
+
+target_link_libraries(
+  costmap_generator
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(
+  costmap_generator
+  ${catkin_EXPORTED_TARGETS}
+)
+
+add_executable(
+  costmap_generator_lanelet2
+  nodes/costmap_generator/costmap_generator_lanelet2_node.cpp
+  nodes/costmap_generator/costmap_generator_lanelet2.cpp
+  nodes/costmap_generator/points_to_costmap.cpp
+  nodes/costmap_generator/objects_to_costmap.cpp
+)
+
+target_link_libraries(
+  costmap_generator_lanelet2
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(
+  costmap_generator_lanelet2
+  ${catkin_EXPORTED_TARGETS}
+)
+
+install(
+  TARGETS 
+    costmap_generator
+    costmap_generator_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
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  catkin_add_gtest(test-costmap_generator test/src/test_costmap_generator.cpp)
+  target_link_libraries(test-costmap_generator ${catkin_LIBRARIES} costmap_generator_lib)
+endif()

src/ros/catkin/src/costmap_generator/README.md

@@ -0,0 +1,71 @@
+# The `costmap_generator` Package
+## costmap_generator
+
+This node reads `PointCloud` and/or `DetectedObjectArray` and creates an `OccupancyGrid` and `GridMap`. `VectorMap` is optional.
+
+**You need to subscribe at least one of `PointCloud` or `DetectedObjectArray` to generate costmap.**
+
+#### Input topics
+`/points_no_ground` (sensor_msgs::PointCloud2) : from ray_ground_filter or compare map filter. It contains filtered points with the ground removed.
+
+`/prediction/moving_predictor/objects` (autoware_msgs::DetectedObjectArray): predicted objects from naive_motion_predict.
+
+`/vector_map`: from the VectorMap publisher. `/tf` to obtain the transform between the vector map(map_frame) and the sensor(sensor_frame) .
+
+
+#### Output topics
+`/semantics/costmap` (grid_map::GridMap) is the output costmap, with values ranging from 0.0-1.0.
+
+`/semantics/costmap_generator/occupancy_grid` (nav_msgs::OccupancyGrid) is the output OccupancyGrid, with values ranging from 0-100.
+
+##### How to launch
+It can be launched as follows:
+ 1. Using the Runtime Manager by clicking the `costmap_generator` checkbox under the *Semantics* section in the Computing tab.
+ 2. From a sourced terminal executing: `roslaunch costmap_generator costmap_generator.launch`.
+
+##### Parameters available in roslaunch and rosrun
+* `use_objects` Whether if using `DetectedObjectArray` or not (default value: true).
+* `use_points` Whether if using `PointCloud` or not (default value: true).
+* `use_wayarea` Whether if using `Wayarea` from `VectorMap` or not (default value: true).
+* `objects_input` Input topic for `autoware_msgs::DetectedObjectArray` (default value: /prediction/moving_predictor/objects).
+* `points_input` Input topic for sensor_msgs::PointCloud2 (default value: points_no_ground).
+* `lidar_frame` Lidar sensor coordinate. Cost is calculated based on this coordinate (default value: velodyne).
+* `map_frame` VectorMap's coordinate. (default value: map).
+* `grid_min_value` Minimum cost for gridmap (default value: 0.0).
+* `grid_max_value` Maximum cost for gridmap (default value: 1.0).
+* `grid_resolution` Resolution for gridmap (default value: 0.2).
+* `grid_length_x` Size of gridmap for x direction (default value: 50).
+* `grid_length_y` Size of gridmap for y direction (default value: 30).
+* `grid_position_x` Offset from coordinate in x direction (default value: 20).
+* `grid_position_y` Offset from coordinate in y direction (default value:  0).
+* `maximum_lidar_height_thres` Maximum height threshold for pointcloud data (default value:  0.3).
+* `minimum_lidar_height_thres` Minimum height threshold for pointcloud data (default value:  -2.2).
+* `expand_rectangle_size` Expand object's rectangle with this value (default value: 1).
+* `size_of_expansion_kernel` Kernel size for blurring effect on object's costmap (default value: 9).
+
+---
+
+## Instruction Videos
+
+[![](https://img.youtube.com/vi/f7kSVJ23Mtw/0.jpg)](https://www.youtube.com/watch?v=f7kSVJ23Mtw)
+
+---
+
+## costmap_generator_lanelet2
+This node behave exactly like `costmap_generator` node, but uses different map format for wayarea.
+
+#### Input topics
+`/points_no_ground` (sensor_msgs::PointCloud2) : from ray_ground_filter or compare map filter. It contains filtered points with the ground removed.
+
+`/prediction/moving_predictor/objects` (autoware_msgs::DetectedObjectArray): predicted objects from naive_motion_predict.
+
+`/lanelet_map_bin`: from the `lanelet2_map_loader`. `/tf` to obtain the transform between the vector map(map_frame) and the sensor(sensor_frame) .
+
+#### Output topics
+same as `costmap_generator`
+
+#### How to launch
+run:
+```
+roslaunch costmap_generator costmap_generator_lanelet2.launch
+```

src/ros/catkin/src/costmap_generator/include/costmap_generator/costmap_generator.h

@@ -0,0 +1,144 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  * Neither the name of Autoware nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ ********************/
+
+#ifndef COSTMAP_GENERATOR_H
+#define COSTMAP_GENERATOR_H
+
+// headers in ROS
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <grid_map_ros/grid_map_ros.hpp>
+#include <grid_map_ros/GridMapRosConverter.hpp>
+#include <grid_map_msgs/GridMap.h>
+
+// headers in local directory
+#include "vector_map/vector_map.h"
+#include "autoware_msgs/DetectedObjectArray.h"
+#include "points_to_costmap.h"
+#include "objects_to_costmap.h"
+
+// headers in STL
+#include <memory>
+
+class CostmapGenerator
+{
+public:
+  CostmapGenerator();
+  ~CostmapGenerator();
+
+  void init();
+  void run();
+
+private:
+  friend class TestClass;
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+  bool use_objects_box_;
+  bool use_objects_convex_hull_;
+  bool use_points_;
+  bool use_wayarea_;
+
+  bool has_subscribed_wayarea_;
+
+  std::string lidar_frame_;
+  std::string map_frame_;
+  double grid_min_value_;
+  double grid_max_value_;
+  double grid_resolution_;
+  double grid_length_x_;
+  double grid_length_y_;
+  double grid_position_x_;
+  double grid_position_y_;
+
+  double maximum_lidar_height_thres_;
+  double minimum_lidar_height_thres_;
+
+  double expand_polygon_size_;
+  int size_of_expansion_kernel_;
+
+  vector_map::VectorMap vmap_;
+
+  grid_map::GridMap costmap_;
+
+  ros::Publisher pub_costmap_;
+  ros::Publisher pub_occupancy_grid_;
+  ros::Subscriber sub_waypoint_;
+  ros::Subscriber sub_points_;
+  ros::Subscriber sub_objects_;
+
+  tf::TransformListener tf_listener_;
+
+  std::vector<std::vector<geometry_msgs::Point>> area_points_;
+
+  PointsToCostmap points2costmap_;
+  ObjectsToCostmap objects2costmap_;
+
+  const std::string OBJECTS_BOX_COSTMAP_LAYER_;
+  const std::string OBJECTS_CONVEX_HULL_COSTMAP_LAYER_;
+  const std::string SENSOR_POINTS_COSTMAP_LAYER_;
+  const std::string VECTORMAP_COSTMAP_LAYER_;
+  const std::string COMBINED_COSTMAP_LAYER_;
+
+  /// \brief callback for DetectedObjectArray
+  /// \param[in] in_objects input DetectedObjectArray usually from prediction or perception component
+  void objectsCallback(const autoware_msgs::DetectedObjectArray::ConstPtr& in_ojects);
+
+  /// \brief callback for sensor_msgs::PointCloud2
+  /// \param[in] in_sensor_points input sensot_msgs::PointCloud2. Assuming groud-fitered pointcloud by default
+  void sensorPointsCallback(const sensor_msgs::PointCloud2::ConstPtr& in_sensor_points);
+
+  /// \brief initialize gridmap parameters based on rosparam
+  void initGridmap();
+
+  /// \brief publish ros msg: /semantics/costmap (grid_map::GridMap),
+  ///                         /semantics/costmap_generator/occupancy_grid(nav_msgs::OccupancyGrid)
+  /// \param[in] gridmap with calculated cost
+  /// \param[in] input ros header
+  void publishRosMsg(const grid_map::GridMap& gridmap, const std_msgs::Header& in_header);
+
+  /// \brief calculate cost from pointcloud data
+  /// \param[in] in_sensor_points: subscribed pointcloud data
+  grid_map::Matrix generateSensorPointsCostmap(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_sensor_points);
+
+  /// \brief calculate cost from DetectedObjectArray
+  /// \param[in] in_objects: subscribed DetectedObjectArray
+  grid_map::Matrix generateObjectsCostmap(const autoware_msgs::DetectedObjectArray::ConstPtr& in_objects,
+                                          const bool use_objects_convex_hull);
+
+  /// \brief calculate cost from vectormap
+  grid_map::Matrix generateVectormapCostmap();
+
+  /// \brief calculate cost for final output
+  grid_map::Matrix generateCombinedCostmap();
+};
+
+#endif  // COSTMAP_GENERATOR_H

src/ros/catkin/src/costmap_generator/include/costmap_generator/costmap_generator_lanelet2.h

@@ -0,0 +1,162 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  * Neither the name of Autoware nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ ********************/
+
+#ifndef COSTMAP_GENERATOR_LANELET2_H
+#define COSTMAP_GENERATOR_LANELET2_H
+
+// headers in ROS
+#include <ros/ros.h>
+
+#include <grid_map_msgs/GridMap.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <tf/transform_listener.h>
+#include <grid_map_ros/GridMapRosConverter.hpp>
+#include <grid_map_ros/grid_map_ros.hpp>
+
+// headers in Autoware
+#include <autoware_lanelet2_msgs/MapBin.h>
+#include <autoware_msgs/DetectedObjectArray.h>
+#include <costmap_generator/objects_to_costmap.h>
+#include <costmap_generator/points_to_costmap.h>
+#include <lanelet2_extension/utility/message_conversion.h>
+
+// headers in STL
+#include <memory>
+
+class CostmapGeneratorLanelet2
+{
+public:
+  CostmapGeneratorLanelet2();
+
+  void init();
+  void run();
+
+private:
+  friend class TestClass;
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+  bool use_objects_box_;
+  bool use_objects_convex_hull_;
+  bool use_points_;
+  bool use_wayarea_;
+
+  bool has_subscribed_wayarea_;
+
+  bool loaded_lanelet_map_ = false;
+  lanelet::LaneletMapPtr lanelet_map_;
+  bool use_all_road_lanelets_ = true;
+
+  std::string lidar_frame_;
+  std::string map_frame_;
+  double grid_min_value_;
+  double grid_max_value_;
+  double grid_resolution_;
+  double grid_length_x_;
+  double grid_length_y_;
+  double grid_position_x_;
+  double grid_position_y_;
+
+  double maximum_lidar_height_thres_;
+  double minimum_lidar_height_thres_;
+
+  double expand_polygon_size_;
+  int size_of_expansion_kernel_;
+
+  grid_map::GridMap costmap_;
+
+  ros::Subscriber sub_lanelet_bin_map_;
+
+  ros::Publisher pub_costmap_;
+  ros::Publisher pub_occupancy_grid_;
+  ros::Subscriber sub_waypoint_;
+  ros::Subscriber sub_points_;
+  ros::Subscriber sub_objects_;
+
+  tf::TransformListener tf_listener_;
+
+  std::vector<std::vector<geometry_msgs::Point>> area_points_;
+
+  PointsToCostmap points2costmap_;
+  ObjectsToCostmap objects2costmap_;
+
+  const std::string OBJECTS_BOX_COSTMAP_LAYER_;
+  const std::string OBJECTS_CONVEX_HULL_COSTMAP_LAYER_;
+  const std::string SENSOR_POINTS_COSTMAP_LAYER_;
+  const std::string VECTORMAP_COSTMAP_LAYER_;
+  const std::string LANELET2_COSTMAP_LAYER_;
+  const std::string COMBINED_COSTMAP_LAYER_;
+
+  /// \brief callback for loading landlet2 map
+  void laneletBinMapCallback(const autoware_lanelet2_msgs::MapBin& msg);
+
+  /// \brief wait for lanelet2 map to load and build routing graph
+  void initLaneletMap();
+
+  /// \brief callback for DetectedObjectArray
+  /// \param[in] in_objects input DetectedObjectArray usually from prediction or perception component
+  void objectsCallback(const autoware_msgs::DetectedObjectArray::ConstPtr& in_ojects);
+
+  /// \brief callback for sensor_msgs::PointCloud2
+  /// \param[in] in_sensor_points input sensot_msgs::PointCloud2. Assuming groud-fitered pointcloud by default
+  void sensorPointsCallback(const sensor_msgs::PointCloud2::ConstPtr& in_sensor_points);
+
+  /// \brief initialize gridmap parameters based on rosparam
+  void initGridmap();
+
+  /// \brief publish ros msg: /semantics/costmap (grid_map::GridMap),
+  ///                         /semantics/costmap_generator/occupancy_grid(nav_msgs::OccupancyGrid)
+  /// \param[in] gridmap with calculated cost
+  /// \param[in] input ros header
+  void publishRosMsg(const grid_map::GridMap& gridmap, const std_msgs::Header& in_header);
+
+  /// \brief set area_points from lanelet polygons
+  /// \param [in] input lanelet_map
+  /// \param [out] calculated area_points of lanelet polygons
+  void loadRoadAreasFromLaneletMap(const lanelet::LaneletMapPtr lanelet_map,
+                                   std::vector<std::vector<geometry_msgs::Point>>* area_points);
+
+  /// \brief calculate cost from pointcloud data
+  /// \param[in] in_sensor_points: subscribed pointcloud data
+  grid_map::Matrix generateSensorPointsCostmap(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_sensor_points);
+
+  /// \brief calculate cost from DetectedObjectArray
+  /// \param[in] in_objects: subscribed DetectedObjectArray
+  grid_map::Matrix generateObjectsCostmap(const autoware_msgs::DetectedObjectArray::ConstPtr& in_objects,
+                                          const bool use_objects_convex_hull);
+
+  /// \brief calculate cost from lanelet2 map
+  grid_map::Matrix generateLanelet2Costmap();
+
+  /// \brief calculate cost for final output
+  grid_map::Matrix generateCombinedCostmap();
+};
+
+#endif  // COSTMAP_GENERATOR_H

src/ros/catkin/src/costmap_generator/include/costmap_generator/objects_to_costmap.h

@@ -0,0 +1,105 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  * Neither the name of Autoware nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ ********************/
+
+#ifndef OBJECTS_TO_COSTMAP_H
+#define OBJECTS_TO_COSTMAP_H
+
+// headers in ROS
+#include <ros/ros.h>
+#include <grid_map_ros/grid_map_ros.hpp>
+
+// headers in local directory
+#include "autoware_msgs/DetectedObjectArray.h"
+
+class ObjectsToCostmap
+{
+public:
+  ObjectsToCostmap();
+  ~ObjectsToCostmap();
+
+  /// \brief calculate cost from DetectedObjectArray
+  /// \param[in] costmap: initialized gridmap
+  /// \param[in] expand_polygon_size: expand object's costmap polygon
+  /// \param[in] size_of_expansion_kernel: kernel size for blurring cost
+  /// \param[in] in_objects: subscribed DetectedObjectArray
+  /// \param[out] calculated cost in grid_map::Matrix format
+  grid_map::Matrix makeCostmapFromObjects(const grid_map::GridMap& costmap,
+                                          const double expand_polygon_size, const double size_of_expansion_kernel,
+                                          const autoware_msgs::DetectedObjectArray::ConstPtr& in_objects,
+                                          const bool use_objects_convex_hull);
+
+private:
+  friend class TestClass;
+
+  const int NUMBER_OF_POINTS;
+  const int NUMBER_OF_DIMENSIONS;
+  const std::string OBJECTS_COSTMAP_LAYER_;
+  const std::string BLURRED_OBJECTS_COSTMAP_LAYER_;
+
+  /// \brief make 4 rectangle points from centroid position and orientation
+  /// \param[in] in_object: subscribed one of DetectedObjectArray
+  /// \param[in] expand_rectangle_size: expanding 4 points
+  /// \param[out] 4 rectangle points
+  Eigen::MatrixXd makeRectanglePoints(const autoware_msgs::DetectedObject& in_object,
+                                      const double expand_rectangle_size);
+
+  /// \brief make polygon(grid_map::Polygon) from 4 rectangle's points
+  /// \param[in] in_object: subscribed one of DetectedObjectArray
+  /// \param[in] expand_rectangle_size: expanding 4 points
+  /// \param[out] polygon with 4 rectangle points
+  grid_map::Polygon makePolygonFromObjectBox(const autoware_msgs::DetectedObject& in_object,
+                                          const double expand_rectangle_size);
+
+  /// \brief make expanded point from convex hull's point
+  /// \param[in] in_centroid: object's centroid
+  /// \param[in] in_corner_point one of convex hull points
+  /// \param[in] expand_polygon_size  the param for expanding convex_hull points
+  /// \param[out] expanded point
+  geometry_msgs::Point makeExpandedPoint(const geometry_msgs::Point& in_centroid,
+                                         const geometry_msgs::Point32& in_corner_point,
+                                         const double expand_polygon_size);
+
+  /// \brief make polygon(grid_map::Polygon) from convex hull points
+  /// \param[in] in_centroid: object's centroid
+  /// \param[in] expand_polygon_size: expanding convex_hull points
+  /// \param[out] polygon object with convex hull points
+  grid_map::Polygon makePolygonFromObjectConvexHull(const autoware_msgs::DetectedObject& in_object,
+                                                    const double expand_polygon_size);
+
+  /// \brief set cost in polygon by using DetectedObject's score
+  /// \param[in] polygon: 4 rectangle points in polygon format
+  /// \param[in] gridmap_layer_name: target gridmap layer name for calculated cost
+  /// \param[in] score: set score as a cost for costmap
+  /// \param[in] objects_costmap: update cost in this objects_costmap[gridmap_layer_name]
+  void setCostInPolygon(const grid_map::Polygon& polygon, const std::string& gridmap_layer_name, const float score,
+                        grid_map::GridMap& objects_costmap);
+};
+
+#endif  // OBJECTS_TO_COSTMAP_H

src/ros/catkin/src/costmap_generator/include/costmap_generator/points_to_costmap.h

@@ -0,0 +1,109 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  * Neither the name of Autoware nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ ********************/
+
+#ifndef POINTS_TO_COSTMAP_H
+#define POINTS_TO_COSTMAP_H
+
+// headers in ROS
+#include <ros/ros.h>
+#include <grid_map_ros/grid_map_ros.hpp>
+
+// headers in PCL
+#include <pcl_ros/point_cloud.h>
+
+class PointsToCostmap
+{
+public:
+  PointsToCostmap();
+  ~PointsToCostmap();
+
+  /// \brief calculate cost from sensor points
+  /// \param[in] maximum_height_thres: Maximum height threshold for pointcloud data
+  /// \param[in] minimum_height_thres: Minimum height threshold for pointcloud data
+  /// \param[in] grid_min_value: Minimum cost for costmap
+  /// \param[in] grid_max_value: Maximum cost fot costmap
+  /// \param[in] gridmap: costmap based on gridmap
+  /// \param[in] gridmap_layer_name: gridmap layer name for gridmap
+  /// \param[in] in_sensor_points: subscribed pointcloud
+  /// \param[out] calculated cost in grid_map::Matrix format
+  grid_map::Matrix makeCostmapFromSensorPoints(const double maximum_height_thres, const double minimum_height_thres,
+                                               const double grid_min_value, const double grid_max_value,
+                                               const grid_map::GridMap& gridmap, const std::string& gridmap_layer_name,
+                                               const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_sensor_points);
+
+private:
+  friend class TestClass;
+
+  double grid_length_x_;
+  double grid_length_y_;
+  double grid_resolution_;
+  double grid_position_x_;
+  double grid_position_y_;
+  double y_cell_size_;
+  double x_cell_size_;
+
+  /// \brief initialize gridmap parameters
+  /// \param[in] gridmap: gridmap object to be initialized
+  void initGridmapParam(const grid_map::GridMap& gridmap);
+
+  /// \brief check if index is valid in the gridmap
+  /// \param[in] grid_ind: grid index corresponding with one of pointcloud
+  /// \param[out] bool: true if index is valid
+  bool isValidInd(const grid_map::Index& grid_ind);
+
+  /// \brief Get index from one of pointcloud
+  /// \param[in] point: one of subscribed pointcloud
+  /// \param[out] index in gridmap
+  grid_map::Index fetchGridIndexFromPoint(const pcl::PointXYZ& point);
+
+  /// \brief Assign pointcloud to appropriate cell in gridmap
+  /// \param[in] gridmap: costmap based on gridmap
+  /// \param[in] in_sensor_points: subscribed pointcloud
+  /// \param[out] grid-x-length x grid-y-length size grid stuffed with point's height in corresponding grid cell
+  std::vector<std::vector<std::vector<double>>>
+  assignPoints2GridCell(const grid_map::GridMap& gridmap, const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_sensor_points);
+
+  /// \brief calculate costmap from subscribed pointcloud
+  /// \param[in] maximum_height_thres: Maximum height threshold for pointcloud data
+  /// \param[in] minimum_height_thres: Minimum height threshold for pointcloud data
+  /// \param[in] grid_min_value: Minimum cost for costmap
+  /// \param[in] grid_max_value: Maximum cost fot costmap
+  /// \param[in] gridmap: costmap based on gridmap
+  /// \param[in] gridmap_layer_name: gridmap layer name for gridmap
+  /// \param[in] grid_vec: grid-x-length x grid-y-length size grid stuffed with point's height in corresponding grid
+  /// cell
+  /// \param[out] caculated costmap in grid_map::Matrix format
+  grid_map::Matrix calculateCostmap(const double maximum_height_thres, const double minimum_lidar_height_thres,
+                                    const double grid_min_value, const double grid_max_value,
+                                    const grid_map::GridMap& gridmap, const std::string& gridmap_layer_name,
+                                    const std::vector<std::vector<std::vector<double>>> grid_vec);
+};
+
+#endif  // POINTS_TO_COSTMAP_H

src/ros/catkin/src/costmap_generator/interface.yml

@@ -0,0 +1,3 @@
+- name: /costmap_generator
+  publish: [/semantics/costmap]
+  subscribe: [/points_no_ground, /vector_map, /prediction/moving_predictor/objects]

src/ros/catkin/src/costmap_generator/launch/costmap_generator.launch

@@ -0,0 +1,47 @@
+<launch>
+  <!-- node parameters -->
+  <arg name="lidar_frame" default="velodyne" />
+  <arg name="map_frame" default="map" />
+  <arg name="grid_min_value" default="0.0" />
+  <arg name="grid_max_value" default="1.0" />
+  <arg name="grid_resolution" default="0.2" />
+  <arg name="grid_length_x" default="50" />
+  <arg name="grid_length_y" default="30" />
+  <arg name="grid_position_x" default="20" />
+  <arg name="grid_position_y" default="0" />
+  <arg name="maximum_lidar_height_thres" default="0.3" />
+  <arg name="minimum_lidar_height_thres" default="-2.2" />
+  <arg name="expand_polygon_size" default="1.0" />
+  <arg name="size_of_expansion_kernel" default="9" />
+  <arg name="use_objects_box" default="false" />
+  <arg name="use_objects_convex_hull" default="true" />
+  <arg name="use_points" default="true" />
+  <arg name="use_wayarea" default="true" />
+
+  <arg name="objects_input" default="/prediction/motion_predictor/objects" />
+  <arg name="points_input" default="/points_no_ground" />
+
+  <!-- Launch node -->
+  <node pkg="costmap_generator" type="costmap_generator" name="costmap_generator" output="screen">
+    <param name="lidar_frame" value="$(arg lidar_frame)" />
+    <param name="map_frame" value="$(arg map_frame)" />
+    <param name="grid_min_value" value="$(arg grid_min_value)" />
+    <param name="grid_max_value" value="$(arg grid_max_value)" />
+    <param name="grid_resolution" value="$(arg grid_resolution)" />
+    <param name="grid_length_x" value="$(arg grid_length_x)" />
+    <param name="grid_length_y" value="$(arg grid_length_y)" />
+    <param name="grid_position_x" value="$(arg grid_position_x)" />
+    <param name="grid_position_y" value="$(arg grid_position_y)" />
+    <param name="maximum_lidar_height_thres" value="$(arg maximum_lidar_height_thres)" />
+    <param name="minimum_lidar_height_thres" value="$(arg minimum_lidar_height_thres)" />
+    <param name="expand_polygon_size" value="$(arg expand_polygon_size)" />
+    <param name="size_of_expansion_kernel" value="$(arg size_of_expansion_kernel)" />
+    <param name="use_objects_box" value="$(arg use_objects_box)" />
+    <param name="use_objects_convex_hull" value="$(arg use_objects_convex_hull)" />
+    <param name="use_points" value="$(arg use_points)" />
+    <param name="use_wayarea" value="$(arg use_wayarea)" />
+
+    <remap from="/prediction/motion_predictor/objects"  to="$(arg objects_input)" />
+    <remap from="/points_no_ground"                     to="$(arg points_input)" />
+  </node>
+</launch>

src/ros/catkin/src/costmap_generator/launch/costmap_generator_lanelet2.launch

@@ -0,0 +1,47 @@
+<launch>
+  <!-- node parameters -->
+  <arg name="lidar_frame" default="velodyne" />
+  <arg name="map_frame" default="map" />
+  <arg name="grid_min_value" default="0.0" />
+  <arg name="grid_max_value" default="1.0" />
+  <arg name="grid_resolution" default="0.2" />
+  <arg name="grid_length_x" default="50" />
+  <arg name="grid_length_y" default="30" />
+  <arg name="grid_position_x" default="20" />
+  <arg name="grid_position_y" default="0" />
+  <arg name="maximum_lidar_height_thres" default="0.3" />
+  <arg name="minimum_lidar_height_thres" default="-2.2" />
+  <arg name="expand_polygon_size" default="1.0" />
+  <arg name="size_of_expansion_kernel" default="9" />
+  <arg name="use_objects_box" default="false" />
+  <arg name="use_objects_convex_hull" default="true" />
+  <arg name="use_points" default="true" />
+  <arg name="use_wayarea" default="true" />
+
+  <arg name="objects_input" default="/prediction/motion_predictor/objects" />
+  <arg name="points_input" default="/points_no_ground" />
+
+  <!-- Launch node -->
+  <node pkg="costmap_generator" type="costmap_generator_lanelet2" name="costmap_generator_lanelet2" output="screen">
+    <param name="lidar_frame" value="$(arg lidar_frame)" />
+    <param name="map_frame" value="$(arg map_frame)" />
+    <param name="grid_min_value" value="$(arg grid_min_value)" />
+    <param name="grid_max_value" value="$(arg grid_max_value)" />
+    <param name="grid_resolution" value="$(arg grid_resolution)" />
+    <param name="grid_length_x" value="$(arg grid_length_x)" />
+    <param name="grid_length_y" value="$(arg grid_length_y)" />
+    <param name="grid_position_x" value="$(arg grid_position_x)" />
+    <param name="grid_position_y" value="$(arg grid_position_y)" />
+    <param name="maximum_lidar_height_thres" value="$(arg maximum_lidar_height_thres)" />
+    <param name="minimum_lidar_height_thres" value="$(arg minimum_lidar_height_thres)" />
+    <param name="expand_polygon_size" value="$(arg expand_polygon_size)" />
+    <param name="size_of_expansion_kernel" value="$(arg size_of_expansion_kernel)" />
+    <param name="use_objects_box" value="$(arg use_objects_box)" />
+    <param name="use_objects_convex_hull" value="$(arg use_objects_convex_hull)" />
+    <param name="use_points" value="$(arg use_points)" />
+    <param name="use_wayarea" value="$(arg use_wayarea)" />
+
+    <remap from="/prediction/motion_predictor/objects"  to="$(arg objects_input)" />
+    <remap from="/points_no_ground"                     to="$(arg points_input)" />
+  </node>
+</launch>

src/ros/catkin/src/costmap_generator/launch/costmap_generator_option.launch

@@ -0,0 +1,83 @@
+<launch>
+  <!-- node parameters -->
+  <arg name="lidar_frame" default="velodyne" />
+  <arg name="map_frame" default="map" />
+  <arg name="grid_min_value" default="0.0" />
+  <arg name="grid_max_value" default="1.0" />
+  <arg name="grid_resolution" default="0.2" />
+  <arg name="grid_length_x" default="50" />
+  <arg name="grid_length_y" default="30" />
+  <arg name="grid_position_x" default="20" />
+  <arg name="grid_position_y" default="0" />
+  <arg name="maximum_lidar_height_thres" default="0.3" />
+  <arg name="minimum_lidar_height_thres" default="-2.2" />
+  <arg name="expand_polygon_size" default="1.0" />
+  <arg name="size_of_expansion_kernel" default="9" />
+  <arg name="use_objects_box" default="false" />
+  <arg name="use_objects_convex_hull" default="true" />
+  <arg name="use_points" default="true" />
+  <arg name="use_wayarea" default="true" />
+
+  <arg name="objects_input" default="/prediction/motion_predictor/objects" />
+  <arg name="points_input" default="/points_no_ground" />
+
+
+  <arg name="use_ll2" default="false"/>
+
+  <!-- Launch node depending on use_ll2 arg-->
+
+<group  if="$(arg use_ll2)">
+
+  <node pkg="costmap_generator" type="costmap_generator_lanelet2" name="costmap_generator_lanelet2" output="screen">
+    <param name="lidar_frame" value="$(arg lidar_frame)" />
+    <param name="map_frame" value="$(arg map_frame)" />
+    <param name="grid_min_value" value="$(arg grid_min_value)" />
+    <param name="grid_max_value" value="$(arg grid_max_value)" />
+    <param name="grid_resolution" value="$(arg grid_resolution)" />
+    <param name="grid_length_x" value="$(arg grid_length_x)" />
+    <param name="grid_length_y" value="$(arg grid_length_y)" />
+    <param name="grid_position_x" value="$(arg grid_position_x)" />
+    <param name="grid_position_y" value="$(arg grid_position_y)" />
+    <param name="maximum_lidar_height_thres" value="$(arg maximum_lidar_height_thres)" />
+    <param name="minimum_lidar_height_thres" value="$(arg minimum_lidar_height_thres)" />
+    <param name="expand_polygon_size" value="$(arg expand_polygon_size)" />
+    <param name="size_of_expansion_kernel" value="$(arg size_of_expansion_kernel)" />
+    <param name="use_objects_box" value="$(arg use_objects_box)" />
+    <param name="use_objects_convex_hull" value="$(arg use_objects_convex_hull)" />
+    <param name="use_points" value="$(arg use_points)" />
+    <param name="use_wayarea" value="$(arg use_wayarea)" />
+
+    <remap from="/prediction/motion_predictor/objects"  to="$(arg objects_input)" />
+    <remap from="/points_no_ground"                     to="$(arg points_input)" />
+  </node>
+  
+</group>
+
+<group  unless="$(arg use_ll2)">
+
+  <node pkg="costmap_generator" type="costmap_generator" name="costmap_generator" output="screen">
+    <param name="lidar_frame" value="$(arg lidar_frame)" />
+    <param name="map_frame" value="$(arg map_frame)" />
+    <param name="grid_min_value" value="$(arg grid_min_value)" />
+    <param name="grid_max_value" value="$(arg grid_max_value)" />
+    <param name="grid_resolution" value="$(arg grid_resolution)" />
+    <param name="grid_length_x" value="$(arg grid_length_x)" />
+    <param name="grid_length_y" value="$(arg grid_length_y)" />
+    <param name="grid_position_x" value="$(arg grid_position_x)" />
+    <param name="grid_position_y" value="$(arg grid_position_y)" />
+    <param name="maximum_lidar_height_thres" value="$(arg maximum_lidar_height_thres)" />
+    <param name="minimum_lidar_height_thres" value="$(arg minimum_lidar_height_thres)" />
+    <param name="expand_polygon_size" value="$(arg expand_polygon_size)" />
+    <param name="size_of_expansion_kernel" value="$(arg size_of_expansion_kernel)" />
+    <param name="use_objects_box" value="$(arg use_objects_box)" />
+    <param name="use_objects_convex_hull" value="$(arg use_objects_convex_hull)" />
+    <param name="use_points" value="$(arg use_points)" />
+    <param name="use_wayarea" value="$(arg use_wayarea)" />
+
+    <remap from="/prediction/motion_predictor/objects"  to="$(arg objects_input)" />
+    <remap from="/points_no_ground"                     to="$(arg points_input)" />
+  </node>
+  
+</group>
+
+</launch>

src/ros/catkin/src/costmap_generator/nodes/costmap_generator/costmap_generator.cpp

@@ -0,0 +1,207 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  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.
+ ********************/
+
+// headers in local directory
+#include "object_map/object_map_utils.hpp"
+#include "costmap_generator/costmap_generator.h"
+
+// Constructor
+CostmapGenerator::CostmapGenerator()
+  : private_nh_("~")
+  , has_subscribed_wayarea_(false)
+  , OBJECTS_BOX_COSTMAP_LAYER_("objects_box")
+  , OBJECTS_CONVEX_HULL_COSTMAP_LAYER_("objects_convex_hull")
+  , SENSOR_POINTS_COSTMAP_LAYER_("sensor_points")
+  , VECTORMAP_COSTMAP_LAYER_("vectormap")
+  , COMBINED_COSTMAP_LAYER_("costmap")
+{
+}
+
+CostmapGenerator::~CostmapGenerator()
+{
+}
+
+void CostmapGenerator::init()
+{
+  private_nh_.param<std::string>("lidar_frame", lidar_frame_, "velodyne");
+  private_nh_.param<std::string>("map_frame", map_frame_, "map");
+  private_nh_.param<double>("grid_min_value", grid_min_value_, 0.0);
+  private_nh_.param<double>("grid_max_value", grid_max_value_, 1.0);
+  private_nh_.param<double>("grid_resolution", grid_resolution_, 0.2);
+  private_nh_.param<double>("grid_length_x", grid_length_x_, 50);
+  private_nh_.param<double>("grid_length_y", grid_length_y_, 30);
+  private_nh_.param<double>("grid_position_x", grid_position_x_, 20);
+  private_nh_.param<double>("grid_position_y", grid_position_y_, 0);
+  private_nh_.param<double>("maximum_lidar_height_thres", maximum_lidar_height_thres_, 0.3);
+  private_nh_.param<double>("minimum_lidar_height_thres", minimum_lidar_height_thres_, -2.2);
+  private_nh_.param<bool>("use_objects_box", use_objects_box_, false);
+  private_nh_.param<bool>("use_objects_convex_hull", use_objects_convex_hull_, true);
+  private_nh_.param<bool>("use_points", use_points_, true);
+  private_nh_.param<bool>("use_wayarea", use_wayarea_, true);
+  private_nh_.param<double>("expand_polygon_size", expand_polygon_size_, 1.0);
+  private_nh_.param<int>("size_of_expansion_kernel", size_of_expansion_kernel_, 9);
+  std::cout<<" init. "<<std::endl;
+  initGridmap();
+}
+
+void CostmapGenerator::run()
+{
+  pub_costmap_ = nh_.advertise<grid_map_msgs::GridMap>("/semantics/costmap", 1);
+  pub_occupancy_grid_ = nh_.advertise<nav_msgs::OccupancyGrid>("/semantics/costmap_generator/occupancy_grid", 1);
+
+  if (use_objects_box_ || use_objects_convex_hull_)
+  {
+    sub_objects_ = nh_.subscribe("/prediction/motion_predictor/objects", 1, &CostmapGenerator::objectsCallback, this);
+  }
+
+  if (use_points_)
+  {
+    std::cout<<" use points. "<<std::endl;
+    sub_points_ = nh_.subscribe("/points_no_ground", 1, &CostmapGenerator::sensorPointsCallback, this);
+  }
+}
+
+void CostmapGenerator::objectsCallback(const autoware_msgs::DetectedObjectArray::ConstPtr& in_objects)
+{
+  if(use_objects_box_)
+  {
+    const bool use_convex_hull = !use_objects_box_;
+    costmap_[OBJECTS_BOX_COSTMAP_LAYER_] = generateObjectsCostmap(in_objects, use_convex_hull);
+  }
+
+  if(use_objects_convex_hull_)
+  {
+    costmap_[OBJECTS_CONVEX_HULL_COSTMAP_LAYER_] = generateObjectsCostmap(in_objects, use_objects_convex_hull_);
+  }
+  costmap_[VECTORMAP_COSTMAP_LAYER_] = generateVectormapCostmap();
+  costmap_[COMBINED_COSTMAP_LAYER_] = generateCombinedCostmap();
+
+  std_msgs::Header in_header = in_objects->header;
+  publishRosMsg(costmap_, in_header);
+}
+
+void CostmapGenerator::sensorPointsCallback(const sensor_msgs::PointCloud2::ConstPtr& in_sensor_points_msg)
+{
+  std::cout<<" senser points. "<<std::endl;
+  
+  pcl::PointCloud<pcl::PointXYZ>::Ptr in_sensor_points(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(*in_sensor_points_msg, *in_sensor_points);
+  costmap_[SENSOR_POINTS_COSTMAP_LAYER_] = generateSensorPointsCostmap(in_sensor_points);
+  costmap_[VECTORMAP_COSTMAP_LAYER_] = generateVectormapCostmap();
+  costmap_[COMBINED_COSTMAP_LAYER_] = generateCombinedCostmap();
+
+  std_msgs::Header in_header = in_sensor_points_msg->header;
+  publishRosMsg(costmap_, in_header);
+}
+
+void CostmapGenerator::initGridmap()
+{
+  costmap_.setFrameId(lidar_frame_);
+  costmap_.setGeometry(grid_map::Length(grid_length_x_, grid_length_y_), grid_resolution_,
+                       grid_map::Position(grid_position_x_, grid_position_y_));
+
+  costmap_.add(SENSOR_POINTS_COSTMAP_LAYER_, grid_min_value_);
+  costmap_.add(OBJECTS_BOX_COSTMAP_LAYER_, grid_min_value_);
+  costmap_.add(OBJECTS_CONVEX_HULL_COSTMAP_LAYER_, grid_min_value_);
+  costmap_.add(VECTORMAP_COSTMAP_LAYER_, grid_min_value_);
+  costmap_.add(COMBINED_COSTMAP_LAYER_, grid_min_value_);
+}
+
+grid_map::Matrix
+CostmapGenerator::generateSensorPointsCostmap(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_sensor_points)
+{
+  grid_map::Matrix sensor_points_costmap = points2costmap_.makeCostmapFromSensorPoints(
+      maximum_lidar_height_thres_, minimum_lidar_height_thres_, grid_min_value_, grid_max_value_, costmap_,
+      SENSOR_POINTS_COSTMAP_LAYER_, in_sensor_points);
+  return sensor_points_costmap;
+}
+
+grid_map::Matrix
+CostmapGenerator::generateObjectsCostmap(const autoware_msgs::DetectedObjectArray::ConstPtr& in_objects,
+                                         const bool use_convex_hull)
+{
+  grid_map::Matrix objects_costmap = objects2costmap_.makeCostmapFromObjects(
+    costmap_, expand_polygon_size_, size_of_expansion_kernel_, in_objects, use_convex_hull);
+  return objects_costmap;
+}
+
+// Only this funstion depends on object_map_utils
+grid_map::Matrix CostmapGenerator::generateVectormapCostmap()
+{
+  grid_map::GridMap vectormap_costmap = costmap_;
+  if (use_wayarea_)
+  {
+    if (!has_subscribed_wayarea_)
+    {
+      object_map::LoadRoadAreasFromVectorMap(private_nh_, area_points_);
+    }
+    if (!area_points_.empty())
+    {
+      std::cout<<" area points is not empty"<<std::endl;
+      has_subscribed_wayarea_ = true;
+      object_map::FillPolygonAreas(vectormap_costmap, area_points_, VECTORMAP_COSTMAP_LAYER_, grid_max_value_,
+                                   grid_min_value_, grid_min_value_, grid_max_value_, lidar_frame_, map_frame_,
+                                   tf_listener_);
+    }
+  }
+  return vectormap_costmap[VECTORMAP_COSTMAP_LAYER_];
+}
+
+grid_map::Matrix CostmapGenerator::generateCombinedCostmap()
+{
+  // assuming combined_costmap is calculated by element wise max operation
+  grid_map::GridMap combined_costmap = costmap_;
+  combined_costmap[COMBINED_COSTMAP_LAYER_].setConstant(grid_min_value_);
+  combined_costmap[COMBINED_COSTMAP_LAYER_] =
+      combined_costmap[COMBINED_COSTMAP_LAYER_].cwiseMax(combined_costmap[SENSOR_POINTS_COSTMAP_LAYER_]);
+  combined_costmap[COMBINED_COSTMAP_LAYER_] =
+      combined_costmap[COMBINED_COSTMAP_LAYER_].cwiseMax(combined_costmap[VECTORMAP_COSTMAP_LAYER_]);
+  combined_costmap[COMBINED_COSTMAP_LAYER_] =
+      combined_costmap[COMBINED_COSTMAP_LAYER_].cwiseMax(combined_costmap[OBJECTS_BOX_COSTMAP_LAYER_]);
+  combined_costmap[COMBINED_COSTMAP_LAYER_] =
+      combined_costmap[COMBINED_COSTMAP_LAYER_].cwiseMax(combined_costmap[OBJECTS_CONVEX_HULL_COSTMAP_LAYER_]);
+  return combined_costmap[COMBINED_COSTMAP_LAYER_];
+}
+
+void CostmapGenerator::publishRosMsg(const grid_map::GridMap& costmap, const std_msgs::Header& in_header)
+{
+  nav_msgs::OccupancyGrid out_occupancy_grid;
+  grid_map::GridMapRosConverter::toOccupancyGrid(costmap, COMBINED_COSTMAP_LAYER_, grid_min_value_, grid_max_value_,
+                                                 out_occupancy_grid);
+  out_occupancy_grid.header = in_header;
+  std::cout<<" public."<<std::endl;
+  pub_occupancy_grid_.publish(out_occupancy_grid);
+
+  grid_map_msgs::GridMap out_gridmap_msg;
+  grid_map::GridMapRosConverter::toMessage(costmap, out_gridmap_msg);
+  out_gridmap_msg.info.header = in_header;
+  std::cout<<" public 2."<<std::endl;
+  pub_costmap_.publish(out_gridmap_msg);
+}

src/ros/catkin/src/costmap_generator/nodes/costmap_generator/costmap_generator_lanelet2.cpp

@@ -0,0 +1,236 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  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 <costmap_generator/costmap_generator_lanelet2.h>
+#include <object_map/object_map_utils.hpp>
+
+#include <lanelet2_extension/utility/query.h>
+#include <lanelet2_extension/visualization/visualization.h>
+// Constructor
+CostmapGeneratorLanelet2::CostmapGeneratorLanelet2()
+  : private_nh_("~")
+  , has_subscribed_wayarea_(false)
+  , OBJECTS_BOX_COSTMAP_LAYER_("objects_box")
+  , OBJECTS_CONVEX_HULL_COSTMAP_LAYER_("objects_convex_hull")
+  , SENSOR_POINTS_COSTMAP_LAYER_("sensor_points")
+  , LANELET2_COSTMAP_LAYER_("lanelet2")
+  , COMBINED_COSTMAP_LAYER_("costmap")
+{
+}
+
+void CostmapGeneratorLanelet2::init()
+{
+  private_nh_.param<std::string>("lidar_frame", lidar_frame_, "velodyne");
+  private_nh_.param<std::string>("map_frame", map_frame_, "map");
+  private_nh_.param<double>("grid_min_value", grid_min_value_, 0.0);
+  private_nh_.param<double>("grid_max_value", grid_max_value_, 1.0);
+  private_nh_.param<double>("grid_resolution", grid_resolution_, 0.2);
+  private_nh_.param<double>("grid_length_x", grid_length_x_, 50);
+  private_nh_.param<double>("grid_length_y", grid_length_y_, 30);
+  private_nh_.param<double>("grid_position_x", grid_position_x_, 20);
+  private_nh_.param<double>("grid_position_y", grid_position_y_, 0);
+  private_nh_.param<double>("maximum_lidar_height_thres", maximum_lidar_height_thres_, 0.3);
+  private_nh_.param<double>("minimum_lidar_height_thres", minimum_lidar_height_thres_, -2.2);
+  private_nh_.param<bool>("use_objects_box", use_objects_box_, false);
+  private_nh_.param<bool>("use_objects_convex_hull", use_objects_convex_hull_, true);
+  private_nh_.param<bool>("use_points", use_points_, true);
+  private_nh_.param<bool>("use_wayarea", use_wayarea_, true);
+  private_nh_.param<double>("expand_polygon_size", expand_polygon_size_, 1.0);
+  private_nh_.param<int>("size_of_expansion_kernel", size_of_expansion_kernel_, 9);
+
+  initGridmap();
+}
+
+void CostmapGeneratorLanelet2::run()
+{
+  pub_costmap_ = nh_.advertise<grid_map_msgs::GridMap>("semantics/costmap", 1);
+  pub_occupancy_grid_ = nh_.advertise<nav_msgs::OccupancyGrid>("semantics/costmap_generator/occupancy_grid", 1);
+
+  sub_objects_ =
+      nh_.subscribe("prediction/motion_predictor/objects", 1, &CostmapGeneratorLanelet2::objectsCallback, this);
+
+  sub_points_ = nh_.subscribe("points_no_ground", 1, &CostmapGeneratorLanelet2::sensorPointsCallback, this);
+
+  sub_lanelet_bin_map_ = nh_.subscribe("lanelet_map_bin", 1, &CostmapGeneratorLanelet2::laneletBinMapCallback, this);
+}
+
+void CostmapGeneratorLanelet2::loadRoadAreasFromLaneletMap(const lanelet::LaneletMapPtr lanelet_map,
+                                                           std::vector<std::vector<geometry_msgs::Point>>* area_points)
+{
+  // use all lanelets in map of subtype road to give way area
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map);
+  lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets);
+
+  // convert lanelets to polygons and put into area_points array
+  for (const auto& ll : road_lanelets)
+  {
+    geometry_msgs::Polygon poly;
+    lanelet::visualization::lanelet2Polygon(ll, &poly);
+
+    std::vector<geometry_msgs::Point> poly_pts;
+    for (const auto& p : poly.points)
+    {
+      // convert from Point32 to Point
+      geometry_msgs::Point gp;
+      lanelet::utils::conversion::toGeomMsgPt(p, &gp);
+      poly_pts.push_back(gp);
+    }
+    area_points->push_back(poly_pts);
+  }
+
+  has_subscribed_wayarea_ = true;
+}
+
+void CostmapGeneratorLanelet2::laneletBinMapCallback(const autoware_lanelet2_msgs::MapBin& msg)
+{
+  if (!use_wayarea_)
+  {
+    return;
+  }
+
+  lanelet_map_ = std::make_shared<lanelet::LaneletMap>();
+  lanelet::utils::conversion::fromBinMsg(msg, lanelet_map_);
+  loaded_lanelet_map_ = true;
+  loadRoadAreasFromLaneletMap(lanelet_map_, &area_points_);
+}
+
+void CostmapGeneratorLanelet2::objectsCallback(const autoware_msgs::DetectedObjectArray::ConstPtr& in_objects)
+{
+  if (!use_objects_box_ && !use_objects_convex_hull_)
+  {
+    return;
+  }
+
+  if (use_objects_box_)
+  {
+    const bool use_convex_hull = false;
+    costmap_[OBJECTS_BOX_COSTMAP_LAYER_] = generateObjectsCostmap(in_objects, use_convex_hull);
+  }
+
+  if (use_objects_convex_hull_)
+  {
+    costmap_[OBJECTS_CONVEX_HULL_COSTMAP_LAYER_] = generateObjectsCostmap(in_objects, use_objects_convex_hull_);
+  }
+  costmap_[LANELET2_COSTMAP_LAYER_] = generateLanelet2Costmap();
+  costmap_[COMBINED_COSTMAP_LAYER_] = generateCombinedCostmap();
+
+  std_msgs::Header in_header = in_objects->header;
+  publishRosMsg(costmap_, in_header);
+}
+
+void CostmapGeneratorLanelet2::sensorPointsCallback(const sensor_msgs::PointCloud2::ConstPtr& in_sensor_points_msg)
+{
+  if (!use_points_)
+  {
+    return;
+  }
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr in_sensor_points(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(*in_sensor_points_msg, *in_sensor_points);
+  costmap_[SENSOR_POINTS_COSTMAP_LAYER_] = generateSensorPointsCostmap(in_sensor_points);
+  costmap_[LANELET2_COSTMAP_LAYER_] = generateLanelet2Costmap();
+  costmap_[COMBINED_COSTMAP_LAYER_] = generateCombinedCostmap();
+
+  std_msgs::Header in_header = in_sensor_points_msg->header;
+  publishRosMsg(costmap_, in_header);
+}
+
+void CostmapGeneratorLanelet2::initGridmap()
+{
+  costmap_.setFrameId(lidar_frame_);
+  costmap_.setGeometry(grid_map::Length(grid_length_x_, grid_length_y_), grid_resolution_,
+                       grid_map::Position(grid_position_x_, grid_position_y_));
+
+  costmap_.add(SENSOR_POINTS_COSTMAP_LAYER_, grid_min_value_);
+  costmap_.add(OBJECTS_BOX_COSTMAP_LAYER_, grid_min_value_);
+  costmap_.add(OBJECTS_CONVEX_HULL_COSTMAP_LAYER_, grid_min_value_);
+  costmap_.add(LANELET2_COSTMAP_LAYER_, grid_min_value_);
+  costmap_.add(COMBINED_COSTMAP_LAYER_, grid_min_value_);
+}
+
+grid_map::Matrix
+CostmapGeneratorLanelet2::generateSensorPointsCostmap(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_sensor_points)
+{
+  grid_map::Matrix sensor_points_costmap = points2costmap_.makeCostmapFromSensorPoints(
+      maximum_lidar_height_thres_, minimum_lidar_height_thres_, grid_min_value_, grid_max_value_, costmap_,
+      SENSOR_POINTS_COSTMAP_LAYER_, in_sensor_points);
+  return sensor_points_costmap;
+}
+
+grid_map::Matrix CostmapGeneratorLanelet2::generateObjectsCostmap(
+    const autoware_msgs::DetectedObjectArray::ConstPtr& in_objects, const bool use_convex_hull)
+{
+  grid_map::Matrix objects_costmap = objects2costmap_.makeCostmapFromObjects(
+      costmap_, expand_polygon_size_, size_of_expansion_kernel_, in_objects, use_convex_hull);
+  return objects_costmap;
+}
+
+grid_map::Matrix CostmapGeneratorLanelet2::generateLanelet2Costmap()
+{
+  grid_map::GridMap lanelet2_costmap = costmap_;
+  if (use_wayarea_ && !area_points_.empty())
+  {
+    has_subscribed_wayarea_ = true;
+    object_map::FillPolygonAreas(lanelet2_costmap, area_points_, LANELET2_COSTMAP_LAYER_, grid_max_value_,
+                                 grid_min_value_, grid_min_value_, grid_max_value_, lidar_frame_, map_frame_,
+                                 tf_listener_);
+  }
+  return lanelet2_costmap[LANELET2_COSTMAP_LAYER_];
+}
+
+grid_map::Matrix CostmapGeneratorLanelet2::generateCombinedCostmap()
+{
+  // assuming combined_costmap is calculated by element wise max operation
+  grid_map::GridMap combined_costmap = costmap_;
+  combined_costmap[COMBINED_COSTMAP_LAYER_].setConstant(grid_min_value_);
+  combined_costmap[COMBINED_COSTMAP_LAYER_] =
+      combined_costmap[COMBINED_COSTMAP_LAYER_].cwiseMax(combined_costmap[SENSOR_POINTS_COSTMAP_LAYER_]);
+  combined_costmap[COMBINED_COSTMAP_LAYER_] =
+      combined_costmap[COMBINED_COSTMAP_LAYER_].cwiseMax(combined_costmap[LANELET2_COSTMAP_LAYER_]);
+  combined_costmap[COMBINED_COSTMAP_LAYER_] =
+      combined_costmap[COMBINED_COSTMAP_LAYER_].cwiseMax(combined_costmap[OBJECTS_BOX_COSTMAP_LAYER_]);
+  combined_costmap[COMBINED_COSTMAP_LAYER_] =
+      combined_costmap[COMBINED_COSTMAP_LAYER_].cwiseMax(combined_costmap[OBJECTS_CONVEX_HULL_COSTMAP_LAYER_]);
+  return combined_costmap[COMBINED_COSTMAP_LAYER_];
+}
+
+void CostmapGeneratorLanelet2::publishRosMsg(const grid_map::GridMap& costmap, const std_msgs::Header& in_header)
+{
+  nav_msgs::OccupancyGrid out_occupancy_grid;
+  grid_map::GridMapRosConverter::toOccupancyGrid(costmap, COMBINED_COSTMAP_LAYER_, grid_min_value_, grid_max_value_,
+                                                 out_occupancy_grid);
+  out_occupancy_grid.header = in_header;
+  pub_occupancy_grid_.publish(out_occupancy_grid);
+
+  grid_map_msgs::GridMap out_gridmap_msg;
+  grid_map::GridMapRosConverter::toMessage(costmap, out_gridmap_msg);
+  out_gridmap_msg.info.header = in_header;
+  pub_costmap_.publish(out_gridmap_msg);
+}

src/ros/catkin/src/costmap_generator/nodes/costmap_generator/costmap_generator_lanelet2_node.cpp

@@ -0,0 +1,43 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  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 <costmap_generator/costmap_generator_lanelet2.h>
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "costmap_generator_lanelet2");
+  CostmapGeneratorLanelet2 costmap_generator_ll2;
+  costmap_generator_ll2.init();
+  costmap_generator_ll2.run();
+  ros::spin();
+
+  return 0;
+}

src/ros/catkin/src/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp

@@ -0,0 +1,43 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  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 "costmap_generator/costmap_generator.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "costmap_generator");
+  CostmapGenerator costmap_generator;
+  costmap_generator.init();
+  costmap_generator.run();
+  ros::spin();
+
+  return 0;
+}

src/ros/catkin/src/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp

@@ -0,0 +1,181 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  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.
+ ********************/
+
+// headers in standard library
+#include <cmath>
+
+// headers in ROS
+#include <tf/transform_datatypes.h>
+
+// headers in local directory
+#include "costmap_generator/objects_to_costmap.h"
+
+// Constructor
+ObjectsToCostmap::ObjectsToCostmap() :
+NUMBER_OF_POINTS(4),
+NUMBER_OF_DIMENSIONS(2),
+OBJECTS_COSTMAP_LAYER_("objects_costmap"),
+BLURRED_OBJECTS_COSTMAP_LAYER_("blurred_objects_costmap")
+{
+}
+
+ObjectsToCostmap::~ObjectsToCostmap()
+{
+}
+
+Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints(const autoware_msgs::DetectedObject& in_object,
+                                                     const double expand_rectangle_size)
+{
+  double length = in_object.dimensions.x + expand_rectangle_size;
+  double width = in_object.dimensions.y + expand_rectangle_size;
+  Eigen::MatrixXd origin_points(NUMBER_OF_DIMENSIONS, NUMBER_OF_POINTS);
+  origin_points << length / 2, length / 2, -length / 2, -length / 2, width / 2, -width / 2, -width / 2, width / 2;
+
+  double yaw = tf::getYaw(in_object.pose.orientation);
+  Eigen::MatrixXd rotation_matrix(NUMBER_OF_DIMENSIONS, NUMBER_OF_DIMENSIONS);
+  rotation_matrix << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw);
+  Eigen::MatrixXd rotated_points = rotation_matrix * origin_points;
+
+  double dx = in_object.pose.position.x;
+  double dy = in_object.pose.position.y;
+  Eigen::MatrixXd transformed_points(NUMBER_OF_DIMENSIONS, NUMBER_OF_POINTS);
+  Eigen::MatrixXd ones = Eigen::MatrixXd::Ones(1, NUMBER_OF_POINTS);
+  transformed_points.row(0) = rotated_points.row(0) + dx * ones;
+  transformed_points.row(1) = rotated_points.row(1) + dy * ones;
+  return transformed_points;
+}
+
+grid_map::Polygon ObjectsToCostmap::makePolygonFromObjectBox(const autoware_msgs::DetectedObject& in_object,
+                                                         const double expand_rectangle_size)
+{
+  grid_map::Polygon polygon;
+  polygon.setFrameId(in_object.header.frame_id);
+  Eigen::MatrixXd rectangle_points = makeRectanglePoints(in_object, expand_rectangle_size);
+  for (int col = 0; col < rectangle_points.cols(); col++)
+  {
+    polygon.addVertex(grid_map::Position(rectangle_points(0, col), rectangle_points(1, col)));
+  }
+  return polygon;
+}
+
+geometry_msgs::Point ObjectsToCostmap::makeExpandedPoint(const geometry_msgs::Point& in_centroid,
+                                                        const geometry_msgs::Point32& in_corner_point,
+                                                        const double expand_polygon_size)
+{
+  geometry_msgs::Point expanded_point;
+  if(expand_polygon_size == 0)
+  {
+    expanded_point.x = in_corner_point.x;
+    expanded_point.y = in_corner_point.y;
+    return expanded_point;
+  }
+  double theta = std::atan2(in_corner_point.y - in_centroid.y, in_corner_point.x - in_centroid.x);
+  double delta_x = expand_polygon_size * std::cos(theta);
+  double delta_y = expand_polygon_size * std::sin(theta);
+  expanded_point.x = in_corner_point.x + delta_x;
+  expanded_point.y = in_corner_point.y + delta_y;
+  return expanded_point;
+}
+
+
+grid_map::Polygon ObjectsToCostmap::makePolygonFromObjectConvexHull(const autoware_msgs::DetectedObject& in_object,
+                                                                   const double expand_polygon_size)
+{
+  grid_map::Polygon polygon;
+  polygon.setFrameId(in_object.header.frame_id);
+
+  double initial_z = in_object.convex_hull.polygon.points[0].z;
+  for (size_t index = 0; index < in_object.convex_hull.polygon.points.size(); index++)
+  {
+    if(in_object.convex_hull.polygon.points[index].z == initial_z)
+    {
+      geometry_msgs::Point centroid = in_object.pose.position;
+      geometry_msgs::Point expanded_point = makeExpandedPoint(centroid,
+        in_object.convex_hull.polygon.points[index], expand_polygon_size);
+      polygon.addVertex(grid_map::Position(expanded_point.x, expanded_point.y));
+    }
+  }
+  return polygon;
+}
+
+void ObjectsToCostmap::setCostInPolygon(const grid_map::Polygon& polygon, const std::string& gridmap_layer_name,
+                                       const float score, grid_map::GridMap& objects_costmap)
+{
+  grid_map::PolygonIterator iterators(objects_costmap, polygon);
+  for (grid_map::PolygonIterator iterator(objects_costmap, polygon); !iterator.isPastEnd(); ++iterator)
+  {
+    const float current_score = objects_costmap.at(gridmap_layer_name, *iterator);
+    if (score > current_score)
+    {
+      objects_costmap.at(gridmap_layer_name, *iterator) = score;
+    }
+  }
+}
+
+grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects(const grid_map::GridMap& costmap,
+                                                         const double expand_polygon_size,
+                                                         const double size_of_expansion_kernel,
+                                                         const autoware_msgs::DetectedObjectArray::ConstPtr& in_objects,
+                                                         const bool use_objects_convex_hull)
+{
+  grid_map::GridMap objects_costmap = costmap;
+  objects_costmap.add(OBJECTS_COSTMAP_LAYER_, 0);
+  objects_costmap.add(BLURRED_OBJECTS_COSTMAP_LAYER_, 0);
+
+  for (const auto& object : in_objects->objects)
+  {
+    grid_map::Polygon polygon, expanded_polygon;
+    if(use_objects_convex_hull)
+    {
+      expanded_polygon = makePolygonFromObjectConvexHull(object, expand_polygon_size);
+    }
+    else
+    {
+      expanded_polygon = makePolygonFromObjectBox(object, expand_polygon_size);
+    }
+    setCostInPolygon(expanded_polygon, OBJECTS_COSTMAP_LAYER_, object.score, objects_costmap);
+    setCostInPolygon(expanded_polygon, BLURRED_OBJECTS_COSTMAP_LAYER_, object.score, objects_costmap);
+  }
+  // Applying mean filter to expanded gridmap
+  const grid_map::SlidingWindowIterator::EdgeHandling edge_handling =
+      grid_map::SlidingWindowIterator::EdgeHandling::CROP;
+  for (grid_map::SlidingWindowIterator iterator(objects_costmap, BLURRED_OBJECTS_COSTMAP_LAYER_, edge_handling,
+                                                size_of_expansion_kernel);
+       !iterator.isPastEnd(); ++iterator)
+  {
+    objects_costmap.at(BLURRED_OBJECTS_COSTMAP_LAYER_, *iterator) =
+        iterator.getData().meanOfFinites();  // Blurring.
+  }
+
+  objects_costmap[OBJECTS_COSTMAP_LAYER_] =
+      objects_costmap[OBJECTS_COSTMAP_LAYER_].cwiseMax(objects_costmap[BLURRED_OBJECTS_COSTMAP_LAYER_]);
+
+  return objects_costmap[OBJECTS_COSTMAP_LAYER_];
+}

src/ros/catkin/src/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp

@@ -0,0 +1,139 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  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 "costmap_generator/points_to_costmap.h"
+
+// Constructor
+PointsToCostmap::PointsToCostmap()
+{
+}
+
+PointsToCostmap::~PointsToCostmap()
+{
+}
+
+void PointsToCostmap::initGridmapParam(const grid_map::GridMap& gridmap)
+{
+  grid_length_x_ = gridmap.getLength().x();
+  grid_length_y_ = gridmap.getLength().y();
+  grid_resolution_ = gridmap.getResolution();
+  grid_position_x_ = gridmap.getPosition().x();
+  grid_position_y_ = gridmap.getPosition().y();
+}
+
+bool PointsToCostmap::isValidInd(const grid_map::Index& grid_ind)
+{
+  bool is_valid = false;
+  int x_grid_ind = grid_ind.x();
+  int y_grid_ind = grid_ind.y();
+  if (x_grid_ind >= 0 && x_grid_ind < std::ceil(grid_length_x_ * (1 / grid_resolution_)) && y_grid_ind >= 0 &&
+      y_grid_ind < std::ceil(grid_length_y_ * (1 / grid_resolution_)))
+  {
+    is_valid = true;
+  }
+  return is_valid;
+}
+
+grid_map::Index PointsToCostmap::fetchGridIndexFromPoint(const pcl::PointXYZ& point)
+{
+  // calculate out_grid_map position
+  const double origin_x_offset = grid_length_x_ / 2.0 - grid_position_x_;
+  const double origin_y_offset = grid_length_y_ / 2.0 - grid_position_y_;
+  // coordinate conversion for making index. Set bottom left to the origin of coordinate (0, 0) in gridmap area
+  double mapped_x = (grid_length_x_ - origin_x_offset - point.x) / grid_resolution_;
+  double mapped_y = (grid_length_y_ - origin_y_offset - point.y) / grid_resolution_;
+
+  int mapped_x_ind = std::ceil(mapped_x);
+  int mapped_y_ind = std::ceil(mapped_y);
+  grid_map::Index index(mapped_x_ind, mapped_y_ind);
+  return index;
+}
+
+std::vector<std::vector<std::vector<double>>> PointsToCostmap::assignPoints2GridCell(
+    const grid_map::GridMap& gridmap, const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_sensor_points)
+{
+  double y_cell_size = std::ceil(grid_length_y_ * (1 / grid_resolution_));
+  double x_cell_size = std::ceil(grid_length_x_ * (1 / grid_resolution_));
+  std::vector<double> z_vec;
+  std::vector<std::vector<double>> vec_y_z(y_cell_size, z_vec);
+  std::vector<std::vector<std::vector<double>>> vec_x_y_z(x_cell_size, vec_y_z);
+
+  for (const auto& point : *in_sensor_points)
+  {
+    grid_map::Index grid_ind = fetchGridIndexFromPoint(point);
+    if (isValidInd(grid_ind))
+    {
+      vec_x_y_z[grid_ind.x()][grid_ind.y()].push_back(point.z);
+    }
+  }
+  return vec_x_y_z;
+}
+
+grid_map::Matrix PointsToCostmap::calculateCostmap(const double maximum_height_thres,
+                                                  const double minimum_lidar_height_thres, const double grid_min_value,
+                                                  const double grid_max_value, const grid_map::GridMap& gridmap,
+                                                  const std::string& gridmap_layer_name,
+                                                  const std::vector<std::vector<std::vector<double>>> grid_vec)
+{
+  grid_map::Matrix gridmap_data = gridmap[gridmap_layer_name];
+  for (size_t x_ind = 0; x_ind < grid_vec.size(); x_ind++)
+  {
+    for (size_t y_ind = 0; y_ind < grid_vec[0].size(); y_ind++)
+    {
+      if (grid_vec[x_ind][y_ind].size() == 0)
+      {
+        gridmap_data(x_ind, y_ind) = grid_min_value;
+        continue;
+      }
+      for (const auto& z : grid_vec[x_ind][y_ind])
+      {
+        if (z > maximum_height_thres || z < minimum_lidar_height_thres)
+        {
+          continue;
+        }
+        gridmap_data(x_ind, y_ind) = grid_max_value;
+        break;
+      }
+    }
+  }
+  return gridmap_data;
+}
+
+grid_map::Matrix PointsToCostmap::makeCostmapFromSensorPoints(
+    const double maximum_height_thres, const double minimum_lidar_height_thres, const double grid_min_value,
+    const double grid_max_value, const grid_map::GridMap& gridmap, const std::string& gridmap_layer_name,
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_sensor_points)
+{
+  initGridmapParam(gridmap);
+  std::vector<std::vector<std::vector<double>>> grid_vec = assignPoints2GridCell(gridmap, in_sensor_points);
+  grid_map::Matrix costmap = calculateCostmap(maximum_height_thres, minimum_lidar_height_thres, grid_min_value,
+                                              grid_max_value, gridmap, gridmap_layer_name, grid_vec);
+  return costmap;
+}

src/ros/catkin/src/costmap_generator/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>costmap_generator</name>
+  <version>1.12.0</version>
+  <description>The costmap_generator package</description>
+  <maintainer email="cirpue49@yahoo.co.jp">ando</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>autoware_msgs</depend>
+  <depend>grid_map_ros</depend>
+  <depend>object_map</depend>
+  <depend>pcl_ros</depend>
+  <depend>roscpp</depend>
+  <depend>tf</depend>
+  <depend>vector_map</depend>
+  <depend>lanelet2_extension</depend>
+</package>

src/ros/catkin/src/costmap_generator/test/src/test_costmap_generator.cpp

@@ -0,0 +1,457 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  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 <gtest/gtest.h>
+
+#include "costmap_generator/costmap_generator.h"
+#include "test_costmap_generator.hpp"
+
+class TestSuite : public ::testing::Test
+{
+public:
+  TestSuite()
+  {
+  }
+  ~TestSuite()
+  {
+  }
+
+  TestClass test_obj_;
+
+protected:
+  void SetUp()
+  {
+    test_obj_.objects2costmap_ = new ObjectsToCostmap();
+    test_obj_.points2costmap_ = new PointsToCostmap();
+    test_obj_.dummy_point_ = new geometry_msgs::Point;
+    test_obj_.dummy_pcl_point_ = new pcl::PointXYZ;
+    test_obj_.dummy_object_ = new autoware_msgs::DetectedObject;
+    test_obj_.dummy_costmap_ = new grid_map::GridMap;
+    test_obj_.dummy_objects_array_.reset(new autoware_msgs::DetectedObjectArray);
+
+    test_obj_.fillDummyObjectParam(test_obj_.dummy_object_);
+    test_obj_.fillDummyCostmapParam(test_obj_.dummy_costmap_);
+    test_obj_.fillDummyObjectsArrayParam(test_obj_.dummy_objects_array_);
+    test_obj_.initDummy3DVecParam();
+
+  };
+  void TearDown()
+  {
+    delete test_obj_.objects2costmap_;
+    delete test_obj_.points2costmap_;
+    delete test_obj_.dummy_point_;
+    delete test_obj_.dummy_pcl_point_;
+    delete test_obj_.dummy_object_;
+    delete test_obj_.dummy_costmap_;
+  };
+};
+
+TEST_F(TestSuite, CheckMakeRectanglePoints)
+{
+  test_obj_.dummy_point_->x = 2;
+  test_obj_.dummy_point_->y = 2;
+  test_obj_.dummy_point_->z = 0;
+  geometry_msgs::Vector3 dimensions;
+  dimensions.x = 2;
+  dimensions.y = 4;
+  dimensions.z = 1;
+  const double pi = std::atan(1) * 4;
+  geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(pi / 2);
+
+  autoware_msgs::DetectedObject object;
+  object.pose.position = *test_obj_.dummy_point_;
+  object.pose.orientation = quat;
+  object.dimensions = dimensions;
+
+  Eigen::MatrixXd rectangle_points = test_obj_.makeRectanglePoints(object, 0);
+
+  // following rep103 ros coordinate
+  const double expected_x = 0;
+  const double expected_y = 3;
+  const double buffer = 0.01;
+  EXPECT_NEAR(expected_x, rectangle_points(0, 0), buffer);
+  EXPECT_DOUBLE_EQ(expected_y, rectangle_points(1, 0));
+}
+
+TEST_F(TestSuite, CheckAssignPoints2GridCell)
+{
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr in_sensor_points(new pcl::PointCloud<pcl::PointXYZ>);
+  test_obj_.dummy_pcl_point_->x = 0.5;
+  test_obj_.dummy_pcl_point_->y = 0.5;
+  test_obj_.dummy_pcl_point_->z = 100;
+  in_sensor_points->push_back(*test_obj_.dummy_pcl_point_);
+
+
+  std::vector<std::vector<std::vector<double>>> grid_vec =
+    test_obj_.assignPoints2GridCell(*test_obj_.dummy_costmap_,
+                                    in_sensor_points);
+
+  const double expected_value = 100;
+  EXPECT_EQ(expected_value, grid_vec[5][5][0]);
+}
+
+TEST_F(TestSuite, CheckFetchGridIndexFromPoint)
+{
+
+  test_obj_.dummy_pcl_point_->x = 3.5;
+  test_obj_.dummy_pcl_point_->y = 0.5;
+  test_obj_.dummy_pcl_point_->z = 100;
+
+  grid_map::Index index =
+  test_obj_.fetchGridIndexFromPoint(*test_obj_.dummy_costmap_,
+                                    *test_obj_.dummy_pcl_point_);
+
+
+  const double expected_x_ind = 2;
+  const double expected_y_ind = 5;
+  EXPECT_EQ(expected_x_ind, index.x());
+  EXPECT_EQ(expected_y_ind, index.y());
+}
+
+TEST_F(TestSuite, CheckValidIndex)
+{
+  grid_map::Index index(2,5);
+  bool is_valid = test_obj_.isValidInd(*test_obj_.dummy_costmap_, index);
+
+  bool expected_valid = true;
+  EXPECT_EQ(expected_valid, is_valid);
+}
+
+TEST_F(TestSuite, CheckNonValidIndex)
+{
+  grid_map::Index index(2,10);
+  bool is_valid = test_obj_.isValidInd(*test_obj_.dummy_costmap_, index);
+
+  bool expected_valid = false;
+  EXPECT_EQ(expected_valid, is_valid);
+}
+
+TEST_F(TestSuite, CheckCalculationPointsCostmap)
+{
+  test_obj_.dummy_3d_vec_->at(5)[5].push_back(2.2);
+
+  grid_map::Matrix costmap_mat = test_obj_.calculateCostmap(
+    test_obj_.dummy_maximum_lidar_height_thres_,
+    test_obj_.dummy_minimum_lidar_height_thres_,
+    test_obj_.dummy_grid_min_value_,
+    test_obj_.dummy_grid_max_value_,
+    *test_obj_.dummy_costmap_,
+    test_obj_.dummy_layer_name_,
+    *test_obj_.dummy_3d_vec_);
+  double expected_cost = 1.0;
+  EXPECT_DOUBLE_EQ(expected_cost, costmap_mat(5,5));
+}
+
+TEST_F(TestSuite, CheckHeightThresholdForCost)
+{
+  test_obj_.dummy_3d_vec_->at(5)[5].push_back(3.2);
+
+  grid_map::Matrix costmap_mat = test_obj_.calculateCostmap(
+    test_obj_.dummy_maximum_lidar_height_thres_,
+    test_obj_.dummy_minimum_lidar_height_thres_,
+    test_obj_.dummy_grid_min_value_,
+    test_obj_.dummy_grid_max_value_,
+    *test_obj_.dummy_costmap_,
+    test_obj_.dummy_layer_name_,
+    *test_obj_.dummy_3d_vec_);
+  double expected_cost = 0.0;
+  EXPECT_DOUBLE_EQ(expected_cost, costmap_mat(5,5));
+}
+
+TEST_F(TestSuite, CheckMakeExpandedPoints)
+{
+  double expand_polygon_size = 1;
+
+  geometry_msgs::Point in_centroid;
+  in_centroid.x = 0;
+  in_centroid.y = 0;
+  in_centroid.z = 0;
+
+  geometry_msgs::Point32 in_corner_point;
+  in_corner_point.x = 1;
+  in_corner_point.y = 1;
+  in_corner_point.z = 1;
+
+  geometry_msgs::Point expanded_point = test_obj_.makeExpandedPoint(in_centroid, in_corner_point, expand_polygon_size);
+  double expected_x = 1.707109;
+  double expected_y = 1.707109;
+  const double buffer = 0.01;
+  EXPECT_NEAR(expected_x, expanded_point.x, buffer);
+  EXPECT_NEAR(expected_y, expanded_point.y, buffer);
+}
+
+TEST_F(TestSuite, ChecMakePolygonFromObjectConvexhull)
+{
+  double expand_polygon_size = 0;
+  geometry_msgs::Point32 point;
+  point.x = -1;
+  point.y = -1;
+  point.z = 0;
+  test_obj_.dummy_object_->convex_hull.polygon.points.push_back(point);
+  point.x = 1;
+  point.y = 1;
+  point.z = 0;
+  test_obj_.dummy_object_->convex_hull.polygon.points.push_back(point);
+  point.x = 0;
+  point.y = 2;
+  point.z = 0;
+  test_obj_.dummy_object_->convex_hull.polygon.points.push_back(point);
+
+  grid_map::Polygon polygon =
+  test_obj_.makePolygonFromObjectConvexHull(*test_obj_.dummy_object_,
+                                                expand_polygon_size);
+  double expected_0_x = -1;
+  double expected_1_y =  1;
+  EXPECT_EQ(expected_0_x, polygon[0].x());
+  EXPECT_EQ(expected_1_y, polygon[1].y());
+}
+
+TEST_F(TestSuite, ChecMakePolygonFromObjectConvexhullDofferentHeight)
+{
+  double expand_polygon_size = 0;
+  geometry_msgs::Point32 point;
+  point.x = -1;
+  point.y = -1;
+  point.z = 0;
+  test_obj_.dummy_object_->convex_hull.polygon.points.push_back(point);
+  point.x = -1;
+  point.y = 3;
+  point.z = 0;
+  test_obj_.dummy_object_->convex_hull.polygon.points.push_back(point);
+  point.x = 1;
+  point.y = 1;
+  point.z = 0;
+  test_obj_.dummy_object_->convex_hull.polygon.points.push_back(point);
+  point.x = 0;
+  point.y = 2;
+  point.z = 3;
+  test_obj_.dummy_object_->convex_hull.polygon.points.push_back(point);
+
+  grid_map::Polygon polygon = test_obj_.makePolygonFromObjectConvexHull(
+                                  *test_obj_.dummy_object_,
+                                   expand_polygon_size);
+  int num_vertices = polygon.nVertices();
+  double expected_num_vertices = 3;
+  EXPECT_EQ(expected_num_vertices, num_vertices);
+}
+
+TEST_F(TestSuite, CheckSetCostInPolygon)
+{
+  grid_map::Polygon polygon;
+  polygon.setFrameId(test_obj_.dummy_layer_name_);
+  polygon.addVertex(grid_map::Position(-1, -1));
+  polygon.addVertex(grid_map::Position(1, -1));
+  polygon.addVertex(grid_map::Position(1, 1));
+  polygon.addVertex(grid_map::Position(-1, 1));
+
+
+  float score = 1;
+  test_obj_.setCostInPolygon(polygon, test_obj_.dummy_layer_name_,
+                             score, *test_obj_.dummy_costmap_);
+  float expected_score = 1;
+  EXPECT_EQ(expected_score,
+    test_obj_.dummy_costmap_->atPosition(test_obj_.dummy_layer_name_, grid_map::Position(0, 0)));
+}
+
+TEST_F(TestSuite, CheckMakeCostmapFromObjects)
+{
+  geometry_msgs::Point32 point;
+  point.x = -1;
+  point.y = -1;
+  point.z = 0;
+  test_obj_.dummy_object_->convex_hull.polygon.points.push_back(point);
+  point.x =  1;
+  point.y = -1;
+  point.z = 0;
+  test_obj_.dummy_object_->convex_hull.polygon.points.push_back(point);
+  point.x = 1;
+  point.y = 1;
+  point.z = 0;
+  test_obj_.dummy_object_->convex_hull.polygon.points.push_back(point);
+  point.x = 0;
+  point.y = 2;
+  point.z = 0;
+  test_obj_.dummy_object_->convex_hull.polygon.points.push_back(point);
+  autoware_msgs::DetectedObjectArray::Ptr dummy_objects(new autoware_msgs::DetectedObjectArray);
+  dummy_objects->objects.push_back(*test_obj_.dummy_object_);
+
+
+  double expand_polygon_size = 0;
+  double size_of_expansion_kernel = 1;
+  bool use_objects_convex_hull = true;
+  grid_map::Matrix gridmap_mat = test_obj_.makeCostmapFromObjects(
+                                          *test_obj_.dummy_costmap_,
+                                          expand_polygon_size,
+                                          size_of_expansion_kernel,
+                                          dummy_objects,
+                                          use_objects_convex_hull);
+  /*
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 0 1 1 0 0 0 0
+    0 0 0 0 1 1 0 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+  */
+  float expected_score = 1;
+  EXPECT_EQ(expected_score,gridmap_mat(5,5));
+}
+
+TEST_F(TestSuite, CheckMakeCostmapFromObjectsExpandSize)
+{
+  double expand_polygon_size = 1;
+  double size_of_expansion_kernel = 1;
+  bool use_objects_convex_hull = true;
+  grid_map::Matrix gridmap_mat = test_obj_.makeCostmapFromObjects(
+                                          *test_obj_.dummy_costmap_,
+                                          expand_polygon_size,
+                                          size_of_expansion_kernel,
+                                          test_obj_.dummy_objects_array_,
+                                          use_objects_convex_hull);
+  /*
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 1 1 1 1 0 0 0
+    0 0 1 1 1 1 1 0 0 0
+    0 0 0 1 1 1 1 0 0 0
+    0 0 0 0 0 0 1 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+    0 0 0 0 0 0 0 0 0 0
+  */
+  float expected_score = 1;
+  EXPECT_EQ(expected_score,gridmap_mat(6,6));
+}
+
+TEST_F(TestSuite, CheckMakeCostmapFromObjectsBlur)
+{
+  double expand_polygon_size = 0;
+  double size_of_expansion_kernel = 3;
+  bool use_objects_convex_hull = true;
+  grid_map::Matrix gridmap_mat = test_obj_.makeCostmapFromObjects(
+                                          *test_obj_.dummy_costmap_,
+                                          expand_polygon_size,
+                                          size_of_expansion_kernel,
+                                          test_obj_.dummy_objects_array_,
+                                          use_objects_convex_hull);
+  /*
+  0  0  0           0           0           0 0.000228624 0.000635066 0.000683101 0.000629966
+  0  0  0           0           0  0.00137174  0.00358177  0.00346354  0.00183676  0.00128529
+  0  0  0           0   0.0123457   0.0306356   0.0267264   0.0117492  0.00456194  0.00264567
+  0  0  0    0.111111    0.262003    0.204948   0.0719709    0.024008  0.00819001   0.0043936
+  0  0  0    0.234568           1           1    0.105625    0.033391    0.010964  0.00580402
+  0  0  0    0.248285           1           1   0.0989154   0.0330867   0.0112765  0.00618194
+  0  0  0    0.138698    0.207193    0.110906   0.0598489    0.024047  0.00904712  0.00537396
+  0  0  0   0.0154109    0.040335   0.0405389    0.024572   0.0130139  0.00573816  0.00386448
+  0  0  0  0.00171233  0.00641596  0.00985468  0.00865679  0.00553543  0.00302765  0.00242517
+  0  0  0 0.000285388  0.00140228  0.00294549  0.00357616   0.0029614  0.00192074  0.00184339
+  */
+  double expected_score = 0.0598489;
+  double buffer = 0.001;
+  EXPECT_NEAR(expected_score,gridmap_mat(6,6), buffer);
+}
+
+TEST_F(TestSuite, CheckMakeCostmapFromObjectsBox)
+{
+  autoware_msgs::DetectedObjectArray::Ptr dummy_objects(new autoware_msgs::DetectedObjectArray);
+  dummy_objects->objects.push_back(*test_obj_.dummy_object_);
+
+  double expand_polygon_size = 1;
+  double size_of_expansion_kernel = 1;
+  bool use_objects_convex_hull = false;
+  grid_map::Matrix gridmap_mat = test_obj_.makeCostmapFromObjects(
+                                          *test_obj_.dummy_costmap_,
+                                          expand_polygon_size,
+                                          size_of_expansion_kernel,
+                                          dummy_objects,
+                                          use_objects_convex_hull);
+  /*
+  0 0 0 0 0 0 0 0 0 0
+  0 0 0 0 0 0 0 0 0 0
+  0 0 0 0 0 0 0 0 0 0
+  0 0 0 0 0 0 0 0 0 0
+  0 0 0 0 1 1 0 0 0 0
+  0 0 0 0 1 1 0 0 0 0
+  0 0 0 0 1 1 0 0 0 0
+  0 0 0 0 0 0 0 0 0 0
+  0 0 0 0 0 0 0 0 0 0
+  0 0 0 0 0 0 0 0 0 0
+  */
+  float expected_score = 1;
+  EXPECT_EQ(expected_score,gridmap_mat(5,5));
+}
+
+TEST_F(TestSuite, CheckMakeCostmapFromObjectsBoxBlur)
+{
+  test_obj_.dummy_object_->pose.position.x = -4;
+  test_obj_.dummy_object_->pose.position.y = -3;
+  test_obj_.dummy_object_->pose.position.z = 0;
+  autoware_msgs::DetectedObjectArray::Ptr dummy_objects(new autoware_msgs::DetectedObjectArray);
+  dummy_objects->objects.push_back(*test_obj_.dummy_object_);
+
+  double expand_polygon_size = 1;
+  double size_of_expansion_kernel = 3;
+  bool use_objects_convex_hull = false;
+  grid_map::Matrix gridmap_mat = test_obj_.makeCostmapFromObjects(
+                                          *test_obj_.dummy_costmap_,
+                                          expand_polygon_size,
+                                          size_of_expansion_kernel,
+                                          dummy_objects,
+                                          use_objects_convex_hull);
+  /*
+  0 0 0 0 0 0           0           0           0           0
+  0 0 0 0 0 0           0           0           0           0
+  0 0 0 0 0 0           0           0           0           0
+  0 0 0 0 0 0           0           0           0           0
+  0 0 0 0 0 0           0           0           0 0.000228624
+  0 0 0 0 0 0           0           0  0.00137174  0.00537266
+  0 0 0 0 0 0           0   0.0123457   0.0306356   0.0406435
+  0 0 0 0 0 0    0.111111    0.262003    0.206481    0.115094
+  0 0 0 0 0 0    0.234568           1           1    0.196272
+  0 0 0 0 0 0    0.372428           1           1    0.263083
+  */
+  float expected_score = 0.11111;
+  float buffer = 0.00001;
+  EXPECT_NEAR(expected_score, gridmap_mat(7,6), buffer);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "TestNode");
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/costmap_generator/test/src/test_costmap_generator.hpp

@@ -0,0 +1,266 @@
+/*
+ *  Copyright (c) 2018, Nagoya University
+ *  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 "costmap_generator/costmap_generator.h"
+
+class TestClass
+{
+private:
+  const double dummy_grid_length_x_;
+  const double dummy_grid_length_y_;
+  const double dummy_grid_resolution_;
+  const double dummy_grid_position_x_;
+  const double dummy_grid_position_y_;
+  const double dummy_initialize_cost_;
+public:
+  TestClass();
+
+  const double dummy_maximum_lidar_height_thres_;
+  const double dummy_minimum_lidar_height_thres_;
+  const double dummy_grid_min_value_;
+  const double dummy_grid_max_value_;
+  const std::string dummy_layer_name_;
+
+  geometry_msgs::Point* dummy_point_;
+
+  pcl::PointXYZ* dummy_pcl_point_;
+
+  autoware_msgs::DetectedObject* dummy_object_;
+
+  autoware_msgs::DetectedObjectArray::Ptr dummy_objects_array_;
+
+  grid_map::GridMap* dummy_costmap_;
+
+  PointsToCostmap *points2costmap_;
+
+  std::unique_ptr<std::vector<std::vector<std::vector<double>>>> dummy_3d_vec_;
+
+  void fillDummyObjectParam(autoware_msgs::DetectedObject* dummy_object);
+
+  void fillDummyObjectsArrayParam(autoware_msgs::DetectedObjectArray::Ptr dummy_objects_array);
+
+  void fillDummyCostmapParam(grid_map::GridMap* dummy_costmap);
+
+  void initDummy3DVecParam();
+
+
+  std::vector<std::vector<std::vector<double>>>
+  assignPoints2GridCell(const grid_map::GridMap& gridmap, const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_sensor_points);
+
+  grid_map::Index fetchGridIndexFromPoint(const grid_map::GridMap& gridmap, const pcl::PointXYZ& point);
+
+  bool isValidInd(const grid_map::GridMap& gridmap, const grid_map::Index& grid_ind);
+
+  grid_map::Matrix calculateCostmap(const double maximum_height_thres,
+                                    const double minimum_lidar_height_thres, const double grid_min_value,
+                                    const double grid_max_value, const grid_map::GridMap& gridmap,
+                                    const std::string& gridmap_layer_name,
+                                    const std::vector<std::vector<std::vector<double>>> grid_vec);
+
+
+  ObjectsToCostmap *objects2costmap_;
+  Eigen::MatrixXd makeRectanglePoints(const autoware_msgs::DetectedObject& in_object,
+    const double expand_rectangle_size);
+  geometry_msgs::Point makeExpandedPoint(const geometry_msgs::Point& in_centroid,
+                                         const geometry_msgs::Point32& in_corner_point,
+                                         const double expand_polygon_size);
+
+  grid_map::Polygon makePolygonFromObjectConvexHull(const autoware_msgs::DetectedObject& in_object,
+                                                    const double expand_polygon_size);
+
+  void setCostInPolygon(const grid_map::Polygon& polygon, const std::string& gridmap_layer_name,
+                                       const float score, grid_map::GridMap& objects_costmap);
+
+  grid_map::Matrix makeCostmapFromObjects(const grid_map::GridMap& costmap,
+                                          const double expand_polygon_size,
+                                          const double size_of_expansion_kernel,
+                                          const autoware_msgs::DetectedObjectArray::ConstPtr& in_objects,
+                                          const bool use_objects_convex_hull);
+};
+
+TestClass::TestClass():
+dummy_grid_length_x_(10),
+dummy_grid_length_y_(10),
+dummy_grid_resolution_(1),
+dummy_grid_position_x_(0),
+dummy_grid_position_y_(0),
+dummy_initialize_cost_(0),
+dummy_maximum_lidar_height_thres_(3.0),
+dummy_minimum_lidar_height_thres_(-2.0),
+dummy_grid_min_value_(0.0),
+dummy_grid_max_value_(1.0),
+dummy_layer_name_("test")
+{
+
+}
+
+void TestClass::fillDummyCostmapParam(grid_map::GridMap* dummy_costmap)
+{
+  dummy_costmap->setGeometry(grid_map::Length(dummy_grid_length_x_, dummy_grid_length_y_), dummy_grid_resolution_,
+                      grid_map::Position(dummy_grid_position_x_, dummy_grid_position_y_));
+
+  dummy_costmap->add(dummy_layer_name_, dummy_initialize_cost_);
+}
+
+void TestClass::fillDummyObjectParam(autoware_msgs::DetectedObject* dummy_object)
+{
+  dummy_object->header.frame_id = "test";
+  dummy_object->pose.position.x = 0;
+  dummy_object->pose.position.y = 0;
+  dummy_object->pose.position.z = 0;
+  dummy_object->pose.orientation.x = 0;
+  dummy_object->pose.orientation.y = 0;
+  dummy_object->pose.orientation.z = 0;
+  dummy_object->pose.orientation.w = 1;
+  dummy_object->dimensions.x = 2;
+  dummy_object->dimensions.y = 1.5;
+  dummy_object->dimensions.z = 2;
+  dummy_object->score = 1;
+}
+
+void TestClass::fillDummyObjectsArrayParam(autoware_msgs::DetectedObjectArray::Ptr dummy_objects_array)
+{
+  autoware_msgs::DetectedObject object;
+  object.header.frame_id = "test";
+  geometry_msgs::Point32 point;
+  point.x = -1;
+  point.y = -1;
+  point.z = 0;
+  object.convex_hull.polygon.points.push_back(point);
+  point.x =  1;
+  point.y = -1;
+  point.z = 0;
+  object.convex_hull.polygon.points.push_back(point);
+  point.x = 1;
+  point.y = 1;
+  point.z = 0;
+  object.convex_hull.polygon.points.push_back(point);
+  point.x = 0;
+  point.y = 2;
+  point.z = 0;
+  object.convex_hull.polygon.points.push_back(point);
+  object.pose.position.x = 0;
+  object.pose.position.y = 0;
+  object.pose.position.z = 0;
+  object.score = 1;
+  dummy_objects_array->objects.push_back(object);
+}
+
+void TestClass::initDummy3DVecParam()
+{
+  double y_cell_size = std::ceil(dummy_grid_length_y_ * (1 / dummy_grid_resolution_));
+  double x_cell_size = std::ceil(dummy_grid_length_x_ * (1 / dummy_grid_resolution_));
+  std::vector<double> z_vec;
+  std::vector<std::vector<double>> vec_y_z(y_cell_size, z_vec);
+  dummy_3d_vec_.reset(new
+    std::vector<std::vector<std::vector<double>>>(x_cell_size, vec_y_z));
+}
+
+
+Eigen::MatrixXd TestClass::makeRectanglePoints(const autoware_msgs::DetectedObject& in_object,
+                                               const double expanded_rectangle_size)
+{
+  return objects2costmap_->makeRectanglePoints(in_object, expanded_rectangle_size);
+}
+
+std::vector<std::vector<std::vector<double>>> TestClass::assignPoints2GridCell(
+    const grid_map::GridMap& gridmap, const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_sensor_points)
+{
+  points2costmap_->grid_length_x_ = gridmap.getLength()[0];
+  points2costmap_->grid_length_y_ = gridmap.getLength()[1];
+  points2costmap_->grid_resolution_ = gridmap.getResolution();
+  points2costmap_->grid_position_x_ = gridmap.getPosition()[0];
+  points2costmap_->grid_position_y_ = gridmap.getPosition()[1];
+  return points2costmap_->assignPoints2GridCell(gridmap, in_sensor_points);
+}
+
+grid_map::Index TestClass::fetchGridIndexFromPoint(const grid_map::GridMap& gridmap, const pcl::PointXYZ& point)
+{
+  points2costmap_->grid_length_x_ = gridmap.getLength()[0];
+  points2costmap_->grid_length_y_ = gridmap.getLength()[1];
+  points2costmap_->grid_resolution_ = gridmap.getResolution();
+  points2costmap_->grid_position_x_ = gridmap.getPosition()[0];
+  points2costmap_->grid_position_y_ = gridmap.getPosition()[1];
+  return points2costmap_->fetchGridIndexFromPoint(point);
+}
+
+bool TestClass::isValidInd(const grid_map::GridMap& gridmap, const grid_map::Index& grid_ind)
+{
+  points2costmap_->grid_length_x_ = gridmap.getLength()[0];
+  points2costmap_->grid_length_y_ = gridmap.getLength()[1];
+  points2costmap_->grid_resolution_ = gridmap.getResolution();
+  points2costmap_->grid_position_x_ = gridmap.getPosition()[0];
+  points2costmap_->grid_position_y_ = gridmap.getPosition()[1];
+  return points2costmap_->isValidInd(grid_ind);
+}
+
+grid_map::Matrix TestClass::calculateCostmap(const double maximum_height_thres,
+                                                  const double minimum_lidar_height_thres, const double grid_min_value,
+                                                  const double grid_max_value, const grid_map::GridMap& gridmap,
+                                                  const std::string& gridmap_layer_name,
+                                                  const std::vector<std::vector<std::vector<double>>> grid_vec)
+{
+  return points2costmap_->calculateCostmap(maximum_height_thres, minimum_lidar_height_thres,
+                                          grid_min_value, grid_max_value,
+                                          gridmap, gridmap_layer_name, grid_vec);
+}
+
+geometry_msgs::Point TestClass::makeExpandedPoint(const geometry_msgs::Point& in_centroid,
+                                       const geometry_msgs::Point32& in_corner_point,
+                                       const double expand_polygon_size)
+{
+  return objects2costmap_->makeExpandedPoint(in_centroid, in_corner_point, expand_polygon_size);
+}
+
+grid_map::Polygon TestClass::makePolygonFromObjectConvexHull(const autoware_msgs::DetectedObject& in_object,
+                                                  const double expand_polygon_size)
+{
+  return objects2costmap_->makePolygonFromObjectConvexHull(in_object, expand_polygon_size);
+}
+
+void TestClass::setCostInPolygon(const grid_map::Polygon& polygon, const std::string& gridmap_layer_name,
+                                     const float score, grid_map::GridMap& objects_costmap)
+{
+  objects2costmap_->setCostInPolygon(polygon, gridmap_layer_name, score, objects_costmap);
+}
+
+grid_map::Matrix TestClass::makeCostmapFromObjects(const grid_map::GridMap& costmap,
+                                       const double expand_polygon_size,
+                                       const double size_of_expansion_kernel,
+                                       const autoware_msgs::DetectedObjectArray::ConstPtr& in_objects,
+                                       const bool use_objects_convex_hull)
+{
+  return objects2costmap_->makeCostmapFromObjects(costmap,
+                                                  expand_polygon_size,
+                                                  size_of_expansion_kernel,
+                                                  in_objects,
+                                                  use_objects_convex_hull);
+}

src/ros/catkin/src/object_map/CHANGELOG.rst

@@ -0,0 +1,511 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package object_map
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* [Feature] costmap generator (`#1774 <https://github.com/CPFL/Autoware/issues/1774>`_)
+  * * Initial commit for visualization package
+  * Removal of all visualization messages from perception nodes
+  * Visualization dependency removal
+  * Launch file modification
+  * * Fixes to visualization
+  * Error on Clustering CPU
+  * Reduce verbosity on markers
+  * Publish acceleration and velocity from ukf tracker
+  * Remove hardcoded path
+  * Updated README
+  * updated prototype
+  * Prototype update for header and usage
+  * Removed unknown label from being reported
+  * Updated publishing orientation to match develop
+  * * Published all the trackers
+  * Added valid field for visualization and future compatibility with ADAS ROI filtering
+  * Add simple functions
+  * * Reversed back UKF node to develop
+  * Formatted speed
+  * Refactor codes
+  * Make tracking visualization work
+  * Relay class info in tracker node
+  * Remove dependency to jskbbox and rosmarker in ukf tracker
+  * apply rosclang to ukf tracker
+  * Refactor codes
+  * Revert "apply rosclang to ukf tracker"
+  * Revert "Remove dependency to jskbbox and rosmarker in ukf tracker"
+  * Revert "Relay class info in tracker node"
+  * delete dependency to jsk and remove pointcloud_frame
+  * get direction nis
+  * set velocity_reliable true in tracker node
+  * Add divided function
+  * add function
+  * Sanity checks
+  * Relay all the data from input DetectedObject
+  * Divided function work both for immukf and sukf
+  * Add comment
+  * Refactor codes
+  * Pass immukf test
+  * make direction assisted tracking work
+  * Visualization fixes
+  * Refactor codes
+  * Tracker Merging step added
+  * Added launch file support for merging phase
+  * lane assisted with sukf
+  * * change only static objects
+  * keep label of the oldest tracker
+  * Static Object discrimination
+  * Non rotating bouding box
+  * no disappear if detector works
+  * Modify removeRedundant a bit
+  * initial commit for costmap generator
+  * add vague stucture
+  * add brief structure fot points2costmap
+  * refactor codes
+  * add sensor points costmap
+  * add waypoint costmap
+  * bug fix for wayarea2costmap
+  * add simple structure for objects2costmap
+  * add vague structure for waypoints2costmap
+  * Replacement of JSK visualization for RViz Native Markers
+  * add occupancy grid visualization
+  * add objects costmap
+  * add minimum height threshold for pointcloud
+  * debug computing.yaml from objects_topic to objects_input
+  * Add blurred costmap
+  * Add comment on computing.yml
+  * visualizing bug fix
+  * Make sure object's cost is 100 and blurred outside of objects
+  * add costmap_generator package
+  * add unit tests
+  * delete test launch file
+  * Apply autoware ros clang
+  * Add README
+  * sync develop's readme
+  * sync develop's code
+  * add convex hull costmap
+  * Relay ros header appropriately
+  * change interaface for generating costmap from points
+  * add test for points2costmap
+  * Modify for how to pick up convex-hull points
+  * add test
+  * add test for objects2costmap
+  * Added missing include
+  * Added missing grid_map_ros dependency
+  * Updated include
+  * Re-strutured include folders
+  * Generic folder name
+  * Fix/costmap generator (`#2077 <https://github.com/CPFL/Autoware/issues/2077>`_)
+  * segmentation fault in  CheckAssignPoints2GridCell
+  * Remove redundant codes in test
+  * Add some variables in SetUp
+  * rename class
+  * rename files
+  * modify tests
+  * Add scription in SetUp
+  * Remove unnecesary in_object
+  * Refactor test codes
+* [fix] Install commands for all the packages (`#1861 <https://github.com/CPFL/Autoware/issues/1861>`_)
+  * Initial fixes to detection, sensing, semantics and utils
+  * fixing wrong filename on install command
+  * Fixes to install commands
+  * Hokuyo fix name
+  * Fix obj db
+  * Obj db include fixes
+  * End of final cleaning sweep
+  * Incorrect command order in runtime manager
+  * Param tempfile not required by runtime_manager
+  * * Fixes to runtime manager install commands
+  * Remove devel directory from catkin, if any
+  * Updated launch files for robosense
+  * Updated robosense
+  * Fix/add missing install (`#1977 <https://github.com/CPFL/Autoware/issues/1977>`_)
+  * Added launch install to lidar_kf_contour_track
+  * Added install to op_global_planner
+  * Added install to way_planner
+  * Added install to op_local_planner
+  * Added install to op_simulation_package
+  * Added install to op_utilities
+  * Added install to sync
+  * * Improved installation script for pointgrey packages
+  * Fixed nodelet error for gmsl cameras
+  * USe install space in catkin as well
+  * add install to catkin
+  * Fix install directives (`#1990 <https://github.com/CPFL/Autoware/issues/1990>`_)
+  * Fixed installation path
+  * Fixed params installation path
+  * Fixed cfg installation path
+  * Delete cache on colcon_release
+* Fix license notice in corresponding package.xml
+* Contributors: Abraham Monrroy Cano, Kosuke Murakami, amc-nu
+
+1.10.0 (2019-01-17)
+-------------------
+* Fixes for catkin_make
+* Switch to Apache 2 license (develop branch) (`#1741 <https://github.com/CPFL/Autoware/issues/1741>`_)
+  * Switch to Apache 2
+  * Replace BSD-3 license header with Apache 2 and reassign copyright to the
+  Autoware Foundation.
+  * Update license on Python files
+  * Update copyright years
+  * Add #ifndef/define _POINTS_IMAGE_H\_
+  * Updated license comment
+* Use colcon as the build tool (`#1704 <https://github.com/CPFL/Autoware/issues/1704>`_)
+  * Switch to colcon as the build tool instead of catkin
+  * Added cmake-target
+  * Added note about the second colcon call
+  * Added warning about catkin* scripts being deprecated
+  * Fix COLCON_OPTS
+  * Added install targets
+  * Update Docker image tags
+  * Message packages fixes
+  * Fix missing dependency
+* Add grid_map_vizualization to run_depend
+* Feature/perception visualization cleanup (`#1648 <https://github.com/CPFL/Autoware/issues/1648>`_)
+  * * Initial commit for visualization package
+  * Removal of all visualization messages from perception nodes
+  * Visualization dependency removal
+  * Launch file modification
+  * * Fixes to visualization
+  * Error on Clustering CPU
+  * Reduce verbosity on markers
+  * intial commit
+  * * Changed to 2 spaces indentation
+  * Added README
+  * Fixed README messages type
+  * 2 space indenting
+  * ros clang format
+  * Publish acceleration and velocity from ukf tracker
+  * Remove hardcoded path
+  * Updated README
+  * updated prototype
+  * Prototype update for header and usage
+  * Removed unknown label from being reported
+  * Updated publishing orientation to match develop
+  * * Published all the trackers
+  * Added valid field for visualization and future compatibility with ADAS ROI filtering
+  * Add simple functions
+  * Refacor code
+  * * Reversed back UKF node to develop
+  * Formatted speed
+  * Refactor codes
+  * Refactor codes
+  * Refactor codes
+  * Refacor codes
+  * Make tracking visualization work
+  * Relay class info in tracker node
+  * Remove dependency to jskbbox and rosmarker in ukf tracker
+  * apply rosclang to ukf tracker
+  * Refactor codes
+  * Refactor codes
+  * add comment
+  * refactor codes
+  * Revert "Refactor codes"
+  This reverts commit 135aaac46e49cb18d9b76611576747efab3caf9c.
+  * Revert "apply rosclang to ukf tracker"
+  This reverts commit 4f8d1cb5c8263a491f92ae5321e5080cb34b7b9c.
+  * Revert "Remove dependency to jskbbox and rosmarker in ukf tracker"
+  This reverts commit 4fa1dd40ba58065f7afacc5e478001078925b27d.
+  * Revert "Relay class info in tracker node"
+  This reverts commit 1637baac44c8d3d414cc069f3af12a79770439ae.
+  * delete dependency to jsk and remove pointcloud_frame
+  * get direction nis
+  * set velocity_reliable true in tracker node
+  * Add divided function
+  * add function
+  * Sanity checks
+  * Relay all the data from input DetectedObject
+  * Divided function work both for immukf and sukf
+  * Add comment
+  * Refactor codes
+  * Pass immukf test
+  * make direction assisted tracking work
+  * Visualization fixes
+  * Refacor codes
+  * Refactor codes
+  * Refactor codes
+  * refactor codes
+  * refactor codes
+  * Refactor codes
+  * refactor codes
+  * Tracker Merging step added
+  * Added launch file support for merging phase
+  * lane assisted with sukf
+  * Refactor codes
+  * Refactor codes
+  * * change only static objects
+  * keep label of the oldest tracker
+  * Static Object discrimination
+  * Non rotating bouding box
+  * no disappear if detector works
+  * Modify removeRedundant a bit
+  * Replacement of JSK visualization for RViz Native Markers
+  * Added Models namespace to visualization
+  * Naming change for matching the perception component graph
+  * * Added 3D Models for different classes in visualization
+  * 2D Rect node visualize_rects added to visualization_package
+* Fix Ros/ROS naming convention
+* Contributors: Abraham Monrroy Cano, Akihito Ohsato, Esteve Fernandez, amc-nu
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+
+1.8.0 (2018-08-31)
+------------------
+* [Fix] Moved C++11 flag to autoware_build_flags (`#1395 <https://github.com/CPFL/Autoware/pull/1395>`_)
+* [Feature] Makes sure that all binaries have their dependencies linked (`#1385 <https://github.com/CPFL/Autoware/pull/1385>`_)
+* [Fix] Extend and Update interface.yaml (`#1291 <https://github.com/CPFL/Autoware/pull/1291>`_)
+* Contributors: Esteve Fernandez, Kenji Funaoka
+
+1.7.0 (2018-05-18)
+------------------
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* [fix] Fixes for all packages and dependencies (`#1240 <https://github.com/CPFL/Autoware/pull/1240>`_)
+  * Initial Cleanup
+  * fixed also for indigo
+  * kf cjeck
+  * Fix road wizard
+  * Added travis ci
+  * Trigger CI
+  * Fixes to cv_tracker and lidar_tracker cmake
+  * Fix kitti player dependencies
+  * Removed unnecessary dependencies
+  * messages fixing for can
+  * Update build script travis
+  * Travis Path
+  * Travis Paths fix
+  * Travis test
+  * Eigen checks
+  * removed unnecessary dependencies
+  * Eigen Detection
+  * Job number reduced
+  * Eigen3 more fixes
+  * More Eigen3
+  * Even more Eigen
+  * find package cmake modules included
+  * More fixes to cmake modules
+  * Removed non ros dependency
+  * Enable industrial_ci for indidog and kinetic
+  * Wrong install command
+  * fix rviz_plugin install
+  * FastVirtualScan fix
+  * Fix Qt5 Fastvirtualscan
+  * Fixed qt5 system dependencies for rosdep
+  * NDT TKU Fix catkin not pacakged
+  * More in detail dependencies fixes for more packages
+  * GLEW library for ORB
+  * Ignore OrbLocalizer
+  * Ignore Version checker
+  * Fix for driveworks interface
+  * driveworks not catkinpackagedd
+  * Missing catkin for driveworks
+  * libdpm opencv not catkin packaged
+  * catkin lib gnss  not included in obj_db
+  * Points2Polygon fix
+  * More missing dependencies
+  * image viewer not packaged
+  * Fixed SSH2 detection, added viewers for all distros
+  * Fix gnss localizer incorrect dependency config
+  * Fixes to multiple packages dependencies
+  * gnss plib and package
+  * More fixes to gnss
+  * gnss dependencies for gnss_loclaizer
+  * Missing gnss dependency for gnss on localizer
+  * More fixes for dependencies
+  Replaced gnss for autoware_gnss_library
+  * gnss more fixes
+  * fixes to more dependencies
+  * header dependency
+  * Debug message
+  * more debug messages changed back to gnss
+  * debud messages
+  * gnss test
+  * gnss install command
+  * Several fixes for OpenPlanner and its lbiraries
+  * Fixes to ROSInterface
+  * More fixes to robotsdk and rosinterface
+  * robotsdk calibration fix
+  * Fixes to rosinterface robotsdk libraries and its nodes
+  * Fixes to Qt5 missing dependencies in robotsdk
+  * glviewer missing dependencies
+  * Missing qt specific config cmake for robotsdk
+  * disable cv_tracker
+  * Fix to open planner un needed dependendecies
+  * Fixes for libraries indecision maker
+  * Fixes to libraries decision_maker installation
+  * Gazebo on Kinetic
+  * Added Missing library
+  * * Removed Gazebo and synchonization packages
+  * Renames vmap in lane_planner
+  * Added installation commands for missing pakcages
+  * Fixes to lane_planner
+  * Added NDT TKU Glut extra dependencies
+  * ndt localizer/lib fast pcl fixes
+  re enable cv_tracker
+  * Fix kf_lib
+  * Keep industrial_ci
+  * Fixes for dpm library
+  * Fusion lib fixed
+  * dpm and fusion header should match exported project name
+  * Fixes to dpm_ocv  ndt_localizer and pcl_omp
+  * no fast_pcl anymore
+  * fixes to libdpm and its package
+  * CI test
+  * test with native travis ci
+  * missing update for apt
+  * Fixes to pcl_omp installation and headers
+  * Final fixes for tests, modified README
+  * * Fixes to README
+  * Enable industrial_ci
+  * re enable native travis tests
+* Fix/cmake cleanup (`#1156 <https://github.com/CPFL/Autoware/pull/1156>`_)
+  * Initial Cleanup
+  * fixed also for indigo
+  * kf cjeck
+  * Fix road wizard
+  * Added travis ci
+  * Trigger CI
+  * Fixes to cv_tracker and lidar_tracker cmake
+  * Fix kitti player dependencies
+  * Removed unnecessary dependencies
+  * messages fixing for can
+  * Update build script travis
+  * Travis Path
+  * Travis Paths fix
+  * Travis test
+  * Eigen checks
+  * removed unnecessary dependencies
+  * Eigen Detection
+  * Job number reduced
+  * Eigen3 more fixes
+  * More Eigen3
+  * Even more Eigen
+  * find package cmake modules included
+  * More fixes to cmake modules
+  * Removed non ros dependency
+  * Enable industrial_ci for indidog and kinetic
+  * Wrong install command
+  * fix rviz_plugin install
+  * FastVirtualScan fix
+  * Fix Qt5 Fastvirtualscan
+  * Fixed qt5 system dependencies for rosdep
+  * NDT TKU Fix catkin not pacakged
+  * Fixes from industrial_ci
+* Editorial changes to README files. See `#1124 <https://github.com/CPFL/Autoware/pull/1124>`_. (`#1125 <https://github.com/CPFL/Autoware/pull/1125>`_)
+* Feature/occupancygrid filter (`#1002 <https://github.com/CPFL/Autoware/pull/1002>`_)
+  * Add grid map filter node
+  * Add wayarea2grid node
+  * Replace dist_transform with grid_map_filter
+  * Add Runtime Manager UI for grid_map_filter node
+  * Add UI for wayarea2grid node
+  * Add instruction videos
+  * Fix videos
+  * Both node handles were private
+  * Added Comments Documentation
+  Code refactoring to follow standards
+  Added libraries
+  Separation of Vectormap Processing inside Clustering
+  * Added documentation
+  * Changed variable name
+  * Added Road Occupancy Processor package
+  * Added extra documentation
+  Added commands to RunTimeManager
+* Contributors: Abraham Monrroy, David, Kosuke Murakami, TomohitoAndo
+
+1.6.3 (2018-03-06)
+------------------
+
+1.6.2 (2018-02-27)
+------------------
+* Update CHANGELOG
+* Contributors: Yusuke FUJII
+
+1.6.1 (2018-01-20)
+------------------
+* update CHANGELOG
+* Contributors: Yusuke FUJII
+
+1.6.0 (2017-12-11)
+------------------
+* Prepare release for 1.6.0
+* Contributors: Yamato ANDO
+
+1.5.1 (2017-09-25)
+------------------
+* Release/1.5.1 (`#816 <https://github.com/cpfl/autoware/issues/816>`_)
+  * fix a build error by gcc version
+  * fix build error for older indigo version
+  * update changelog for v1.5.1
+  * 1.5.1
+* Contributors: Yusuke FUJII
+
+1.5.0 (2017-09-21)
+------------------
+* Update changelog
+* Add feature to put simulated obstacles in astar planner
+* Contributors: TomohitoAndo, Yusuke FUJII
+
+1.4.0 (2017-08-04)
+------------------
+* version number must equal current release number so we can start releasing in the future
+* added changelogs
+* Contributors: Dejan Pangercic
+
+1.3.1 (2017-07-16)
+------------------
+
+1.3.0 (2017-07-14)
+------------------
+* Add map offset parameters to points2costmap node
+* Add dist_transform node
+* convert to autoware_msgs
+* Contributors: TomohitoAndo, YamatoAndo
+
+1.2.0 (2017-06-07)
+------------------
+* fixed build issues
+* fix circular-dependency
+* Update potential field
+* Contributors: Shohei Fujii, Yukihiro Saito, Yusuke FUJII
+
+1.1.2 (2017-02-27 23:10)
+------------------------
+
+1.1.1 (2017-02-27 22:25)
+------------------------
+
+1.1.0 (2017-02-24)
+------------------
+* Add missing dependency
+* Change to use final object topic in potential field
+* Cleaned potential field node
+* Add vscan points in potential field
+* Add potential field package
+* Contributors: Yukihiro Saito, h_ohta
+
+1.0.1 (2017-01-14)
+------------------
+
+1.0.0 (2016-12-22)
+------------------
+* Add parameter for subscribing topic
+* Fix costmap orientation
+* Fix to subscribe the new topic
+* Ignore 0 ranges
+* Fix cost calculation for unknown costs
+* Change variable name to be easier to understand
+* Fix calculation of index
+* Remove needless nesting
+* Modify calculation for costs
+* Remove needless compiling flags
+* Fix dependencies
+* Remove unused header
+* Initialize a previous position when declared
+* Change variable type from integer to bool
+* Impletement some functions as struct method
+* Use call by value instead of call by reference with premitive data types
+* Add license statement
+* Remeve automatically generated comments
+* Add semantics package
+* Contributors: Syohei YOSHIDA, TomohitoAndo

src/ros/catkin/src/object_map/CMakeLists.txt

@@ -0,0 +1,182 @@
+cmake_minimum_required(VERSION 2.8.12)
+
+project(object_map)
+
+find_package(autoware_build_flags REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  autoware_msgs
+  grid_map_cv
+  grid_map_msgs
+  grid_map_ros
+  lanelet2_extension
+  nav_msgs
+  pcl_conversions
+  pcl_ros
+  roscpp
+  roslint
+  sensor_msgs
+  tf
+  vector_map
+)
+
+# TODO add other files into roslint
+set(ROSLINT_CPP_OPTS "--filter=-build/c++14")
+roslint_cpp(
+  nodes/wayarea2grid_lanelet2/wayarea2grid_lanelet2_node.cpp
+  nodes/wayarea2grid_lanelet2/wayarea2grid_lanelet2.cpp
+  include/wayarea2grid_lanelet2/wayarea2grid_lanelet2.h
+)
+
+find_package(Qt5Core REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(PCL REQUIRED)
+find_package(OpenMP)
+
+if (OPENMP_FOUND)
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif ()
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES object_map_utils_lib
+  CATKIN_DEPENDS
+    pcl_ros
+    pcl_conversions
+    tf
+    sensor_msgs
+    nav_msgs
+    autoware_msgs
+    grid_map_msgs
+    vector_map
+    lanelet2_extension
+)
+
+set(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+### object_map_utils ###
+add_library(object_map_utils_lib
+  include/object_map/object_map_utils.cpp
+  include/object_map/object_map_utils.hpp
+)
+target_link_libraries(object_map_utils_lib
+  ${catkin_LIBRARIES}
+)
+
+### laserscan2costmap ###
+add_executable(laserscan2costmap
+  nodes/laserscan2costmap/laserscan2costmap.cpp
+)
+
+target_link_libraries(laserscan2costmap
+  ${catkin_LIBRARIES}
+)
+
+
+### potential_field ###
+add_executable(potential_field
+  nodes/potential_field/potential_field.cpp
+)
+add_dependencies(potential_field
+  ${catkin_EXPORTED_TARGETS}
+)
+target_link_libraries(potential_field
+  ${catkin_LIBRARIES}
+)
+
+### grid_map_filter ###
+add_library(grid_map_filter_lib
+  nodes/grid_map_filter/grid_map_filter.h
+  include/object_map/object_map_utils.hpp
+  nodes/grid_map_filter/grid_map_filter.cpp
+)
+target_link_libraries(grid_map_filter_lib
+  ${catkin_LIBRARIES}
+  object_map_utils_lib
+)
+add_executable(grid_map_filter
+  nodes/grid_map_filter/grid_map_filter_node.cpp
+  nodes/grid_map_filter/grid_map_filter.h
+)
+target_link_libraries(grid_map_filter
+  ${catkin_LIBRARIES}
+  object_map_utils_lib
+  grid_map_filter_lib
+)
+
+### wayarea2grid ###
+add_library(wayarea2grid_lib
+  nodes/wayarea2grid/wayarea2grid.h
+  include/object_map/object_map_utils.hpp
+  nodes/wayarea2grid/wayarea2grid.cpp
+)
+target_link_libraries(wayarea2grid_lib
+  ${catkin_LIBRARIES}
+  object_map_utils_lib
+)
+
+add_executable(wayarea2grid
+  nodes/wayarea2grid/wayarea2grid.h
+  nodes/wayarea2grid/wayarea2grid_node.cpp
+)
+
+target_link_libraries(wayarea2grid
+  ${catkin_LIBRARIES}
+  wayarea2grid_lib
+)
+      
+add_library(wayarea2grid_lanelet2_lib
+  include/object_map/object_map_utils.hpp
+  nodes/wayarea2grid_lanelet2/wayarea2grid_lanelet2.cpp
+)
+target_link_libraries(wayarea2grid_lanelet2_lib
+  ${catkin_LIBRARIES}
+  object_map_utils_lib
+)
+
+add_executable(wayarea2grid_lanelet2
+  nodes/wayarea2grid_lanelet2/wayarea2grid_lanelet2_node.cpp
+)
+
+target_link_libraries(wayarea2grid_lanelet2
+  ${catkin_LIBRARIES}
+  wayarea2grid_lanelet2_lib
+)
+
+install(
+  TARGETS
+    wayarea2grid
+    wayarea2grid_lib
+    wayarea2grid_lanelet2
+    wayarea2grid_lanelet2_lib
+    grid_map_filter_lib
+    grid_map_filter
+    potential_field
+    laserscan2costmap
+    object_map_utils_lib
+  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
+)
+        
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.hpp"
+)
+
+install(DIRECTORY config/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
+)
+
+if (CATKIN_ENABLE_TESTING)
+  roslint_add_test()
+endif()

src/ros/catkin/src/object_map/README.md

@@ -0,0 +1,105 @@
+# The `object_map` Package
+
+This package contains nodes to extract information from the LiDAR sensor and from ADAS Maps.
+The processed result is published as GridMaps (grid_map) and/or occupancy grids(nav_msgs).
+
+---
+
+## Nodes in the Package
+
+### laserscan2costmap
+
+This node uses 2D LaserScan messages to generate the OccupancyGrid.
+
+#### Input topics
+Default: `/scan` (sensor_msgs::LaserScan) from vscan.
+
+#### Output topics
+`/ring_ogm` (nav_msgs::OccupancyGrid) is the output OccupancyGrid with values ranging from 0-100.
+
+##### How to launch
+It can be launched as follows:
+ 1. Using the Runtime Manager by clicking the `laserscan2costmap` checkbox under the *Semantics* section in the Computing tab.
+ 2. From a sourced terminal by executing: `roslaunch object_map laserscan2costmap.launch`.
+
+##### Parameters available in roslaunch and rosrun
+ * `resolution` defines the equivalent value of a cell in the grid in meters. Smaller values result in better accuracy at the expense of memory and computing cost (default value: 0.1).
+ * `cell_width` represents the width in meters of the OccupancyGrid around the origin of the PointCloud (default: 150).
+ * `cell_height` represents the height in meters of the OccupancyGrid around the origin of the PointCloud (default: 150).
+ * `offset_x` indicates if the center of the OccupancyGrid will be shifted by this distance, to the left(-) or right(+) (default: 25).
+ * `offset_y` indicates if the center of the OccupancyGrid will be shifted by this distance, to the back(-) or front(+)  (default: 0).
+ * `offset_z` indicates if the center of the OccupancyGrid will be shifted by this distance, below(-) or above(+) (default: 0).
+ * `scan_topic` is the PointCloud topic source.
+
+---
+
+### wayarea2grid
+
+The ADAS Maps used in Autoware may contain the definition of the road areas, which is useful for computing whether a region is drivable or not.
+This node reads the data from an ADAS Map (VectorMap) and extracts the 3D positions of the road regions. It projects them into an OccupancyGrid and sets the value to `128` if the area is **road** and `255` if it is not. These values are used because this node uses 8-bit bitmaps (grid_map).
+
+#### Input topics
+`/vector_map` (vector_map_msgs::WayArea) from the VectorMap publisher.
+`/tf` to obtain the transform between the vector map(grid_frame) and the sensor(sensor_frame) .
+
+#### Output topics
+`/grid_map_wayarea` (grid_map::GridMap) is the resulting GridMap with values ranging from 0-255.
+`/occupancy_wayarea` (nav_msgs::OccupancyGrid) is the resulting OccupancyGrid with values ranging from 0-255.
+
+##### How to launch
+It can be launched as follows:
+ 1. Using the Runtime Manager by clicking the `wayarea2grid` checkbox under the *Semantics* section in the Computing tab.
+ 2. From a sourced terminal by executing: `roslaunch object_map wayarea2grid.launch`
+
+##### Parameters available in roslaunch and rosrun
+* `sensor_frame` defines the coordinate frame of the vehicle origin (default value: velodyne).
+* `grid_frame` defines the coordinate frame of the map (default: map).
+* `grid_resolution` defines the equivalent value of a cell in the grid in meters. Smaller values result in better accuracy at the expense of memory and computing cost (default: 0.2).
+* `grid_length_x` represents the width in meters of the OccupancyGrid around the origin of the PointCloud (default: 25).
+* `grid_length_y` represents the height in meters of the OccupancyGrid around the origin of the PointCloud  (default: 0).
+* `grid_position_x` indicates if the center of the OccupancyGrid will be shifted by this distance, left(-) or right(+) (default: 0).
+* `grid_position_y` indicates if the center of the OccupancyGrid will be shifted by this distance, back(-) or front(+) (default: 0).
+
+---
+
+### grid_map_filter
+This node can combine sensor, map, and perception data as well as other OccupancyGrids. It generates OccupancyGrids with useful data for navigation purposes. It publishes three different layers. More details are provided in the output topics section below.
+
+#### Input topics
+`/vector_map` (vector_map_msgs::WayArea) from the VectorMap publisher.
+`/tf` to obtain the transform between the vector map (grid_frame) and the sensor (sensor_frame).
+`/semantics/costmap_generator/occupancy_grid` (nav_msgs::OccupancyGrid) calculated by `costmap_generator` package.
+
+#### Output topics
+`/filtered_grid_map` (grid_map::GridMap) which contains 3 layers and is also published as in regular OccupancyGrid messages.
+`distance_transform` (nav_msgs::OccupancyGrid) applies a distance transform to the OccupancyGrid obtained from the `/realtime_cost_map`, allowing us to obtain a gradient of probabilities surrounding the obstacles PointCloud.
+`dist_wayarea` (nav_msgs::OccupancyGrid) contains a combination of the distance transform and the road data, as described in the `wayarea2grid` node.
+`circle` (nav_msgs::OccupancyGrid) draws a circle surrounding the `/realtime_cost_map`.
+
+The output topics are configured as described in the `grid_map` package, and the configuration file is inside the `config` folder of the package.
+
+##### How to launch
+It can be launched as follows:
+ 1. Using the Runtime Manager by clicking the `grid_map_filter` checkbox under the *Semantics* section in the Computing tab.
+ 2. From a sourced terminal by executing: `roslaunch object_map grid_map_filter.launch`.
+
+##### Parameters available in roslaunch and rosrun
+ * `map_frame` defines the coordinate system of the realtime costmap (default value: map).
+ * `map_topic` defines the topic where the realtime costmap is being published (default: /realtime_cost_map).
+ * `dist_transform_distance` defines the maximum distance to calculate the distance transform, in meters (default: 2.0).
+ * `use_wayarea` indicates whether or not to use the road regions to filter the cost map (default: true).
+ * `use_fill_circle` enables or disables the generation of the circle layer (default: true).
+ * `fill_circle_cost_threshold` indicates the minimum cost value threshold value to decide if a circle will be drawn (default: 20)
+ * `circle_radius` defines the radius of the circle, in meters (default: 1.7).
+
+---
+
+## Instruction Videos
+
+### grid_map_filter
+
+[![](https://img.youtube.com/vi/bl-nG8Zv-C0/0.jpg)](https://www.youtube.com/watch?v=bl-nG8Zv-C0)
+
+### wayarea2grid
+
+[![](https://img.youtube.com/vi/UkcO4V-0NOw/0.jpg)](https://www.youtube.com/watch?v=UkcO4V-0NOw)

src/ros/catkin/src/object_map/config/grid_map_filter.yaml

@@ -0,0 +1,26 @@
+grid_map_topic: /filtered_grid_map
+grid_map_visualizations:
+  - name: original
+    type: occupancy_grid
+    params:
+     layer: original
+     data_min: 0
+     data_max: 255
+  - name: distance_transform
+    type: occupancy_grid
+    params:
+     layer: distance_transform
+     data_min: 0
+     data_max: 255
+  - name: dist_wayarea
+    type: occupancy_grid
+    params:
+     layer: dist_wayarea
+     data_min: 0
+     data_max: 255
+  - name: circle
+    type: occupancy_grid
+    params:
+     layer: circle
+     data_min: 0
+     data_max: 255

src/ros/catkin/src/object_map/config/potential.yaml

@@ -0,0 +1,42 @@
+grid_map_topic: /potential_field
+
+grid_map_visualizations:
+
+  - name: elevation_points
+    type: point_cloud
+    params:
+      layer: potential_field
+
+  - name: map_region
+    type: map_region
+    params:
+      color: 3289650
+      line_width: 0.003
+
+  - name: traversability_grid
+    type: occupancy_grid
+    params:
+      layer: traversability
+      data_min: 0.08
+      data_max: -0.16
+
+  - name: elevation_cells
+    type: grid_cells
+    params:
+      layer: potential_field
+      lower_threshold: 0.0
+      upper_threshold: 0.5
+
+  - name: flat_grid
+    type: flat_point_cloud
+    params:
+      height: 0.0
+
+  - name: surface_normals
+    type: vectors
+    params:
+      layer_prefix: normal_
+      position_layer: potential_field
+      scale: 0.06
+      line_width: 0.005
+      color: 15600153 # red

src/ros/catkin/src/object_map/include/object_map/object_map_utils.cpp

@@ -0,0 +1,201 @@
+/*
+ * 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 "object_map/object_map_utils.hpp"
+
+namespace object_map
+{
+  geometry_msgs::Point TransformPoint(const geometry_msgs::Point &in_point, const tf::Transform &in_tf)
+  {
+    tf::Point tf_point;
+    tf::pointMsgToTF(in_point, tf_point);
+
+    tf_point = in_tf * tf_point;
+
+    geometry_msgs::Point out_point;
+    tf::pointTFToMsg(tf_point, out_point);
+
+    return out_point;
+  }
+
+  void PublishGridMap(const grid_map::GridMap &in_gridmap, const ros::Publisher &in_publisher)
+  {
+    grid_map_msgs::GridMap message;
+    grid_map::GridMapRosConverter::toMessage(in_gridmap, message);
+    in_publisher.publish(message);
+  }
+
+  void PublishOccupancyGrid(const grid_map::GridMap &in_gridmap,
+                            const ros::Publisher &in_publisher,
+                            const std::string& in_layer,
+                            double in_min_value,
+                            double in_max_value,
+                            double in_height)
+  {
+    nav_msgs::OccupancyGrid message;
+    grid_map::GridMapRosConverter::toOccupancyGrid(in_gridmap, in_layer, in_min_value, in_max_value, message );
+    message.info.origin.position.z = in_height;
+    in_publisher.publish(message);
+  }
+
+  tf::StampedTransform FindTransform(const std::string &in_target_frame, const std::string &in_source_frame,
+                                     const tf::TransformListener &in_tf_listener)
+  {
+    tf::StampedTransform transform;
+
+    try
+    {
+      in_tf_listener.lookupTransform(in_target_frame, in_source_frame, ros::Time(0), transform);
+    }
+    catch (tf::TransformException ex)
+    {
+      ROS_ERROR("%s", ex.what());
+    }
+
+    return transform;
+  }
+
+  std::vector<geometry_msgs::Point>
+  SearchAreaPoints(const vector_map::Area &in_area, const vector_map::VectorMap &in_vectormap)
+  {
+    std::vector<geometry_msgs::Point> area_points;
+    std::vector<geometry_msgs::Point> area_points_empty;
+
+    if (in_area.aid == 0)
+      return area_points_empty;
+
+    vector_map_msgs::Line line = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Line>(in_area.slid));
+    // must set beginning line
+    if (line.lid == 0 || line.blid != 0)
+      return area_points_empty;
+
+    // Search all lines in in_area
+    while (line.flid != 0)
+    {
+      vector_map_msgs::Point bp = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Point>(line.bpid));
+      if (bp.pid == 0)
+        return area_points_empty;
+
+      vector_map_msgs::Point fp = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Point>(line.fpid));
+      if (fp.pid == 0)
+        return area_points_empty;
+
+      // 2 points of line
+      area_points.push_back(vector_map::convertPointToGeomPoint(bp));
+      area_points.push_back(vector_map::convertPointToGeomPoint(fp));
+
+      line = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Line>(line.flid));
+      if (line.lid == 0)
+        return area_points_empty;
+    }
+
+    vector_map_msgs::Point bp = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Point>(line.bpid));
+    vector_map_msgs::Point fp = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Point>(line.fpid));
+    if (bp.pid == 0 || fp.pid == 0)
+      return area_points_empty;
+
+    area_points.push_back(vector_map::convertPointToGeomPoint(bp));
+    area_points.push_back(vector_map::convertPointToGeomPoint(fp));
+
+    return area_points;
+  }
+
+  void FillPolygonAreas(grid_map::GridMap &out_grid_map, const std::vector<std::vector<geometry_msgs::Point>> &in_area_points,
+                          const std::string &in_grid_layer_name, const int in_layer_background_value,
+                          const int in_layer_min_value, const int in_fill_color, const int in_layer_max_value,
+                          const std::string &in_tf_target_frame, const std::string &in_tf_source_frame,
+                          const tf::TransformListener &in_tf_listener)
+  {
+    if(!out_grid_map.exists(in_grid_layer_name))
+    {
+      out_grid_map.add(in_grid_layer_name);
+    }
+    out_grid_map[in_grid_layer_name].setConstant(in_layer_background_value);
+
+    cv::Mat original_image;
+    grid_map::GridMapCvConverter::toImage<unsigned char, 1>(out_grid_map,
+                                                            in_grid_layer_name,
+                                                            CV_8UC1,
+                                                            in_layer_min_value,
+                                                            in_layer_max_value,
+                                                            original_image);
+
+    cv::Mat filled_image = original_image.clone();
+
+    tf::StampedTransform tf = FindTransform(in_tf_target_frame, in_tf_source_frame, in_tf_listener);
+
+    // calculate out_grid_map position
+    grid_map::Position map_pos = out_grid_map.getPosition();
+    double origin_x_offset = out_grid_map.getLength().x() / 2.0 - map_pos.x();
+    double origin_y_offset = out_grid_map.getLength().y() / 2.0 - map_pos.y();
+
+    for (const auto &points : in_area_points)
+    {
+      std::vector<cv::Point> cv_points;
+
+      for (const auto &p : points)
+      {
+        // transform to GridMap coordinate
+        geometry_msgs::Point tf_point = TransformPoint(p, tf);
+
+        // coordinate conversion for cv image
+        double cv_x = (out_grid_map.getLength().y() - origin_y_offset - tf_point.y) / out_grid_map.getResolution();
+        double cv_y = (out_grid_map.getLength().x() - origin_x_offset - tf_point.x) / out_grid_map.getResolution();
+        cv_points.emplace_back(cv::Point(cv_x, cv_y));
+      }
+
+      cv::fillConvexPoly(filled_image, cv_points.data(), cv_points.size(), cv::Scalar(in_fill_color));
+    }
+
+    // convert to ROS msg
+    grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(filled_image,
+                                                                      in_grid_layer_name,
+                                                                      out_grid_map,
+                                                                      in_layer_min_value,
+                                                                      in_layer_max_value);
+  }
+
+  void LoadRoadAreasFromVectorMap(ros::NodeHandle& in_private_node_handle,
+                                  std::vector<std::vector<geometry_msgs::Point>>& out_area_points)
+  {
+    vector_map::VectorMap vmap;
+    vmap.subscribe(in_private_node_handle,
+                   vector_map::Category::POINT | vector_map::Category::LINE | vector_map::Category::AREA |
+                   vector_map::Category::WAY_AREA, 10);
+
+    std::vector<vector_map_msgs::WayArea> way_areas =
+        vmap.findByFilter([](const vector_map_msgs::WayArea &way_area)
+                          {
+                            return true;
+                          });
+
+    if (way_areas.empty())
+    {
+      ROS_WARN_STREAM("No WayArea...");
+      return;
+    }
+
+    for (const auto &way_area : way_areas)
+    {
+      vector_map_msgs::Area area = vmap.findByKey(vector_map::Key<vector_map::Area>(way_area.aid));
+      out_area_points.emplace_back(SearchAreaPoints(area, vmap));
+    }
+
+  }
+
+} // namespace object_map

src/ros/catkin/src/object_map/include/object_map/object_map_utils.hpp

@@ -0,0 +1,118 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ ********************
+ *
+ */
+#ifndef PROJECT_OBJECT_MAP_UTILS_H
+#define PROJECT_OBJECT_MAP_UTILS_H
+
+#include <ros/ros.h>
+#include <geometry_msgs/PolygonStamped.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+
+#include <vector_map/vector_map.h>
+
+#include <grid_map_ros/grid_map_ros.hpp>
+#include <grid_map_msgs/GridMap.h>
+#include <grid_map_cv/grid_map_cv.hpp>
+
+namespace object_map
+{
+  /*!
+   * Transforms a point using the given transformation
+   * @param[in] in_point Point to transform
+   * @param[in] in_tf Transformation to apply
+   * @return Transformed point
+   */
+  geometry_msgs::Point TransformPoint(const geometry_msgs::Point &in_point, const tf::Transform &in_tf);
+
+  /*!
+   * Publishes in_gridmap using the specified in_publisher
+   * @param[in] in_gridmap GridMap object to publish
+   * @param[in] in_publisher Valid Publisher object to use
+   */
+  void PublishGridMap(const grid_map::GridMap &in_gridmap, const ros::Publisher &in_publisher);
+
+  /*!
+   * Convert and publishes a GridMap layer to a standard Ros OccupancyGrid
+   * @param[in] in_gridmap GridMap object to extract the layer
+   * @param[in] in_publisher ROS Publisher to use to publish the occupancy grid
+   * @param[in] in_layer Name of the layer to convert
+   * @param[in] in_min_value Minimum value in the layer
+   * @param[in] in_max_value Maximum value in the layer
+   */
+  void PublishOccupancyGrid(const grid_map::GridMap &in_gridmap,
+                            const ros::Publisher &in_publisher,
+                            const std::string &in_layer,
+                            double in_min_value,
+                            double in_max_value,
+                            double in_height);
+
+  /*!
+   * Obtains the registered transform in the tf tree
+   * @param[in] in_target_frame Target frame to obtain the transformation
+   * @param[in] in_source_frame Source Frame from which is desired to transform
+   * @param[in] in_tf_listener Valid Object Listener
+   * @return Current Transform from source to target, if available. Otherwise, it returns an identity transform.
+   */
+  tf::StampedTransform FindTransform(const std::string &in_target_frame,
+                                     const std::string &in_source_frame,
+                                     const tf::TransformListener &in_tf_listener);
+
+  /*!
+     * Loads regions defined as road inside the vector map, according to the field named "wayarea"
+     */
+  void LoadRoadAreasFromVectorMap(ros::NodeHandle& in_private_node_handle,
+                                  std::vector<std::vector<geometry_msgs::Point>>& out_area_points);
+
+  /*!
+   * Extracts all the points forming in_area inside in_vectormap
+   * @param[in] in_area Area to extract its points
+   * @param[in] in_vectormap VectorMap object to which in_area belongs
+   * @return Array of points forming in_area
+   */
+  std::vector<geometry_msgs::Point>
+  SearchAreaPoints(const vector_map::Area &in_area, const vector_map::VectorMap &in_vectormap);
+
+  /*!
+   * Projects the in_area_points forming the road, stores the result in out_grid_map.
+   * @param[out] out_grid_map GridMap object to add the road grid
+   * @param[in] in_area_points Array of points containing the wayareas
+   * @param[in] in_grid_layer_name Name to assign to the layer
+   * @param[in] in_layer_background_value Empty state value
+   * @param[in] in_fill_color Value to fill on wayareas
+   * @param[in] in_layer_min_value Minimum value in the layer
+   * @param[in] in_layer_max_value Maximum value in the later
+   * @param[in] in_tf_target_frame Target frame to transform the wayarea points
+   * @param[in] in_tf_source_frame Source frame, where the points are located
+   * @param[in] in_tf_listener Valid listener to obtain the transformation
+   */
+  void FillPolygonAreas(grid_map::GridMap &out_grid_map,
+                        const std::vector<std::vector<geometry_msgs::Point>> &in_area_points,
+                        const std::string &in_grid_layer_name,
+                        const int in_layer_background_value,
+                        const int in_fill_color,
+                        const int in_layer_min_value,
+                        const int in_layer_max_value,
+                        const std::string &in_tf_target_frame,
+                        const std::string &in_tf_source_frame,
+                        const tf::TransformListener &in_tf_listener);
+
+
+
+} // namespace object_map
+
+#endif //PROJECT_OBJECT_MAP_UTILS_H

src/ros/catkin/src/object_map/include/wayarea2grid_lanelet2/wayarea2grid_lanelet2.h

@@ -0,0 +1,99 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ ********************
+ */
+#ifndef WAYAREA2GRID_LANELET2_WAYAREA2GRID_LANELET2_H
+#define WAYAREA2GRID_LANELET2_WAYAREA2GRID_LANELET2_H
+
+#include <iostream>
+#include <string>
+#include <vector>
+
+// Headers from ROS packages
+#include <cv_bridge/cv_bridge.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+
+// Headers from Autoware
+#include <autoware_lanelet2_msgs/MapBin.h>
+#include <grid_map_msgs/GridMap.h>
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_extension/projection/mgrs_projector.h>
+#include <lanelet2_extension/utility/message_conversion.h>
+#include <lanelet2_extension/utility/query.h>
+#include <lanelet2_extension/visualization/visualization.h>
+#include <grid_map_cv/grid_map_cv.hpp>
+#include <grid_map_ros/grid_map_ros.hpp>
+#include <object_map/object_map_utils.hpp>
+
+// Headers from opencv
+#include <opencv2/highgui/highgui.hpp>
+
+namespace object_map
+{
+constexpr int occupancy_road = 128;
+constexpr int occupancy_no_road = 255;
+
+class WayareaToGridLanelet2
+{
+public:
+  WayareaToGridLanelet2();
+
+  void Run();
+
+private:
+  // handle
+  ros::NodeHandle node_handle_;
+  ros::NodeHandle private_node_handle_;
+
+  ros::Publisher publisher_grid_map_;
+  ros::Publisher publisher_occupancy_;
+
+  grid_map::GridMap gridmap_;
+
+  bool loaded_lanelet_map_ = false;
+  lanelet::LaneletMapPtr lanelet_map_;
+
+  std::string sensor_frame_;
+  std::string grid_frame_;
+
+  const std::string grid_layer_name_ = "wayarea";
+
+  double grid_resolution_;
+  double grid_length_x_;
+  double grid_length_y_;
+  double grid_position_x_;
+  double grid_position_y_;
+  double grid_position_z_;
+
+  tf::TransformListener tf_listener_;
+
+  const int grid_min_value_ = 0;
+  const int grid_max_value_ = 255;
+
+  std::vector<std::vector<geometry_msgs::Point>> area_points_;
+
+  /*!
+   * Initializes ROS Publisher, Subscribers and sets the configuration parameters
+   */
+  void InitializeROSIo();
+  void laneletBinMapCallback(const autoware_lanelet2_msgs::MapBin& msg);
+  void initAreaPointsFromLaneletMap();
+};
+
+}  // namespace object_map
+
+#endif  // WAYAREA2GRID_LANELET2_WAYAREA2GRID_LANELET2_H

src/ros/catkin/src/object_map/interface.yaml

@@ -0,0 +1,15 @@
+- name: /grid_map_filter
+  publish: [/filtered_grid_map]
+  subscribe: [/realtime_cost_map]
+- name: /laserscan2costmap
+  publish: [/ring_ogm]
+  subscribe: [/scan]
+- name: /points2costmap
+  publish: [/realtime_cost_map]
+  subscribe: [/points_lanes, /obstacle_sim_pointcloud]
+- name: /potential_field
+  publish: [/potential_field]
+  subscribe: [/detected_objects, /vscan_points, /next_target_mark]
+- name: /wayarea2grid
+  publish: [/grid_map_wayarea, /occupancy_wayarea]
+  subscribe: []

src/ros/catkin/src/object_map/launch/grid_map_filter.launch

@@ -0,0 +1,29 @@
+<launch>
+  <!-- node parameters -->
+  <arg name="map_frame" default="map" />
+  <arg name="map_topic" default="/realtime_cost_map" />
+  <arg name="dist_transform_distance" default="2.0" />
+  <arg name="use_dist_transform" default="true" />
+  <arg name="use_wayarea" default="true" />
+  <arg name="use_fill_circle" default="true" />
+  <arg name="fill_circle_cost_threshold" default="20" /> <!-- 0 ~ 100 -->
+  <arg name="circle_radius" default="1.7" />
+
+  <!-- Launch node -->
+  <node pkg="object_map" type="grid_map_filter" name="grid_map_filter" output="screen">
+    <param name="map_frame" value="$(arg map_frame)" />
+    <param name="map_topic" value="$(arg map_topic)" />
+    <param name="dist_transform_distance" value="$(arg dist_transform_distance)" />
+    <param name="use_dist_transform" value="$(arg use_dist_transform)" />
+    <param name="use_wayarea" value="$(arg use_wayarea)" />
+    <param name="use_fill_circle" value="$(arg use_fill_circle)" />
+    <param name="fill_circle_cost_threshold" value="$(arg fill_circle_cost_threshold)" />
+    <param name="circle_radius" value="$(arg circle_radius)" />
+  </node>
+
+<!-- Launch the grid map visualizer -->
+  <node pkg="grid_map_visualization" type="grid_map_visualization" name="grid_map_filter_visualization" output="screen">
+    <rosparam command="load" file="$(find object_map)/config/grid_map_filter.yaml" />
+  </node>
+
+</launch>

src/ros/catkin/src/object_map/launch/laserscan2costmap.launch

@@ -0,0 +1,22 @@
+<!-- -->
+<launch>
+    <arg name="resolution" default="0.1" />
+    <arg name="scan_size_x" default="1000" />
+    <arg name="scan_size_y" default="1000" />
+    <arg name="map_size_x" default="500" />
+    <arg name="map_size_y" default="500" />
+    <arg name="scan_topic" default="/scan" />
+    <arg name="sensor_frame" default="/velodyne" />
+
+  <node pkg="object_map" type="laserscan2costmap" name="laserscan2costmap" output="screen">
+        <param name="resolution" value="$(arg resolution)" />
+        <param name="scan_size_x" value="$(arg scan_size_x)" />
+        <param name="scan_size_y" value="$(arg scan_size_y)" />
+        <param name="map_size_x" value="$(arg map_size_x)" />
+        <param name="map_size_y" value="$(arg map_size_y)" />
+        <param name="scan_topic" value="$(arg scan_topic)" />
+        <param name="sensor_frame" value="$(arg sensor_frame)" />
+  </node>
+
+</launch>
+

src/ros/catkin/src/object_map/launch/potential_field.launch

@@ -0,0 +1,27 @@
+<!-- -->
+<launch>
+  <node pkg="grid_map_visualization" type="grid_map_visualization" name="grid_map_visualization">
+    <rosparam command="load" file="$(find object_map)/config/potential.yaml" />
+  </node>
+  <arg name="use_obstacle_box" default="true" />
+  <arg name="use_vscan_points" default="false" />
+  <arg name="use_target_waypoint" default="false" />
+
+  <arg name="map_resolution" default="0.25" />
+  <arg name="map_x_size" default="40.0" />
+  <arg name="map_y_size" default="25.0" />
+  <arg name="map_x_offset" default="10.0" />
+
+  <node pkg="tf" type="static_transform_publisher" name="potential_field_link_tf_publiser" args="$(arg map_x_offset) 0 0 0 0 0 base_link potential_field_link 100" />
+
+  <node pkg="object_map" type="potential_field" name="potential_field">
+    <param name="use_obstacle_box" type="bool" value="$(arg use_obstacle_box)"/>
+    <param name="use_vscan_points" type="bool" value="$(arg use_vscan_points)"/>
+    <param name="use_target_waypoint" type="bool" value="$(arg use_target_waypoint)"/>
+    <param name="map_resolution" type="double" value="$(arg map_resolution)"/>
+    <param name="map_x_size" type="double" value="$(arg map_x_size)"/>
+    <param name="map_y_size" type="double" value="$(arg map_y_size)"/>
+    <param name="map_x_offset" type="double" value="$(arg map_x_offset)"/>
+  </node>
+
+</launch>

src/ros/catkin/src/object_map/launch/wayarea2grid.launch

@@ -0,0 +1,24 @@
+<launch>
+  <!-- node parameters -->
+  <arg name="sensor_frame" default="velodyne" />
+  <arg name="grid_frame" default="map" />
+  <arg name="grid_resolution" default="0.3" />
+  <arg name="grid_length_x" default="150" />
+  <arg name="grid_length_y" default="150" />
+  <arg name="grid_position_x" default="0" />
+  <arg name="grid_position_y" default="0" />
+  <arg name="grid_position_z" default="-2" />
+
+  <!-- Launch node -->
+  <node pkg="object_map" type="wayarea2grid" name="wayarea2grid" output="screen">
+    <param name="sensor_frame" value="$(arg sensor_frame)" />
+    <param name="grid_frame" value="$(arg grid_frame)" />
+    <param name="grid_resolution" value="$(arg grid_resolution)" />
+    <param name="grid_length_x" value="$(arg grid_length_x)" />
+    <param name="grid_length_y" value="$(arg grid_length_y)" />
+    <param name="grid_position_x" value="$(arg grid_position_x)" />
+    <param name="grid_position_y" value="$(arg grid_position_y)" />
+    <param name="grid_position_z" value="$(arg grid_position_z)" />
+  </node>
+
+</launch>

src/ros/catkin/src/object_map/launch/wayarea2grid_lanelet2.launch

@@ -0,0 +1,24 @@
+<launch>
+  <!-- node parameters -->
+  <arg name="sensor_frame" default="velodyne" />
+  <arg name="grid_frame" default="map" />
+  <arg name="grid_resolution" default="0.3" />
+  <arg name="grid_length_x" default="150" />
+  <arg name="grid_length_y" default="150" />
+  <arg name="grid_position_x" default="0" />
+  <arg name="grid_position_y" default="0" />
+  <arg name="grid_position_z" default="-2" />
+
+  <!-- Launch node -->
+  <node pkg="object_map" type="wayarea2grid_lanelet2" name="wayarea2grid_lanelet2" output="screen">
+    <param name="sensor_frame" value="$(arg sensor_frame)" />
+    <param name="grid_frame" value="$(arg grid_frame)" />
+    <param name="grid_resolution" value="$(arg grid_resolution)" />
+    <param name="grid_length_x" value="$(arg grid_length_x)" />
+    <param name="grid_length_y" value="$(arg grid_length_y)" />
+    <param name="grid_position_x" value="$(arg grid_position_x)" />
+    <param name="grid_position_y" value="$(arg grid_position_y)" />
+    <param name="grid_position_z" value="$(arg grid_position_z)" />
+  </node>
+
+</launch>

src/ros/catkin/src/object_map/launch/wayarea2grid_option.launch

@@ -0,0 +1,42 @@
+<launch>
+  <!-- node parameters -->
+  <arg name="sensor_frame" default="velodyne" />
+  <arg name="grid_frame" default="map" />
+  <arg name="grid_resolution" default="0.3" />
+  <arg name="grid_length_x" default="150" />
+  <arg name="grid_length_y" default="150" />
+  <arg name="grid_position_x" default="0" />
+  <arg name="grid_position_y" default="0" />
+  <arg name="grid_position_z" default="-2" />
+  <arg name="use_ll2" default="false" />
+
+  <!-- Launch node -->
+
+  <group if="$(arg use_ll2)">
+    <node pkg="object_map" type="wayarea2grid_lanelet2" name="wayarea2grid_lanelet2" output="screen">
+      <param name="sensor_frame" value="$(arg sensor_frame)" />
+      <param name="grid_frame" value="$(arg grid_frame)" />
+      <param name="grid_resolution" value="$(arg grid_resolution)" />
+      <param name="grid_length_x" value="$(arg grid_length_x)" />
+      <param name="grid_length_y" value="$(arg grid_length_y)" />
+      <param name="grid_position_x" value="$(arg grid_position_x)" />
+      <param name="grid_position_y" value="$(arg grid_position_y)" />
+      <param name="grid_position_z" value="$(arg grid_position_z)" />
+    </node>
+  </group>
+
+  <group unless="$(arg use_ll2)">
+      <node pkg="object_map" type="wayarea2grid" name="wayarea2grid" output="screen">
+        <param name="sensor_frame" value="$(arg sensor_frame)" />
+        <param name="grid_frame" value="$(arg grid_frame)" />
+        <param name="grid_resolution" value="$(arg grid_resolution)" />
+        <param name="grid_length_x" value="$(arg grid_length_x)" />
+        <param name="grid_length_y" value="$(arg grid_length_y)" />
+        <param name="grid_position_x" value="$(arg grid_position_x)" />
+        <param name="grid_position_y" value="$(arg grid_position_y)" />
+        <param name="grid_position_z" value="$(arg grid_position_z)" />
+      </node>
+  </group>
+
+
+</launch>

src/ros/catkin/src/object_map/nodes/grid_map_filter/grid_map_filter.cpp

@@ -0,0 +1,197 @@
+/*
+ * 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 "grid_map_filter.h"
+
+namespace object_map
+{
+
+// Constructor
+  GridMapFilter::GridMapFilter() :
+      private_node_handle_("~")
+  {
+    InitializeROSIo();
+    LoadRoadAreasFromVectorMap(private_node_handle_, area_points_);
+  }
+
+  void GridMapFilter::InitializeROSIo()
+  {
+    private_node_handle_.param<std::string>("map_frame", map_frame_, "map");
+    private_node_handle_.param<std::string>("map_topic", map_topic_, "/realtime_cost_map");
+    private_node_handle_.param<double>("dist_transform_distance", dist_transform_distance_, 3.0);
+    private_node_handle_.param<bool>("use_dist_transform", use_dist_transform_, false);
+    private_node_handle_.param<bool>("use_wayarea", use_wayarea_, false);
+    private_node_handle_.param<bool>("use_fill_circle", use_fill_circle_, false);
+    private_node_handle_.param<int>("fill_circle_cost_threshold", fill_circle_cost_thresh_, 20);
+    private_node_handle_.param<double>("circle_radius", circle_radius_, 1.7);
+
+    occupancy_grid_sub_ = nh_.subscribe<nav_msgs::OccupancyGrid>(map_topic_, 10,
+                                                                 &GridMapFilter::OccupancyGridCallback, this);
+
+    grid_map_pub_ = nh_.advertise<grid_map_msgs::GridMap>("filtered_grid_map", 1, true);
+
+  }
+
+
+  void GridMapFilter::Run()
+  {
+    ros::spin();
+  }
+
+  void GridMapFilter::OccupancyGridCallback(const nav_msgs::OccupancyGridConstPtr &in_message)
+  {
+    // timer start
+    //auto start = std::chrono::system_clock::now();
+
+    std::string original_layer = "original";
+
+    grid_map::GridMap map({original_layer, "distance_transform", "wayarea", "dist_wayarea", "circle"});
+
+    //store costmap map_topic_ into the original layer
+    grid_map::GridMapRosConverter::fromOccupancyGrid(*in_message, "original", map);
+
+    // apply distance transform to OccupancyGrid
+    if (use_dist_transform_)
+    {
+      CreateDistanceTransformLayer(map, original_layer);
+    }
+
+    // fill polygon
+    if (!area_points_.empty() && use_wayarea_)
+    {
+      FillPolygonAreas(map, area_points_, grid_road_layer_, OCCUPANCY_NO_ROAD, OCCUPANCY_ROAD, grid_min_value_,
+                       grid_max_value_, map.getFrameId(), map_frame_, tf_listener_);
+
+      map["dist_wayarea"] = map["distance_transform"] + map["wayarea"];
+    }
+
+    // fill circle
+    if (use_fill_circle_)
+    {
+      int cost_threshold = fill_circle_cost_thresh_;
+      // convert to cv image size
+      int radius = circle_radius_ / map.getResolution();
+      DrawCirclesInLayer(map, original_layer, cost_threshold, radius);
+    }
+
+    // publish grid map as ROS message
+    PublishGridMap(map, grid_map_pub_);
+
+    // timer end
+    //auto end = std::chrono::system_clock::now();
+    //auto usec = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
+    //std::cout << "time: " << usec / 1000.0 << " [msec]" << std::endl;
+  }
+
+  void GridMapFilter::CreateDistanceTransformLayer(grid_map::GridMap &out_grid_map, const std::string &in_layer)
+  {
+    cv::Mat original_image;
+    if (!out_grid_map.exists(in_layer))
+    {
+      ROS_INFO("%s layer not yet available", in_layer.c_str());
+      return;
+    }
+    grid_map::GridMapCvConverter::toImage<unsigned char, 1>(out_grid_map,
+                                                            in_layer,
+                                                            CV_8UC1,
+                                                            original_image);
+
+    cv::Mat binary_image;
+    cv::threshold(original_image,
+                  binary_image,
+                  fill_circle_cost_thresh_,
+                  grid_max_value_,
+                  cv::THRESH_BINARY_INV);
+
+    // distance transform method
+    // 3: fast
+    // 5: slow but accurate
+    cv::Mat dt_image;
+    cv::distanceTransform(binary_image, dt_image, CV_DIST_L2, 5);
+
+    // Convert to int...
+    cv::Mat dt_int_image(dt_image.size(), CV_8UC1);
+    cv::Mat dt_int_inv_image(dt_image.size(), CV_8UC1);
+
+    // max distance for cost propagation
+    double max_dist = dist_transform_distance_; // meter
+    double resolution = out_grid_map.getResolution();
+
+    for (int y = 0; y < dt_image.rows; y++)
+    {
+      for (int x = 0; x < dt_image.cols; x++)
+      {
+        // actual distance [meter]
+        double dist = dt_image.at<float>(y, x) * resolution;
+        if (dist > max_dist)
+          dist = max_dist;
+
+        // Make value range 0 ~ 255
+        int round_dist = dist / max_dist * grid_max_value_;
+        int inv_round_dist = grid_max_value_ - round_dist;
+
+        dt_int_image.at<unsigned char>(y, x) = round_dist;
+        dt_int_inv_image.at<unsigned char>(y, x) = inv_round_dist;
+      }
+    }
+
+    // convert to ROS msg
+    grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(dt_int_inv_image,
+                                                                      "distance_transform",
+                                                                      out_grid_map,
+                                                                      grid_min_value_,
+                                                                      grid_max_value_);
+  }
+
+  void GridMapFilter::DrawCirclesInLayer(grid_map::GridMap &out_gridmap,
+                                         const std::string &in_layer_name,
+                                         double in_draw_threshold,
+                                         double in_radius)
+  {
+    cv::Mat original_image;
+
+    grid_map::GridMapCvConverter::toImage<unsigned char, 1>(out_gridmap,
+                                                            in_layer_name,
+                                                            CV_8UC1,
+                                                            costmap_min_,
+                                                            costmap_max_,
+                                                            original_image);
+
+    cv::Mat filled_image = original_image.clone();
+
+    for (int y = 0; y < original_image.rows; y++)
+    {
+      for (int x = 0; x < original_image.cols; x++)
+      {
+        // uchar -> int
+        int data = original_image.at<unsigned char>(y, x);
+
+        if (data > fill_circle_cost_thresh_)
+        {
+          cv::circle(filled_image, cv::Point(x, y), in_radius, cv::Scalar(OCCUPANCY_CIRCLE), -1, CV_AA);
+        }
+      }
+    }
+    // convert to ROS msg
+    grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(filled_image,
+                                                                      "circle",
+                                                                      out_gridmap,
+                                                                      grid_min_value_,
+                                                                      grid_max_value_);
+  }
+
+
+}  // namespace object_map

src/ros/catkin/src/object_map/nodes/grid_map_filter/grid_map_filter.h

@@ -0,0 +1,107 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ ********************/
+#ifndef GRID_MAP_FILTER_H
+#define GRID_MAP_FILTER_H
+
+#include <iostream>
+#include <vector>
+#include <string>
+#include <chrono>
+
+#include <ros/ros.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <tf/transform_listener.h>
+
+#include <vector_map/vector_map.h>
+
+#include <grid_map_ros/grid_map_ros.hpp>
+#include <grid_map_msgs/GridMap.h>
+#include <grid_map_cv/grid_map_cv.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "object_map/object_map_utils.hpp"
+
+namespace object_map
+{
+
+  class GridMapFilter
+  {
+  public:
+    GridMapFilter();
+
+    void Run();
+
+  private:
+    // handle
+    ros::NodeHandle                 nh_;
+    ros::NodeHandle                 private_node_handle_;
+
+    ros::Publisher                  grid_map_pub_;
+    ros::Subscriber                 occupancy_grid_sub_;
+
+    std::string                     map_frame_;
+    std::string                     map_topic_;
+    const std::string               grid_road_layer_    = "wayarea";
+    double                          dist_transform_distance_;
+    bool                            use_dist_transform_;
+    bool                            use_wayarea_;
+    bool                            use_fill_circle_;
+    int                             fill_circle_cost_thresh_;
+    double                          circle_radius_;
+
+    int                             OCCUPANCY_ROAD      = 128;
+    int                             OCCUPANCY_NO_ROAD   = 255;
+    int                             OCCUPANCY_CIRCLE    = 255;
+    const int                       grid_min_value_     = 0;
+    const int                       grid_max_value_     = 255;
+    const int                       costmap_min_        = 0;
+    const int                       costmap_max_        = 100;
+
+    tf::TransformListener           tf_listener_;
+
+    std::vector<std::vector<geometry_msgs::Point>> area_points_;
+
+    void OccupancyGridCallback(const nav_msgs::OccupancyGridConstPtr &in_message);
+
+    /*!
+     * Initializes ROS Publisher, Subscribers and sets the configuration parameters
+     */
+    void InitializeROSIo();
+
+    /*!
+     * Applies a Distance Transform to the specified layer in in_layer contained in the GridMap
+     * @param[out] out_grid_map GridMap object to add the layer
+     * @param[in] layer Name of the layer to use for the transform
+     */
+    void CreateDistanceTransformLayer(grid_map::GridMap &out_grid_map, const std::string &in_layer);
+
+    /*!
+     * Draws a circle in the specified layer in the given GridMap if the cell value is larger than a threshold
+     * @param[out] out_gridmap GridMap object to modify
+     * @param[in] in_layer_name Name of the layer to draw
+     * @param[in] in_draw_threshold threshold to decide if the circle will be drawn
+     * @param[in] in_radius Radius of the circle
+     */
+    void DrawCirclesInLayer(grid_map::GridMap &out_gridmap,
+                            const std::string &in_layer_name,
+                            double in_draw_threshold,
+                            double in_radius);
+  };
+
+}  // namespace object_map
+#endif  // GRID_MAP_FILTER_H

src/ros/catkin/src/object_map/nodes/grid_map_filter/grid_map_filter_node.cpp

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

src/ros/catkin/src/object_map/nodes/laserscan2costmap/laserscan2costmap.cpp

@@ -0,0 +1,415 @@
+/*
+ * 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 <iostream>
+#include <vector>
+#include <string>
+#include <algorithm>
+
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <tf/transform_listener.h>
+#include <nav_msgs/OccupancyGrid.h>
+
+namespace
+{
+constexpr auto OGM_FRAME = "/map";
+constexpr int OCCUPIED_MAX = 8;
+constexpr int OCCUPIED_MIN = -8;
+constexpr int OCCUPIED_INCREMENT = 2;
+constexpr int FREE_INCREMENT = 1;
+std::string g_sensor_frame = "/velodyne";  // sensor which publihes lasescan message
+std::string g_scan_topic = "/scan";        // laser scan topic
+double g_resolution = 0.1;                 // [m]
+int g_scan_size_x = 1000;                  // actual scanning size
+int g_scan_size_y = 1000;
+int g_map_size_x = 500;  // publishing occupancy grid map size
+int g_map_size_y = 500;
+
+struct Grid
+{
+  int index;
+  int index_x, index_y;
+  double x, y;
+  double weight;
+  double range;
+
+  void calcCoordinate();
+  void calcRange();
+  int calcGlobalIndex(const tf::StampedTransform& transform) const;
+};
+
+struct Cost
+{
+  int occupied = 0;
+  int free     = 0;
+  bool unknown = true;
+
+  void accumulateCost(int occ, int free);
+};
+
+ros::Publisher g_map_pub;
+tf::TransformListener* g_tf_listenerp;
+
+double calcYawFromQuaternion(const tf::Quaternion& q)
+{
+  tf::Matrix3x3 m(q);
+  double roll, pitch, yaw;
+  m.getRPY(roll, pitch, yaw);
+
+  return yaw;
+}
+
+// Accumulate cost value
+void Cost::accumulateCost(int occupied_inc, int free_inc)
+{
+  occupied += occupied_inc - free_inc;
+  free     += free_inc;
+  unknown   = false;
+
+  if (occupied > OCCUPIED_MAX)
+    occupied = OCCUPIED_MAX;
+
+  if (occupied < OCCUPIED_MIN)
+    occupied = OCCUPIED_MIN;
+}
+
+// Calcurate grid's coordinate in sensor frame
+void Grid::calcCoordinate()
+{
+  // index coordinate
+  index_x = index % g_scan_size_x;
+  index_y = (index - index_x) / g_scan_size_x;
+
+  // actual coordinate
+  x = (index_x - g_scan_size_x / 2.0) * g_resolution;
+  y = (index_y - g_scan_size_y / 2.0) * g_resolution;
+}
+
+// Calculate a range from sensor in a certain grid
+void Grid::calcRange()
+{
+  double distance_x = g_resolution * fabs(g_scan_size_x / 2 - index_x);
+  double distance_y = g_resolution * fabs(g_scan_size_y / 2 - index_y);
+
+  range = sqrt(distance_x * distance_x + distance_y * distance_y);
+}
+
+// Change local index into global index
+int Grid::calcGlobalIndex(const tf::StampedTransform& transform) const
+{
+  // Calculate index in global frame
+  int ix = (x + transform.getOrigin().x()) / g_resolution;
+  int iy = (y + transform.getOrigin().y()) / g_resolution;
+
+  // Make indexes positive value
+  int global_grid_x = (ix + 100000 * g_scan_size_x) % g_scan_size_x;
+  int global_grid_y = (iy + 100000 * g_scan_size_y) % g_scan_size_y;
+
+  int global_index = global_grid_x + global_grid_y * g_scan_size_x;
+
+  return global_index;
+}
+
+int calcGlobalIndex(int grid_x, int grid_y, const tf::StampedTransform& transform)
+{
+  // Point coordinate in velodyne(local) frame
+  double local_x = (grid_x - g_scan_size_x / 2.0) * g_resolution;
+  double local_y = (grid_y - g_scan_size_y / 2.0) * g_resolution;
+
+  // Calculate index in global coordinate
+  int ix = (local_x + transform.getOrigin().x()) / g_resolution;
+  int iy = (local_y + transform.getOrigin().y()) / g_resolution;
+
+  // Make indexes positive value
+  int global_grid_x = (ix + 100000 * g_scan_size_x) % g_scan_size_x;
+  int global_grid_y = (iy + 100000 * g_scan_size_y) % g_scan_size_y;
+
+  int global_index = global_grid_x + global_grid_y * g_scan_size_x;
+
+  return global_index;
+}
+
+void preCasting(const sensor_msgs::LaserScan& scan, std::vector<std::vector<Grid>>* precasted_grids)
+{
+  int iangle_size = 2 * M_PI / scan.angle_increment;
+  // We decide if a grid is passed by laser by this search_step
+  double search_step = g_resolution / 10.0;
+
+  // Grid indexes for each laser
+  std::vector<std::vector<int>> grid_indexes(iangle_size);
+
+  for (int iangle = 0; iangle < iangle_size; iangle++)
+  {
+    // Angle of each laser from Lidar
+    double angle = scan.angle_increment * iangle;  // + scan.angle_min;
+    double step = 0;
+
+    while (1)
+    {
+      step += search_step;
+
+      // Calculate a grid index passed by laser scan
+      int grid_x = step * cos(angle) / g_resolution + g_scan_size_x / 2.0;
+      int grid_y = step * sin(angle) / g_resolution + g_scan_size_y / 2.0;
+
+      if (grid_x >= g_scan_size_x || grid_x < 0 || grid_y >= g_scan_size_y || grid_y < 0)
+        break;
+
+      grid_indexes[iangle].push_back(grid_x + grid_y * g_scan_size_x);
+    }
+  }
+
+  // Erase overlapping values
+  auto unique_grid_indexes = grid_indexes;
+  for (auto& vec : unique_grid_indexes)
+    vec.erase(std::unique(vec.begin(), vec.end()), vec.end());
+
+  // Max size of step counts
+  // Precated laser steps for each grid are less than this value
+  // int max_count = g_resolution / search_step * sqrt(2) + 1;
+
+  for (int i = 0; i < iangle_size; i++)
+  {
+    // Push grid indexes for each laser
+    // Each index has weight which is decided by passing laser rate
+    for (const auto& unique_index : unique_grid_indexes[i])
+    {
+      Grid g;
+      g.index = unique_index;
+      // g.weight = 1.0 * std::count(grid_indexes[i].begin(), grid_indexes[i].end(), unique_index) / max_count;
+      g.calcCoordinate();
+      g.calcRange();
+      precasted_grids->at(i).push_back(g);
+    }
+  }
+}
+
+
+// Delete old cost values
+void deleteOldData(std::vector<Cost>* cost_map, double current_x, double current_y, double prev_x, double prev_y)
+{
+  double begin_x = current_x;
+  double begin_y = current_y;
+  double end_x = prev_x;
+  double end_y = prev_y;
+
+  if (begin_x > end_x)
+    std::swap(begin_x, end_x);
+  if (begin_y > end_y)
+    std::swap(begin_y, end_y);
+
+  int ibegin_x = begin_x / g_resolution + g_scan_size_x / 2;
+  int ibegin_y = begin_y / g_resolution + g_scan_size_y / 2;
+  int iend_x = end_x / g_resolution + g_scan_size_x / 2;
+  int iend_y = end_y / g_resolution + g_scan_size_y / 2;
+
+  for (int i = ibegin_x; i < iend_x; i++)
+  {
+    for (int j = 0; j < g_scan_size_y; j++)
+    {
+      int global_grid_x = (i + 100000 * g_scan_size_x) % g_scan_size_x;
+      cost_map->at(global_grid_x + j * g_scan_size_x).occupied = 0;
+      cost_map->at(global_grid_x + j * g_scan_size_x).free     = 0;
+      cost_map->at(global_grid_x + j * g_scan_size_x).unknown  = true;
+    }
+  }
+
+  for (int i = 0; i < g_scan_size_x; i++)
+  {
+    for (int j = ibegin_y; j < iend_y; j++)
+    {
+      int global_grid_y = (j + 100000 * g_scan_size_y) % g_scan_size_y;
+      cost_map->at(i + global_grid_y * g_scan_size_x).occupied = 0;
+      cost_map->at(i + global_grid_y * g_scan_size_x).free     = 0;
+      cost_map->at(i + global_grid_y * g_scan_size_x).unknown  = true;
+    }
+  }
+}
+
+void setOccupancyGridMap(nav_msgs::OccupancyGrid* map, const std_msgs::Header& header,
+                         const tf::StampedTransform& transform)
+{
+  map->header.stamp = header.stamp;
+  map->header.frame_id = OGM_FRAME;
+  map->info.map_load_time = header.stamp;
+  map->info.resolution = g_resolution;
+  map->info.height = g_map_size_y;
+  map->info.width = g_map_size_x;
+  map->info.origin.position.x = transform.getOrigin().x() - (g_map_size_x / 2) * g_resolution;
+  map->info.origin.position.y = transform.getOrigin().y() - (g_map_size_y / 2) * g_resolution;
+  map->info.origin.position.z = transform.getOrigin().z() - 5;
+  map->info.origin.orientation.x = 0;
+  map->info.origin.orientation.y = 0;
+  map->info.origin.orientation.z = 0;
+  map->info.origin.orientation.w = 1;
+}
+
+void createCostMap(const sensor_msgs::LaserScan& scan, const std::vector<std::vector<Grid>>& precasted_grids)
+{
+  tf::StampedTransform transform;
+
+  try
+  {
+    // What time should we use?
+    g_tf_listenerp->lookupTransform(OGM_FRAME, g_sensor_frame, ros::Time(0), transform);
+  }
+  catch (tf::TransformException ex)
+  {
+    ROS_ERROR("%s", ex.what());
+    return;
+  }
+
+  // Save costs in this variable
+  static std::vector<Cost> cost_map(g_scan_size_x * g_scan_size_y);
+
+  static bool initialized_map = false;
+  static double prev_x = transform.getOrigin().x();
+  static double prev_y = transform.getOrigin().y();
+  static nav_msgs::OccupancyGrid map;
+  setOccupancyGridMap(&map, scan.header, transform);
+  if (!initialized_map)
+  {
+    map.data.resize(g_map_size_x * g_map_size_y, -1);
+    initialized_map = true;
+  }
+
+  // Since we implement as ring buffer, we have to delete old data regularly
+  deleteOldData(&cost_map, transform.getOrigin().x(), transform.getOrigin().y(), prev_x, prev_y);
+  prev_x = transform.getOrigin().x();
+  prev_y = transform.getOrigin().y();
+
+  // Vehicle's orientation
+  double yaw = calcYawFromQuaternion(transform.getRotation());
+
+  // Original yaw is -PI ~ PI, so make its range 0 ~ 2PI
+  if (yaw < 0)
+    yaw += 2 * M_PI;
+  static double laser_offset = fabs(scan.angle_min);
+
+  // For tough_urg
+  // static double manual_offset = -10.0 / 180 * M_PI + M_PI;
+  // int index_offset = (yaw + laser_offset + manual_offset) / scan.angle_increment;
+
+  int index_offset = (yaw + laser_offset) / scan.angle_increment;
+  static int iangle_size = 2 * M_PI / scan.angle_increment;
+
+  //----------------- RING OCCUPANCY GRID MAPPING --------------
+  // Accumulate grid costs for each laser scan
+  for (size_t i = 0; i < scan.ranges.size(); i++)
+  {
+    double range = scan.ranges[i];
+
+    // Ignore 0 ranges
+    if (range == 0)
+      continue;
+
+    // If laserscan does not reach objects, make a range max
+    if (scan.ranges[i] > scan.range_max)
+      range = scan.range_max;
+
+    int precasted_index = (i + index_offset) % iangle_size;
+    int obstacle_index = -1;
+    for (const auto& g : precasted_grids[precasted_index])
+    {
+      obstacle_index++;
+
+      if (g.range > range)
+        break;
+
+      // Free range
+      int global_index = g.calcGlobalIndex(transform);
+      cost_map[global_index].accumulateCost(0, FREE_INCREMENT);
+    }
+
+    // Obstacle
+    int global_index = precasted_grids[precasted_index][obstacle_index].calcGlobalIndex(transform);
+    cost_map[global_index].accumulateCost(OCCUPIED_INCREMENT, 0);
+
+  }
+
+
+  // Begining of grid index of publishing OGM
+  static int origin_index_x = (g_scan_size_x - g_map_size_x) / 2.0;
+  static int origin_index_y = (g_scan_size_y - g_map_size_y) / 2.0;
+  // static int origin_index = origin_index_x + origin_index_y * g_scan_size_x;
+
+  // Set cost values for publishing OccuppancyGridMap
+  for (int i = 0; i < g_map_size_y; i++)
+  {
+    for (int j = 0; j < g_map_size_x; j++)
+    {
+      int scanmap_index_x = origin_index_x + j;
+      int scanmap_index_y = origin_index_y + i;
+
+      int global_index = calcGlobalIndex(scanmap_index_x, scanmap_index_y, transform);
+      if (cost_map[global_index].unknown)
+        map.data[j + i * g_map_size_x] = -1;
+      else
+        map.data[j + i * g_map_size_x] = (cost_map[global_index].occupied + 8) * 6;
+    }
+  }
+
+  g_map_pub.publish(map);
+}
+
+// Make CostMap from LaserScan message
+void laserScanCallback(const sensor_msgs::LaserScanConstPtr& msg)
+{
+  static bool precasted = false;
+  static int iangle_size = 2 * M_PI / msg->angle_increment;
+  static std::vector<std::vector<Grid>> precasted_grids(iangle_size);
+
+  if (!precasted)
+  {
+    preCasting(*msg, &precasted_grids);
+    precasted = true;
+  }
+
+  // Create costmap and publish
+  createCostMap(*msg, precasted_grids);
+
+  return;
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "laserscan2costmap");
+
+  tf::TransformListener tf_listener;
+  g_tf_listenerp = &tf_listener;
+
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  private_nh.param<double>("resolution", g_resolution, 0.1);
+  private_nh.param<int>("scan_size_x", g_scan_size_x, 1000);
+  private_nh.param<int>("scan_size_y", g_scan_size_y, 1000);
+  private_nh.param<int>("map_size_x", g_map_size_x, 500);
+  private_nh.param<int>("map_size_y", g_map_size_y, 500);
+  private_nh.param<std::string>("scan_topic", g_scan_topic, "/scan");
+  private_nh.param<std::string>("sensor_frame", g_sensor_frame, "/velodyne");
+
+  ros::Subscriber laserscan_sub = nh.subscribe(g_scan_topic, 1, laserScanCallback);
+
+  g_map_pub = nh.advertise<nav_msgs::OccupancyGrid>("/ring_ogm", 1);
+
+  ros::spin();
+
+  return 0;
+}

src/ros/catkin/src/object_map/nodes/potential_field/potential_field.cpp

@@ -0,0 +1,340 @@
+#include "tf/transform_listener.h"
+#include <cmath>
+#include <geometry_msgs/PointStamped.h>
+#include <grid_map_msgs/GridMap.h>
+#include <grid_map_ros/grid_map_ros.hpp>
+#include <iostream>
+#include <jsk_recognition_msgs/BoundingBox.h>
+#include <jsk_recognition_msgs/BoundingBoxArray.h>
+#include "autoware_msgs/DetectedObject.h"
+#include "autoware_msgs/DetectedObjectArray.h"
+#include <pcl_conversions/pcl_conversions.h>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+using namespace grid_map;
+
+class PotentialField {
+private:
+  ros::NodeHandle nh_;
+  ros::Publisher publisher_;
+  ros::Subscriber waypoint_subscriber_;
+  ros::Subscriber vscan_subscriber_;
+  ros::Subscriber obj_subscriber_;
+  bool use_target_waypoint_;
+  bool use_obstacle_box_;
+  bool use_vscan_points_;
+  double map_x_size_;
+  double map_y_size_;
+  double map_resolution_;
+  double tf_x_;
+  double tf_z_;
+  double map_x_offset_;
+  GridMap map_;
+  class ObstacleFieldParameter {
+  public:
+    ObstacleFieldParameter() : ver_x_p(0.9), ver_y_p(0.9) {}
+    double ver_x_p;
+    double ver_y_p;
+  };
+  class TargetWaypointFieldParamater {
+  public:
+    TargetWaypointFieldParamater() : ver_x_p(1.0), ver_y_p(1.0) {}
+    double ver_x_p;
+    double ver_y_p;
+  };
+  class VscanPointsFieldParamater {
+  public:
+    VscanPointsFieldParamater() : around_x(0.5), around_y(0.5) {}
+    double around_x;
+    double around_y;
+  };
+
+  void obj_callback(autoware_msgs::DetectedObjectArray::ConstPtr obj_msg);
+  void target_waypoint_callback(
+      visualization_msgs::Marker::ConstPtr target_point_msgs);
+  void vscan_points_callback(sensor_msgs::PointCloud2::ConstPtr vscan_msg);
+  void publish_potential_field();
+
+public:
+  PotentialField();
+  void run();
+  void init();
+};
+
+PotentialField::PotentialField()
+    : tf_x_(1.2), tf_z_(2.0),
+      map_({"potential_field", "obstacle_field", "target_waypoint_field",
+            "vscan_points_field"}) {
+  ros::NodeHandle private_nh("~");
+  if (!private_nh.getParam("use_obstacle_box", use_obstacle_box_)) {
+    ROS_INFO("use obstacle_box");
+    use_obstacle_box_ = true;
+  }
+  if (!private_nh.getParam("use_vscan_points", use_vscan_points_)) {
+    ROS_INFO("use vscan points");
+    use_vscan_points_ = true;
+  }
+  if (!private_nh.getParam("use_target_waypoint", use_target_waypoint_)) {
+    ROS_INFO("don't use target_waypoint");
+    use_target_waypoint_ = false;
+  }
+  if (!private_nh.getParam("map_resolution", map_resolution_)) {
+    map_resolution_ = 0.25;
+    ROS_INFO("map resolution %f", map_resolution_);
+  }
+  if (!private_nh.getParam("map_x_size", map_x_size_)) {
+    map_x_size_ = 40.0;
+    ROS_INFO("map x size %f", map_x_size_);
+  }
+  if (!private_nh.getParam("map_y_size", map_y_size_)) {
+    map_y_size_ = 25.0;
+    ROS_INFO("map y size %f", map_y_size_);
+  }
+  if (!private_nh.getParam("map_x_offset", map_x_offset_)) {
+    map_x_offset_ = 10.0;
+    ROS_INFO("map x offset %f", map_x_offset_);
+  }
+  publisher_ =
+      nh_.advertise<grid_map_msgs::GridMap>("/potential_field", 1, true);
+
+  if (use_obstacle_box_)
+    obj_subscriber_ = nh_.subscribe("/detected_objects", 1,
+                                    &PotentialField::obj_callback, this);
+  if (use_vscan_points_)
+    vscan_subscriber_ = nh_.subscribe(
+        "/vscan_points", 1, &PotentialField::vscan_points_callback, this);
+  if (use_target_waypoint_)
+    waypoint_subscriber_ =
+        nh_.subscribe("/next_target_mark", 1,
+                      &PotentialField::target_waypoint_callback, this);
+}
+
+void PotentialField::init() {
+  ROS_INFO("Created map");
+  map_.setFrameId("/potential_field_link");
+  map_.setGeometry(Length(map_x_size_, map_y_size_), map_resolution_);
+  for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
+    Position position;
+    map_.getPosition(*it, position);
+    map_.at("obstacle_field", *it) = 0.0;
+    map_.at("target_waypoint_field", *it) = 0.0;
+    map_.at("vscan_points_field", *it) = 0.0;
+    map_.at("potential_field", *it) = 0.0;
+  }
+  ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
+           map_.getLength().x(), map_.getLength().y(), map_.getSize()(0),
+           map_.getSize()(1));
+}
+void PotentialField::run() { ros::spin(); }
+
+void PotentialField::publish_potential_field() {
+  grid_map_msgs::GridMap message;
+
+  map_["potential_field"] =
+      map_["obstacle_field"].cwiseMax(map_["vscan_points_field"]) +
+      map_["target_waypoint_field"];
+  GridMapRosConverter::toMessage(map_, message);
+  publisher_.publish(message);
+  ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.",
+                    message.info.header.stamp.toSec());
+}
+void PotentialField::obj_callback(
+    autoware_msgs::DetectedObjectArray::ConstPtr obj_msg) { // Create grid map.
+  static ObstacleFieldParameter param;
+  double ver_x_p(param.ver_x_p);
+  double ver_y_p(param.ver_y_p);
+
+  // Add data to grid map.
+  ros::Time time = ros::Time::now();
+
+  for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
+
+    Position position;
+    map_.getPosition(*it, position);
+    map_.at("obstacle_field", *it) = 0.0;
+    for (int i(0); i < (int)obj_msg->objects.size(); ++i) {
+      double pos_x =
+          obj_msg->objects.at(i).pose.position.x + tf_x_ - map_x_offset_;
+      double pos_y = obj_msg->objects.at(i).pose.position.y;
+      double len_x = obj_msg->objects.at(i).dimensions.x / 2.0;
+      double len_y = obj_msg->objects.at(i).dimensions.y / 2.0;
+
+      double r, p, y;
+      tf::Quaternion quat(obj_msg->objects.at(i).pose.orientation.x,
+                          obj_msg->objects.at(i).pose.orientation.y,
+                          obj_msg->objects.at(i).pose.orientation.z,
+                          obj_msg->objects.at(i).pose.orientation.w);
+      tf::Matrix3x3(quat).getRPY(r, p, y);
+
+      double rotated_pos_x = std::cos(-1.0 * y) * (position.x() - pos_x) -
+                             std::sin(-1.0 * y) * (position.y() - pos_y) +
+                             pos_x;
+      double rotated_pos_y = std::sin(-1.0 * y) * (position.x() - pos_x) +
+                             std::cos(-1.0 * y) * (position.y() - pos_y) +
+                             pos_y;
+
+      if (-0.5 < pos_x && pos_x < 4.0) {
+        if (-1.0 < pos_y && pos_y < 1.0)
+          continue;
+      }
+      if (pos_x - len_x < rotated_pos_x && rotated_pos_x < pos_x + len_x) {
+        if (pos_y - len_y < rotated_pos_y && rotated_pos_y < pos_y + len_y) {
+          map_.at("obstacle_field", *it) =
+              std::max(std::exp(0.0),
+                       static_cast<double>(map_.at("obstacle_field", *it)));
+        } else if (rotated_pos_y < pos_y - len_y) {
+          map_.at("obstacle_field", *it) = std::max(
+              std::exp(
+                  (-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) /
+                           std::pow(2.0 * ver_y_p, 2.0)))),
+              static_cast<double>(map_.at("obstacle_field", *it)));
+        } else if (pos_y + len_y < rotated_pos_y) {
+          map_.at("obstacle_field", *it) = std::max(
+              std::exp(
+                  (-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) /
+                           std::pow(2.0 * ver_y_p, 2.0)))),
+              static_cast<double>(map_.at("obstacle_field", *it)));
+        }
+      } else if (rotated_pos_x < pos_x - len_x) {
+        if (rotated_pos_y < pos_y - len_y) {
+          map_.at("obstacle_field", *it) = std::max(
+              std::exp(
+                  (-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) /
+                           std::pow(2.0 * ver_y_p, 2.0))) +
+                  (-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) /
+                           std::pow(2.0 * ver_x_p, 2.0)))),
+              static_cast<double>(map_.at("obstacle_field", *it)));
+        } else if (pos_y + len_y < rotated_pos_y) {
+          map_.at("obstacle_field", *it) = std::max(
+              std::exp(
+                  (-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) /
+                           std::pow(2.0 * ver_y_p, 2.0))) +
+                  (-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) /
+                           std::pow(2.0 * ver_x_p, 2.0)))),
+              static_cast<double>(map_.at("obstacle_field", *it)));
+        } else if (pos_y - len_y < rotated_pos_y &&
+                   rotated_pos_y < pos_y + len_y) {
+          map_.at("obstacle_field", *it) = std::max(
+              std::exp(
+                  (-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) /
+                           std::pow(2.0 * ver_x_p, 2.0)))),
+              static_cast<double>(map_.at("obstacle_field", *it)));
+        }
+      } else if (pos_x + len_x < rotated_pos_x) {
+        if (rotated_pos_y < pos_y - len_y) {
+          map_.at("obstacle_field", *it) = std::max(
+              std::exp(
+                  (-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) /
+                           std::pow(2.0 * ver_y_p, 2.0))) +
+                  (-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) /
+                           std::pow(2.0 * ver_x_p, 2.0)))),
+              static_cast<double>(map_.at("obstacle_field", *it)));
+        } else if (pos_y + len_y / 2.0 < rotated_pos_y) {
+          map_.at("obstacle_field", *it) = std::max(
+              std::exp(
+                  (-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) /
+                           std::pow(2.0 * ver_y_p, 2.0))) +
+                  (-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) /
+                           std::pow(2.0 * ver_x_p, 2.0)))),
+              static_cast<double>(map_.at("obstacle_field", *it)));
+        } else if (pos_y - len_y < rotated_pos_y &&
+                   rotated_pos_y < pos_y + len_y) {
+          map_.at("obstacle_field", *it) = std::max(
+              std::exp(
+                  (-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) /
+                           std::pow(2.0 * ver_x_p, 2.0)))),
+              static_cast<double>(map_.at("obstacle_field", *it)));
+        }
+      }
+    }
+  }
+  // Publish grid map.
+  map_.setTimestamp(time.toNSec());
+  publish_potential_field();
+}
+
+void PotentialField::target_waypoint_callback(
+    visualization_msgs::Marker::ConstPtr target_point_msgs) {
+  static TargetWaypointFieldParamater param;
+  double ver_x_p(param.ver_x_p);
+  double ver_y_p(param.ver_y_p);
+  ros::Time time = ros::Time::now();
+  geometry_msgs::PointStamped in, out;
+  in.header = target_point_msgs->header;
+  in.point = target_point_msgs->pose.position;
+  tf::TransformListener tflistener;
+  try {
+    ros::Time now = ros::Time(0);
+    tflistener.waitForTransform("/map", "/potential_field_link", now,
+                                ros::Duration(10.0));
+    tflistener.transformPoint("/potential_field_link", in.header.stamp, in,
+                              "/map", out);
+
+  } catch (tf::TransformException &ex) {
+    ROS_ERROR("%s", ex.what());
+  }
+
+  for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
+    Position position;
+    map_.getPosition(*it, position);
+    map_.at("target_waypoint_field", *it) = 0.0;
+    map_.at("target_waypoint_field", *it) -=
+        0.5 * std::exp((-1.0 * (std::pow((position.y() - (out.point.y)), 2.0) /
+                                std::pow(2.0 * ver_y_p, 2.0))) +
+                       (-1.0 * (std::pow((position.x() - (out.point.x)), 2.0) /
+                                std::pow(2.0 * ver_x_p, 2.0))));
+  }
+  map_.setTimestamp(time.toNSec());
+}
+
+void PotentialField::vscan_points_callback(
+    sensor_msgs::PointCloud2::ConstPtr vscan_msg) {
+  static VscanPointsFieldParamater param;
+  double around_x(param.around_x);
+  double around_y(param.around_y);
+
+  ros::Time time = ros::Time::now();
+  pcl::PointCloud<pcl::PointXYZ> pcl_vscan;
+  pcl::fromROSMsg(*vscan_msg, pcl_vscan);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_vscan_ptr(
+      new pcl::PointCloud<pcl::PointXYZ>(pcl_vscan));
+
+  double length_x = map_.getLength().x() / 2.0;
+  double length_y = map_.getLength().y() / 2.0;
+
+  for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
+    Position position;
+    map_.getPosition(*it, position);
+    map_.at("vscan_points_field", *it) = 0.0;
+    for (int i(0); i < (int)pcl_vscan.size(); ++i) {
+      double point_x = pcl_vscan.at(i).x - map_x_offset_;
+      if (3.0 < pcl_vscan.at(i).z + tf_z_ || pcl_vscan.at(i).z + tf_z_ < 0.3)
+        continue;
+      if (length_x < point_x && point_x < -1.0 * length_x)
+        continue;
+      if (length_y < pcl_vscan.at(i).y && pcl_vscan.at(i).y < -1.0 * length_y)
+        continue;
+      if ((point_x + tf_x_) - around_x < position.x() &&
+          position.x() < point_x + tf_x_ + around_x) {
+        if (pcl_vscan.at(i).y - around_y < position.y() &&
+            position.y() < pcl_vscan.at(i).y + around_y) {
+          map_.at("vscan_points_field", *it) = 1.0; // std::exp(0.0) ;
+        }
+      }
+    }
+  }
+  map_.setTimestamp(time.toNSec());
+  publish_potential_field();
+}
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "potential_field");
+
+  PotentialField potential_field;
+  potential_field.init();
+  potential_field.run();
+  return 0;
+}

src/ros/catkin/src/object_map/nodes/wayarea2grid/wayarea2grid.cpp

@@ -0,0 +1,83 @@
+/*
+ * 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 "wayarea2grid.h"
+
+namespace object_map
+{
+
+  WayareaToGrid::WayareaToGrid() :
+      private_node_handle_("~")
+  {
+    InitializeROSIo();
+    LoadRoadAreasFromVectorMap(private_node_handle_, area_points_);
+  }
+
+
+  void WayareaToGrid::InitializeROSIo()
+  {
+    private_node_handle_.param<std::string>("sensor_frame", sensor_frame_, "velodyne");
+    private_node_handle_.param<std::string>("map_frame", map_frame_, "map");
+    private_node_handle_.param<double>("grid_resolution", grid_resolution_, 0.2);
+    private_node_handle_.param<double>("grid_length_x", grid_length_x_, 80);
+    private_node_handle_.param<double>("grid_length_y", grid_length_y_, 30);
+    private_node_handle_.param<double>("grid_position_x", grid_position_x_, 20);
+    private_node_handle_.param<double>("grid_position_y", grid_position_y_, 0);
+    private_node_handle_.param<double>("grid_position_z", grid_position_z_, -2.f);
+
+    publisher_grid_map_ = node_handle_.advertise<grid_map_msgs::GridMap>("grid_map_wayarea", 1, true);
+    publisher_occupancy_ = node_handle_.advertise<nav_msgs::OccupancyGrid>("occupancy_wayarea", 1, true);
+  }
+
+  void WayareaToGrid::Run()
+  {
+    bool set_map = false;
+    ros::Rate loop_rate(10);
+
+    while (ros::ok())
+    {
+      if (!set_map)
+      {
+        gridmap_.add(grid_layer_name_);
+        gridmap_.setFrameId(sensor_frame_);
+        gridmap_.setGeometry(grid_map::Length(grid_length_x_, grid_length_y_),
+                        grid_resolution_,
+                        grid_map::Position(grid_position_x_, grid_position_y_));
+        set_map = true;
+      }
+
+      // timer start
+      //auto start = std::chrono::system_clock::now();
+
+      if (!area_points_.empty())
+      {
+        FillPolygonAreas(gridmap_, area_points_, grid_layer_name_, OCCUPANCY_NO_ROAD, OCCUPANCY_ROAD, grid_min_value_,
+                         grid_max_value_, sensor_frame_, map_frame_,
+                         tf_listener_);
+        PublishGridMap(gridmap_, publisher_grid_map_);
+        PublishOccupancyGrid(gridmap_, publisher_occupancy_, grid_layer_name_, grid_min_value_, grid_max_value_, grid_position_z_);
+      }
+
+      // timer end
+      //auto end = std::chrono::system_clock::now();
+      //auto usec = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
+      //std::cout << "time: " << usec / 1000.0 << " [msec]" << std::endl;
+
+      loop_rate.sleep();
+    }
+  }
+
+}  // namespace object_map

src/ros/catkin/src/object_map/nodes/wayarea2grid/wayarea2grid.h

@@ -0,0 +1,91 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ ********************
+ */
+#ifndef WAYAREA_TO_GRID_H
+#define WAYAREA_TO_GRID_H
+
+#include <iostream>
+#include <vector>
+#include <string>
+#include <chrono>
+
+#include <ros/ros.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <tf/transform_listener.h>
+
+#include <vector_map/vector_map.h>
+
+#include <grid_map_ros/grid_map_ros.hpp>
+#include <grid_map_msgs/GridMap.h>
+#include <grid_map_cv/grid_map_cv.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "object_map/object_map_utils.hpp"
+
+namespace object_map
+{
+
+  class WayareaToGrid
+  {
+  public:
+    WayareaToGrid();
+
+    void Run();
+
+  private:
+    // handle
+    ros::NodeHandle         node_handle_;
+    ros::NodeHandle         private_node_handle_;
+
+    ros::Publisher          publisher_grid_map_;
+    ros::Publisher          publisher_occupancy_;
+
+    grid_map::GridMap       gridmap_;
+
+    std::string             sensor_frame_;
+    std::string             map_frame_;
+
+    const std::string       grid_layer_name_ = "wayarea";
+
+    double                  grid_resolution_;
+    double                  grid_length_x_;
+    double                  grid_length_y_;
+    double                  grid_position_x_;
+    double                  grid_position_y_;
+    double                  grid_position_z_;
+
+    tf::TransformListener   tf_listener_;
+
+    int                     OCCUPANCY_ROAD      = 128;
+    int                     OCCUPANCY_NO_ROAD   = 255;
+    const int               grid_min_value_     = 0;
+    const int               grid_max_value_     = 255;
+
+    std::vector<std::vector<geometry_msgs::Point>> area_points_;
+
+    /*!
+     * Initializes ROS Publisher, Subscribers and sets the configuration parameters
+     */
+    void InitializeROSIo();
+
+
+  };
+
+}  // namespace object_map
+
+#endif  // WAYAREA_TO_GRID

src/ros/catkin/src/object_map/nodes/wayarea2grid/wayarea2grid_node.cpp

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

src/ros/catkin/src/object_map/nodes/wayarea2grid_lanelet2/wayarea2grid_lanelet2.cpp

@@ -0,0 +1,114 @@
+/*
+ * 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 <wayarea2grid_lanelet2/wayarea2grid_lanelet2.h>
+
+#include <string>
+#include <vector>
+
+namespace object_map
+{
+WayareaToGridLanelet2::WayareaToGridLanelet2() : private_node_handle_("~")
+{
+  InitializeROSIo();
+  initAreaPointsFromLaneletMap();
+}
+
+void WayareaToGridLanelet2::initAreaPointsFromLaneletMap()
+{
+  ros::Subscriber sub_lanelet_bin_map =
+      node_handle_.subscribe("lanelet_map_bin", 1, &WayareaToGridLanelet2::laneletBinMapCallback, this);
+
+  while (ros::ok() && !loaded_lanelet_map_)
+  {
+    ros::spinOnce();
+    ros::Duration(0.1).sleep();
+  }
+
+  // use all lanelets in map of subtype road to give way area
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_);
+  lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets);
+
+  // convert lanelets to polygons and put into area_points array
+  for (const auto& ll : road_lanelets)
+  {
+    std::vector<geometry_msgs::Polygon> triangles;
+    lanelet::visualization::lanelet2Triangle(ll, &triangles);
+
+    for (const auto& triangle : triangles)
+    {
+      std::vector<geometry_msgs::Point> poly_pts;
+      for (const auto& p : triangle.points)
+      {
+        // convert from Point32 to Point
+        geometry_msgs::Point gp;
+        gp.x = p.x;
+        gp.y = p.y;
+        gp.z = p.z;
+        poly_pts.push_back(gp);
+      }
+      area_points_.push_back(poly_pts);
+    }
+  }
+}
+
+void WayareaToGridLanelet2::InitializeROSIo()
+{
+  private_node_handle_.param<std::string>("sensor_frame", sensor_frame_, "velodyne");
+  private_node_handle_.param<std::string>("grid_frame_", grid_frame_, "map");
+  private_node_handle_.param<double>("grid_resolution", grid_resolution_, 0.2);
+  private_node_handle_.param<double>("grid_length_x", grid_length_x_, 80);
+  private_node_handle_.param<double>("grid_length_y", grid_length_y_, 30);
+  private_node_handle_.param<double>("grid_position_x", grid_position_x_, 20);
+  private_node_handle_.param<double>("grid_position_y", grid_position_y_, 0);
+  private_node_handle_.param<double>("grid_position_z", grid_position_z_, -2.f);
+
+  publisher_grid_map_ = node_handle_.advertise<grid_map_msgs::GridMap>("grid_map_wayarea", 1, true);
+  publisher_occupancy_ = node_handle_.advertise<nav_msgs::OccupancyGrid>("occupancy_wayarea", 1, true);
+}
+
+void WayareaToGridLanelet2::laneletBinMapCallback(const autoware_lanelet2_msgs::MapBin& msg)
+{
+  lanelet_map_ = std::make_shared<lanelet::LaneletMap>();
+  lanelet::utils::conversion::fromBinMsg(msg, lanelet_map_);
+  loaded_lanelet_map_ = true;
+}
+
+void WayareaToGridLanelet2::Run()
+{
+  ros::Rate loop_rate(10);
+
+  gridmap_.add(grid_layer_name_);
+  gridmap_.setFrameId(sensor_frame_);
+  gridmap_.setGeometry(grid_map::Length(grid_length_x_, grid_length_y_), grid_resolution_,
+                       grid_map::Position(grid_position_x_, grid_position_y_));
+
+  while (ros::ok())
+  {
+    if (!area_points_.empty())
+    {
+      FillPolygonAreas(gridmap_, area_points_, grid_layer_name_, occupancy_no_road, occupancy_road, grid_min_value_,
+                       grid_max_value_, sensor_frame_, grid_frame_, tf_listener_);
+      PublishGridMap(gridmap_, publisher_grid_map_);
+      PublishOccupancyGrid(gridmap_, publisher_occupancy_, grid_layer_name_, grid_min_value_, grid_max_value_,
+                           grid_position_z_);
+    }
+
+    loop_rate.sleep();
+  }
+}
+
+}  // namespace object_map

src/ros/catkin/src/object_map/nodes/wayarea2grid_lanelet2/wayarea2grid_lanelet2_node.cpp

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

src/ros/catkin/src/object_map/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>object_map</name>
+  <version>1.12.0</version>
+  <description>The object_map package</description>
+  <maintainer email="antm678@ertl.jp">ando</maintainer>
+  <maintainer email="abrahammonrroy@yahoo.com">amc</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>autoware_msgs</depend>
+  <depend>grid_map_cv</depend>
+  <depend>grid_map_msgs</depend>
+  <depend>grid_map_ros</depend>
+  <depend>nav_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf</depend>
+  <depend>vector_map</depend>
+  <depend>lanelet2_extension</depend>
+</package>