Skip to content

Commit

Permalink
feat: add costmap_generator package (autowarefoundation#32)
Browse files Browse the repository at this point in the history 
* release v0.4.0

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Revert "remove ROS1 packages temporarily"

This reverts commit d3dea73de174d06d8c3c97500db37501c957f521.

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Rename launch files to launch.xml (autowarefoundation#28)

* Port costmap generator (autowarefoundation#93)

* ported CMakeLists.txt and package.xml to ROS2.

dependency issues on grid_map_ros and grid_map_cv yet to be resolved.

* working on porting changes in the code

* porting in progress

* porting in progress

* fix subscription errors

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* porting costmap_generator from ROS1 to ROS2 completed

* edited CMakeLists.txt and package.xml to fix compile and dependency
errors, replaced adding external dependencies directly to adding them
via rosdep.

* fix parameter declarations

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix TFs

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix launch files

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* delete unnecessary files

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Convert calls of Duration to Duration::from_seconds where appropriate (autowarefoundation#131)

* Rename h files to hpp (autowarefoundation#142)

* Change includes

* Rename files

* Adjustments to make things compile

* Other packages

* Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143)

* Use quotes for includes where appropriate (autowarefoundation#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

* Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151)

* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs

* fixing trasient_local in ROS2 packages (autowarefoundation#160)

* Enable lints in costmap_generator (autowarefoundation#149)

* [costmap_generator] set transient_local() option for map subscriber (autowarefoundation#217)

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* reduce terminal ouput for better error message visibility (autowarefoundation#200)

* reduce terminal ouput for better error message visibility

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* [costmap_generator] fix waiting for first transform

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix tests

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix test

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Fix typos in planning modules (autowarefoundation#866) (autowarefoundation#275)

* fix typos in planning

* fix corresponding typos in planning

* revert fixed typos temporarily due to its impact on launchers

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* Sensor data qos (autowarefoundation#407)

* Use sensor data qos for pointcloud preprocessor

Signed-off-by: Autoware <autoware@tier4.jp>

* Use sensor data qos for pointcloud

Signed-off-by: Autoware <autoware@tier4.jp>

* Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Use sensor data qos for livox tag filter and vector map filter

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Autoware <autoware@tier4.jp>

* Fix transform (autowarefoundation#420)

* Replace rclcpp::Time(0) by tf2::TimePointZero() in lookupTransform

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix canTransform

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix test

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ros2 fix topic name part1 (autowarefoundation#408)

* Fix topic name of lane_departure_checker debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of mpc_follower debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of velocity_controller debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of motion_velocity_optimizer debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of lane_change_planner debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of behavior_velocity_planner debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of obstacle_avoidance_planner debug

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of behavior_velocity_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of motion_velocity_optimizer

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of lane_departure_checker

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of mpc_follower

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of behavior_velocity_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of velocity_controller

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of lane_change_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of obstacle_avoidance_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of obstacle_stop_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of costmap_generator

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of freespace_planner

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of surround_obstacle_checker

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of costmap_generator

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of emergency_handler

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint errors

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix typo

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* add use_sim-time option (autowarefoundation#454)

* Unify Apache-2.0 license name (autowarefoundation#1242)

* Refine BSD license name (autowarefoundation#1244)

* Make planning modules components (autowarefoundation#1263)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix/fix costmap generator bugs ros2 (autowarefoundation#1358)

* Fix/fix costmap generator bugs (autowarefoundation#1276)

* Apply format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Transform pointcloud before generating costmap

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix makeExpandedPoint

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Use tf2 buffer core API

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Fix wrong rate in freespace_planner (autowarefoundation#1564)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add markdownlint and prettier (autowarefoundation#1661)

* Add markdownlint and prettier

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore .param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* add sort-package-xml hook in pre-commit (autowarefoundation#1881)

* add sort xml hook in pre-commit

* change retval to exit_status

* rename

* add prettier plugin-xml

* use early return

* add license note

* add tier4 license

* restore prettier

* change license order

* move local hooks to public repo

* move prettier-xml to pre-commit-hooks-ros

* update version for bug-fix

* apply pre-commit

* Change formatter to clang-format and black (autowarefoundation#2332)

* Revert "Temporarily comment out pre-commit hooks"

This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3.

* Replace ament_lint_common with autoware_lint_common

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply Black

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Port parking planner packages from .Auto (autowarefoundation#600)

* Copy code of 'vehicle_constants_manager'

* Fix vehicle_constants_manager for ROS galactic

* Rm .iv costmap_generator freespace_planner freespace_planning_aglorihtms

* Add astar_search (from .Auto)

* Copy freespace_planner from .Auto

* Update freespace_planner for .IV

* Copy costmap_generator from .Auto

* Copy and update had_map_utils from .Auto

* Update costmap_generator

* Copy costmap_generator_nodes

* Update costmap_generator_nodes

* Comment out all tests

* Move vehicle_constant_managers to tmp_autoware_auto_dependencies

* ignore pre-commit for back-ported packages

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* ignore testing

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Port parking modules (autowarefoundation#738)

* Port costmap_generator

* Port freespace_planner

* fix readme

* ci(pre-commit): autofix

* fix readme

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Autoware <autoware@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@tkimura4 @mitsudome-r
@nnmm @nik-tier4 @TakaHoribe @kmiya @wep21 @kenji-miyake @isamu-takagi @KeisukeShima @maxime-clem @rej55 @1222-takeshi @pre-commit-ci
16 people authored Dec 4, 2021
1 parent eb65e6f commit 1f9486e
Show file tree
Hide file tree
Showing 12 changed files with 1,624 additions and 0 deletions.
55 changes: 55 additions & 0 deletions planning/costmap_generator/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
cmake_minimum_required(VERSION 3.5)

project(costmap_generator)

if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake_auto REQUIRED)
find_package(PCL REQUIRED COMPONENTS common io)
find_package(FLANN REQUIRED)
ament_auto_find_build_dependencies()

include_directories(
include
${GRID_MAP_INCLUDE_DIR}
${PCL_COMMON_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)

ament_auto_add_library(costmap_generator_lib SHARED
nodes/costmap_generator/points_to_costmap.cpp
nodes/costmap_generator/objects_to_costmap.cpp
nodes/costmap_generator/object_map_utils.cpp
)
target_link_libraries(costmap_generator_lib
${PCL_LIBRARIES}
FLANN::FLANN
)

ament_auto_add_library(costmap_generator_node SHARED
nodes/costmap_generator/costmap_generator_node.cpp
)
target_link_libraries(costmap_generator_node
${PCL_LIBRARIES}
costmap_generator_lib
)

rclcpp_components_register_node(costmap_generator_node
PLUGIN "CostmapGenerator"
EXECUTABLE costmap_generator
)

if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package(INSTALL_TO_SHARE launch)
89 changes: 89 additions & 0 deletions planning/costmap_generator/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
# costmap_generator

## costmap_generator_node

This node reads `PointCloud` and/or `DynamicObjectArray` and creates an `OccupancyGrid` and `GridMap`. `VectorMap(Lanelet2)` is optional.

### Input topics

| Name | Type | Description |
| ------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------- |
| `~input/objects` | autoware_auto_perception_msgs::PredictedObjects | predicted objects, for obstacles areas |
| `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects |
| `~input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map, for drivable areas |
| `~input/scenario` | autoware_planning_msgs::Scenario | scenarios to be activated, for node activation |

### Output topics

| Name | Type | Description |
| ------------------------ | ----------------------- | -------------------------------------------------- |
| `~output/grid_map` | grid_map_msgs::GridMap | costmap as GridMap, values are from 0.0 to 1.0 |
| `~output/occupancy_grid` | nav_msgs::OccupancyGrid | costmap as OccupancyGrid, values are from 0 to 100 |

### Output TFs

None

### How to launch

1. Write your remapping info in `costmap_generator.launch` or add args when executing `roslaunch`
2. Run `roslaunch costmap_generator costmap_generator.launch`

### Parameters

| Name | Type | Description |
| ---------------------------- | ------ | ------------------------------------------------------- |
| `update_rate` | double | timer's update rate |
| `use_objects` | bool | whether using `~input/objects` or not |
| `use_points` | bool | whether using `~input/points_no_ground` or not |
| `use_wayarea` | bool | whether using `wayarea` from `~input/vector_map` or not |
| `costmap_frame` | string | created costmap's coordinate |
| `vehicle_frame` | string | vehicle's coordinate |
| `map_frame` | string | map's coordinate |
| `grid_min_value` | double | minimum cost for gridmap |
| `grid_max_value` | double | maximum cost for gridmap |
| `grid_resolution` | double | resolution for gridmap |
| `grid_length_x` | int | size of gridmap for x direction |
| `grid_length_y` | int | size of gridmap for y direction |
| `grid_position_x` | int | offset from coordinate in x direction |
| `grid_position_y` | int | offset from coordinate in y direction |
| `maximum_lidar_height_thres` | double | maximum height threshold for pointcloud data |
| `minimum_lidar_height_thres` | double | minimum height threshold for pointcloud data |
| `expand_rectangle_size` | double | expand object's rectangle with this value |
| `size_of_expansion_kernel` | int | kernel size for blurring effect on object's costmap |

### Flowchart

```plantuml
@startuml
title onTimer
start
if (scenario is active?) then (yes)
else (no)
stop
endif
:get current pose;
:set the center of costmap to current pose;
if (use wayarea?) then (yes)
:generate wayarea costmap;
endif
if (use objects?) then (yes)
:generate objects costmap;
endif
if (use points?) then (yes)
:generate points costmap;
endif
:combine costmap;
:publish costmap;
stop
@enduml
```
194 changes: 194 additions & 0 deletions planning/costmap_generator/include/costmap_generator/costmap_generator.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,194 @@
// Copyright 2020 Tier IV, Inc.
//
// 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.

/*
* 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__COSTMAP_GENERATOR_HPP_
#define COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_

#include "costmap_generator/objects_to_costmap.hpp"
#include "costmap_generator/points_to_costmap.hpp"

#include <grid_map_ros/GridMapRosConverter.hpp>
#include <grid_map_ros/grid_map_ros.hpp>
#include <lanelet2_extension/utility/message_conversion.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_planning_msgs/msg/scenario.hpp>

#include <grid_map_msgs/msg/grid_map.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <memory>
#include <string>
#include <vector>

class CostmapGenerator : public rclcpp::Node
{
public:
explicit CostmapGenerator(const rclcpp::NodeOptions & node_options);

private:
bool use_objects_;
bool use_points_;
bool use_wayarea_;

lanelet::LaneletMapPtr lanelet_map_;
autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_;
sensor_msgs::msg::PointCloud2::ConstSharedPtr points_;

std::string costmap_frame_;
std::string vehicle_frame_;
std::string map_frame_;

double update_rate_;

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_;

rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr pub_costmap_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_occupancy_grid_;

rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_points_;
rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr
sub_objects_;
rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_lanelet_bin_map_;
rclcpp::Subscription<autoware_planning_msgs::msg::Scenario>::SharedPtr sub_scenario_;

rclcpp::TimerBase::SharedPtr timer_;

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;

std::vector<std::vector<geometry_msgs::msg::Point>> area_points_;

PointsToCostmap points2costmap_;
ObjectsToCostmap objects2costmap_;

autoware_planning_msgs::msg::Scenario::ConstSharedPtr scenario_;

struct LayerName
{
static constexpr const char * objects = "objects";
static constexpr const char * points = "points";
static constexpr const char * wayarea = "wayarea";
static constexpr const char * combined = "combined";
};

/// \brief wait for lanelet2 map to load and build routing graph
void initLaneletMap();

/// \brief callback for loading lanelet2 map
void onLaneletMapBin(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);

/// \brief callback for DynamicObjectArray
/// \param[in] in_objects input DynamicObjectArray usually from prediction or perception
/// component
void onObjects(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg);

/// \brief callback for sensor_msgs::PointCloud2
/// \param[in] in_points input sensor_msgs::PointCloud2. Assuming ground-filtered pointcloud
/// by default
void onPoints(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);

void onScenario(const autoware_planning_msgs::msg::Scenario::ConstSharedPtr msg);

void onTimer();

/// \brief initialize gridmap parameters based on rosparam
void initGridmap();

/// \brief publish ros msg: grid_map::GridMap, and nav_msgs::OccupancyGrid
/// \param[in] gridmap with calculated cost
void publishCostmap(const grid_map::GridMap & costmap);

/// \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::msg::Point>> * area_points);

/// \brief set area_points from parking-area polygons
/// \param [in] input lanelet_map
/// \param [out] calculated area_points of lanelet polygons
void loadParkingAreasFromLaneletMap(
const lanelet::LaneletMapPtr lanelet_map,
std::vector<std::vector<geometry_msgs::msg::Point>> * area_points);

/// \brief calculate cost from pointcloud data
/// \param[in] in_points: subscribed pointcloud data
grid_map::Matrix generatePointsCostmap(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & in_points);

/// \brief calculate cost from DynamicObjectArray
/// \param[in] in_objects: subscribed DynamicObjectArray
grid_map::Matrix generateObjectsCostmap(
const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects);

/// \brief calculate cost from lanelet2 map
grid_map::Matrix generateWayAreaCostmap();

/// \brief calculate cost for final output
grid_map::Matrix generateCombinedCostmap();
};

#endif // COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_
Loading

0 comments on commit 1f9486e

Please sign in to comment.