diff --git a/planning/costmap_generator/CMakeLists.txt b/planning/costmap_generator/CMakeLists.txt new file mode 100644 index 0000000000000..e6662870b73be --- /dev/null +++ b/planning/costmap_generator/CMakeLists.txt @@ -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) diff --git a/planning/costmap_generator/README.md b/planning/costmap_generator/README.md new file mode 100644 index 0000000000000..4bd88558c9859 --- /dev/null +++ b/planning/costmap_generator/README.md @@ -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 +``` diff --git a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp new file mode 100644 index 0000000000000..88b8c9a834cd4 --- /dev/null +++ b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp @@ -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 +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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::SharedPtr pub_costmap_; + rclcpp::Publisher::SharedPtr pub_occupancy_grid_; + + rclcpp::Subscription::SharedPtr sub_points_; + rclcpp::Subscription::SharedPtr + sub_objects_; + rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; + rclcpp::Subscription::SharedPtr sub_scenario_; + + rclcpp::TimerBase::SharedPtr timer_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + std::vector> 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> * 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> * 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_ diff --git a/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp b/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp new file mode 100644 index 0000000000000..3c7129d06c84f --- /dev/null +++ b/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp @@ -0,0 +1,97 @@ +// 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 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 COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#define COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +namespace object_map +{ +/*! + * 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 rclcpp::Publisher::SharedPtr 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 rclcpp::Publisher::SharedPtr in_publisher, + const std::string & in_layer, double in_min_value, double in_max_value, double in_height); + +/*! + * 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> & 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 tf2_ros::Buffer & in_tf_buffer); + +} // namespace object_map + +#endif // COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ diff --git a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp new file mode 100644 index 0000000000000..92915ab392f3d --- /dev/null +++ b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp @@ -0,0 +1,122 @@ +// 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__OBJECTS_TO_COSTMAP_HPP_ +#define COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ + +#include +#include + +#include + +#include + +class ObjectsToCostmap +{ +public: + ObjectsToCostmap(); + + /// \brief calculate cost from PredictedObjects + /// \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 PredictedObjects + /// \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_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); + +private: + 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 PredictedObjects + /// \param[in] expand_rectangle_size: expanding 4 points + /// \param[out] 4 rectangle points + Eigen::MatrixXd makeRectanglePoints( + const autoware_auto_perception_msgs::msg::PredictedObject & 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 PredictedObjects + /// \param[in] expand_rectangle_size: expanding 4 points + /// \param[out] polygon with 4 rectangle points + grid_map::Polygon makePolygonFromObjectBox( + const std_msgs::msg::Header & header, + const autoware_auto_perception_msgs::msg::PredictedObject & 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::msg::Point makeExpandedPoint( + const geometry_msgs::msg::Point & in_centroid, + const geometry_msgs::msg::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 std_msgs::msg::Header & header, + const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const double expand_polygon_size); + + /// \brief set cost in polygon by using DynamicObject'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 // COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ diff --git a/planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp b/planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp new file mode 100644 index 0000000000000..4928d841659de --- /dev/null +++ b/planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp @@ -0,0 +1,120 @@ +// 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__POINTS_TO_COSTMAP_HPP_ +#define COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ + +#include +#include + +#include + +#include +#include + +class PointsToCostmap +{ +public: + /// \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 makeCostmapFromPoints( + 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 & in_sensor_points); + +private: + 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] 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>> assignPoints2GridCell( + const pcl::PointCloud & 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] calculated 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>> grid_vec); +}; + +#endif // COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ diff --git a/planning/costmap_generator/launch/costmap_generator.launch.xml b/planning/costmap_generator/launch/costmap_generator.launch.xml new file mode 100644 index 0000000000000..aca124b9bdbfa --- /dev/null +++ b/planning/costmap_generator/launch/costmap_generator.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp new file mode 100644 index 0000000000000..dab02fa0dc940 --- /dev/null +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -0,0 +1,407 @@ +// 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, private_node + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * private_node 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 + * private_node software without specific prior written permission. + * + * private_node 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 private_node SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ********************/ + +#include "costmap_generator/costmap_generator.hpp" +#include "costmap_generator/object_map_utils.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace +{ +bool isActive(const autoware_planning_msgs::msg::Scenario::ConstSharedPtr scenario) +{ + if (!scenario) { + return false; + } + + const auto & s = scenario->activating_scenarios; + if ( + std::find(std::begin(s), std::end(s), autoware_planning_msgs::msg::Scenario::PARKING) != + std::end(s)) { + return true; + } + + return false; +} + +// Convert from Point32 to Point +std::vector poly2vector(const geometry_msgs::msg::Polygon & poly) +{ + std::vector ps; + for (const auto & p32 : poly.points) { + geometry_msgs::msg::Point p; + p.x = p32.x; + p.y = p32.y; + p.z = p32.z; + ps.push_back(p); + } + return ps; +} + +pcl::PointCloud getTransformedPointCloud( + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, + const geometry_msgs::msg::Transform & transform) +{ + const Eigen::Matrix4f transform_matrix = tf2::transformToEigen(transform).matrix().cast(); + + sensor_msgs::msg::PointCloud2 transformed_msg; + pcl_ros::transformPointCloud(transform_matrix, pointcloud_msg, transformed_msg); + + pcl::PointCloud transformed_pointcloud; + pcl::fromROSMsg(transformed_msg, transformed_pointcloud); + + return transformed_pointcloud; +} + +} // namespace + +CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) +: Node("costmap_generator", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) +{ + // Parameters + costmap_frame_ = this->declare_parameter("costmap_frame", "map"); + vehicle_frame_ = this->declare_parameter("vehicle_frame", "base_link"); + map_frame_ = this->declare_parameter("map_frame", "map"); + update_rate_ = this->declare_parameter("update_rate", 10.0); + grid_min_value_ = this->declare_parameter("grid_min_value", 0.0); + grid_max_value_ = this->declare_parameter("grid_max_value", 1.0); + grid_resolution_ = this->declare_parameter("grid_resolution", 0.2); + grid_length_x_ = this->declare_parameter("grid_length_x", 50); + grid_length_y_ = this->declare_parameter("grid_length_y", 30); + grid_position_x_ = this->declare_parameter("grid_position_x", 20); + grid_position_y_ = this->declare_parameter("grid_position_y", 0); + maximum_lidar_height_thres_ = this->declare_parameter("maximum_lidar_height_thres", 0.3); + minimum_lidar_height_thres_ = this->declare_parameter("minimum_lidar_height_thres", -2.2); + use_objects_ = this->declare_parameter("use_objects", true); + use_points_ = this->declare_parameter("use_points", true); + use_wayarea_ = this->declare_parameter("use_wayarea", true); + expand_polygon_size_ = this->declare_parameter("expand_polygon_size", 1.0); + size_of_expansion_kernel_ = this->declare_parameter("size_of_expansion_kernel", 9); + + // Wait for first tf + // We want to do this before creating subscriptions + while (rclcpp::ok()) { + try { + tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero); + break; + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO(this->get_logger(), "waiting for initial pose..."); + } + rclcpp::sleep_for(std::chrono::milliseconds(5000)); + } + + // Subscribers + using std::placeholders::_1; + sub_objects_ = this->create_subscription( + "~/input/objects", 1, std::bind(&CostmapGenerator::onObjects, this, _1)); + sub_points_ = this->create_subscription( + "~/input/points_no_ground", rclcpp::SensorDataQoS(), + std::bind(&CostmapGenerator::onPoints, this, _1)); + sub_lanelet_bin_map_ = this->create_subscription( + "~/input/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&CostmapGenerator::onLaneletMapBin, this, _1)); + sub_scenario_ = this->create_subscription( + "~/input/scenario", 1, std::bind(&CostmapGenerator::onScenario, this, _1)); + + // Publishers + pub_costmap_ = this->create_publisher("~/output/grid_map", 1); + pub_occupancy_grid_ = + this->create_publisher("~/output/occupancy_grid", 1); + + // Timer + auto timer_callback = std::bind(&CostmapGenerator::onTimer, this); + const auto period = rclcpp::Rate(update_rate_).period(); + timer_ = std::make_shared>( + this->get_clock(), period, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); + + // Initialize + initGridmap(); +} + +void CostmapGenerator::loadRoadAreasFromLaneletMap( + const lanelet::LaneletMapPtr lanelet_map, + std::vector> * 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::msg::Polygon poly; + lanelet::visualization::lanelet2Polygon(ll, &poly); + area_points->push_back(poly2vector(poly)); + } +} + +void CostmapGenerator::loadParkingAreasFromLaneletMap( + const lanelet::LaneletMapPtr lanelet_map, + std::vector> * area_points) +{ + // Parking lots + lanelet::ConstPolygons3d all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map); + for (const auto & ll_poly : all_parking_lots) { + geometry_msgs::msg::Polygon poly; + lanelet::utils::conversion::toGeomMsgPoly(ll_poly, &poly); + area_points->push_back(poly2vector(poly)); + } + + // Parking spaces + lanelet::ConstLineStrings3d all_parking_spaces = + lanelet::utils::query::getAllParkingSpaces(lanelet_map); + for (const auto & parking_space : all_parking_spaces) { + lanelet::ConstPolygon3d ll_poly; + lanelet::utils::lineStringWithWidthToPolygon(parking_space, &ll_poly); + + geometry_msgs::msg::Polygon poly; + lanelet::utils::conversion::toGeomMsgPoly(ll_poly, &poly); + area_points->push_back(poly2vector(poly)); + } +} + +void CostmapGenerator::onLaneletMapBin( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +{ + lanelet_map_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_); + + if (use_wayarea_) { + loadRoadAreasFromLaneletMap(lanelet_map_, &area_points_); + loadParkingAreasFromLaneletMap(lanelet_map_, &area_points_); + } +} + +void CostmapGenerator::onObjects( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) +{ + objects_ = msg; +} + +void CostmapGenerator::onPoints(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + points_ = msg; +} + +void CostmapGenerator::onScenario(const autoware_planning_msgs::msg::Scenario::ConstSharedPtr msg) +{ + scenario_ = msg; +} + +void CostmapGenerator::onTimer() +{ + if (!isActive(scenario_)) { + return; + } + + // Get current pose + geometry_msgs::msg::TransformStamped tf; + try { + tf = tf_buffer_.lookupTransform( + costmap_frame_, vehicle_frame_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + return; + } + + // Set grid center + grid_map::Position p; + p.x() = tf.transform.translation.x; + p.y() = tf.transform.translation.y; + costmap_.setPosition(p); + + if (use_wayarea_ && lanelet_map_) { + costmap_[LayerName::wayarea] = generateWayAreaCostmap(); + } + + if (use_objects_ && objects_) { + costmap_[LayerName::objects] = generateObjectsCostmap(objects_); + } + + if (use_points_ && points_) { + costmap_[LayerName::points] = generatePointsCostmap(points_); + } + + costmap_[LayerName::combined] = generateCombinedCostmap(); + + publishCostmap(costmap_); +} + +void CostmapGenerator::initGridmap() +{ + costmap_.setFrameId(costmap_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(LayerName::points, grid_min_value_); + costmap_.add(LayerName::objects, grid_min_value_); + costmap_.add(LayerName::wayarea, grid_min_value_); + costmap_.add(LayerName::combined, grid_min_value_); +} + +grid_map::Matrix CostmapGenerator::generatePointsCostmap( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & in_points) +{ + geometry_msgs::msg::TransformStamped points2costmap; + try { + points2costmap = + tf_buffer_.lookupTransform(costmap_frame_, in_points->header.frame_id, tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_ERROR(rclcpp::get_logger("costmap_generator"), "%s", ex.what()); + } + + const auto transformed_points = getTransformedPointCloud(*in_points, points2costmap.transform); + + grid_map::Matrix points_costmap = points2costmap_.makeCostmapFromPoints( + maximum_lidar_height_thres_, minimum_lidar_height_thres_, grid_min_value_, grid_max_value_, + costmap_, LayerName::points, transformed_points); + + return points_costmap; +} + +autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects( + const tf2_ros::Buffer & tf_buffer, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects, + const std::string & target_frame_id, const std::string & src_frame_id) +{ + auto objects = new autoware_auto_perception_msgs::msg::PredictedObjects(); + *objects = *in_objects; + objects->header.frame_id = target_frame_id; + + geometry_msgs::msg::TransformStamped objects2costmap; + try { + objects2costmap = tf_buffer.lookupTransform(target_frame_id, src_frame_id, tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_ERROR(rclcpp::get_logger("costmap_generator"), "%s", ex.what()); + } + + for (auto & object : objects->objects) { + geometry_msgs::msg::PoseStamped output_stamped, input_stamped; + input_stamped.pose = object.kinematics.initial_pose_with_covariance.pose; + tf2::doTransform(input_stamped, output_stamped, objects2costmap); + object.kinematics.initial_pose_with_covariance.pose = output_stamped.pose; + } + + return autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr(objects); +} + +grid_map::Matrix CostmapGenerator::generateObjectsCostmap( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) +{ + const auto object_frame = in_objects->header.frame_id; + const auto transformed_objects = + transformObjects(tf_buffer_, in_objects, costmap_frame_, object_frame); + + grid_map::Matrix objects_costmap = objects2costmap_.makeCostmapFromObjects( + costmap_, expand_polygon_size_, size_of_expansion_kernel_, transformed_objects); + + return objects_costmap; +} + +grid_map::Matrix CostmapGenerator::generateWayAreaCostmap() +{ + grid_map::GridMap lanelet2_costmap = costmap_; + if (!area_points_.empty()) { + object_map::FillPolygonAreas( + lanelet2_costmap, area_points_, LayerName::wayarea, grid_max_value_, grid_min_value_, + grid_min_value_, grid_max_value_, costmap_frame_, map_frame_, tf_buffer_); + } + return lanelet2_costmap[LayerName::wayarea]; +} + +grid_map::Matrix CostmapGenerator::generateCombinedCostmap() +{ + // assuming combined_costmap is calculated by element wise max operation + grid_map::GridMap combined_costmap = costmap_; + + combined_costmap[LayerName::combined].setConstant(grid_min_value_); + + combined_costmap[LayerName::combined] = + combined_costmap[LayerName::combined].cwiseMax(combined_costmap[LayerName::points]); + + combined_costmap[LayerName::combined] = + combined_costmap[LayerName::combined].cwiseMax(combined_costmap[LayerName::wayarea]); + + combined_costmap[LayerName::combined] = + combined_costmap[LayerName::combined].cwiseMax(combined_costmap[LayerName::objects]); + + return combined_costmap[LayerName::combined]; +} + +void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap) +{ + // Set header + std_msgs::msg::Header header; + header.frame_id = costmap_frame_; + header.stamp = this->now(); + + // Publish OccupancyGrid + nav_msgs::msg::OccupancyGrid out_occupancy_grid; + grid_map::GridMapRosConverter::toOccupancyGrid( + costmap, LayerName::combined, grid_min_value_, grid_max_value_, out_occupancy_grid); + out_occupancy_grid.header = header; + pub_occupancy_grid_->publish(out_occupancy_grid); + + // Publish GridMap + auto out_gridmap_msg = grid_map::GridMapRosConverter::toMessage(costmap); + out_gridmap_msg->header = header; + pub_costmap_->publish(*out_gridmap_msg); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(CostmapGenerator) diff --git a/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp b/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp new file mode 100644 index 0000000000000..3680662b94803 --- /dev/null +++ b/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp @@ -0,0 +1,122 @@ +// 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 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 "costmap_generator/object_map_utils.hpp" + +#include +#include + +namespace object_map +{ +void PublishGridMap( + const grid_map::GridMap & in_gridmap, + const rclcpp::Publisher::SharedPtr in_publisher) +{ + auto message = grid_map::GridMapRosConverter::toMessage(in_gridmap); + in_publisher->publish(*message); +} + +void PublishOccupancyGrid( + const grid_map::GridMap & in_gridmap, + const rclcpp::Publisher::SharedPtr in_publisher, + const std::string & in_layer, double in_min_value, double in_max_value, double in_height) +{ + nav_msgs::msg::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); +} + +void FillPolygonAreas( + grid_map::GridMap & out_grid_map, + const std::vector> & 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 tf2_ros::Buffer & in_tf_buffer) +{ + 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( + out_grid_map, in_grid_layer_name, CV_8UC1, in_layer_min_value, in_layer_max_value, + original_image); + + cv::Mat merged_filled_image = original_image.clone(); + + geometry_msgs::msg::TransformStamped transform; + transform = in_tf_buffer.lookupTransform( + in_tf_target_frame, in_tf_source_frame, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + + // calculate out_grid_map position + grid_map::Position map_pos = out_grid_map.getPosition(); + const double origin_x_offset = out_grid_map.getLength().x() / 2.0 - map_pos.x(); + const double origin_y_offset = out_grid_map.getLength().y() / 2.0 - map_pos.y(); + + for (const auto & points : in_area_points) { + std::vector cv_polygon; + + for (const auto & p : points) { + // transform to GridMap coordinate + geometry_msgs::msg::Point transformed_point; + geometry_msgs::msg::PointStamped output_stamped, input_stamped; + input_stamped.point = p; + tf2::doTransform(input_stamped, output_stamped, transform); + transformed_point = output_stamped.point; + + // coordinate conversion for cv image + const double cv_x = (out_grid_map.getLength().y() - origin_y_offset - transformed_point.y) / + out_grid_map.getResolution(); + const double cv_y = (out_grid_map.getLength().x() - origin_x_offset - transformed_point.x) / + out_grid_map.getResolution(); + cv_polygon.emplace_back(cv_x, cv_y); + } + + cv::Mat filled_image = original_image.clone(); + + std::vector> cv_polygons; + cv_polygons.push_back(cv_polygon); + cv::fillPoly(filled_image, cv_polygons, cv::Scalar(in_fill_color)); + + merged_filled_image &= filled_image; + } + + // convert to ROS msg + grid_map::GridMapCvConverter::addLayerFromImage( + merged_filled_image, in_grid_layer_name, out_grid_map, in_layer_min_value, in_layer_max_value); +} + +} // namespace object_map diff --git a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp b/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp new file mode 100644 index 0000000000000..79ab4a3bf66c1 --- /dev/null +++ b/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp @@ -0,0 +1,198 @@ +// 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. + ********************/ + +#include "costmap_generator/objects_to_costmap.hpp" + +#include + +#include +#include + +// Constructor +ObjectsToCostmap::ObjectsToCostmap() +: NUMBER_OF_POINTS(4), + NUMBER_OF_DIMENSIONS(2), + OBJECTS_COSTMAP_LAYER_("objects_costmap"), + BLURRED_OBJECTS_COSTMAP_LAYER_("blurred_objects_costmap") +{ +} + +Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints( + const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const double expand_rectangle_size) +{ + double length = in_object.shape.dimensions.x + expand_rectangle_size; + double width = in_object.shape.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 = tf2::getYaw(in_object.kinematics.initial_pose_with_covariance.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.kinematics.initial_pose_with_covariance.pose.position.x; + double dy = in_object.kinematics.initial_pose_with_covariance.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 std_msgs::msg::Header & header, + const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const double expand_rectangle_size) +{ + grid_map::Polygon polygon; + polygon.setFrameId(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::msg::Point ObjectsToCostmap::makeExpandedPoint( + const geometry_msgs::msg::Point & in_centroid, + const geometry_msgs::msg::Point32 & in_corner_point, const double expand_polygon_size) +{ + geometry_msgs::msg::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_centroid.x + in_corner_point.x + delta_x; + expanded_point.y = in_centroid.y + in_corner_point.y + delta_y; + + return expanded_point; +} + +grid_map::Polygon ObjectsToCostmap::makePolygonFromObjectConvexHull( + const std_msgs::msg::Header & header, + const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const double expand_polygon_size) +{ + grid_map::Polygon polygon; + polygon.setFrameId(header.frame_id); + + double initial_z = in_object.shape.footprint.points[0].z; + for (size_t index = 0; index < in_object.shape.footprint.points.size(); index++) { + if (in_object.shape.footprint.points[index].z == initial_z) { + geometry_msgs::msg::Point centroid = + in_object.kinematics.initial_pose_with_covariance.pose.position; + geometry_msgs::msg::Point expanded_point = + makeExpandedPoint(centroid, in_object.shape.footprint.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) +{ + for (grid_map::PolygonIterator itr(objects_costmap, polygon); !itr.isPastEnd(); ++itr) { + const float current_score = objects_costmap.at(gridmap_layer_name, *itr); + if (score > current_score) { + objects_costmap.at(gridmap_layer_name, *itr) = score; + } + } +} + +grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( + const grid_map::GridMap & costmap, const double expand_polygon_size, + const double size_of_expansion_kernel, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) +{ + 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; + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + polygon = makePolygonFromObjectConvexHull(in_objects->header, object, expand_polygon_size); + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // TODO(Kenji Miyake): Add makePolygonFromObjectCylinder + polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); + } + const auto highest_probability_label = *std::max_element( + object.classification.begin(), object.classification.end(), + [](const auto & c1, const auto & c2) { return c1.probability < c2.probability; }); + const double highest_probability = static_cast(highest_probability_label.probability); + setCostInPolygon(polygon, OBJECTS_COSTMAP_LAYER_, highest_probability, objects_costmap); + setCostInPolygon(polygon, BLURRED_OBJECTS_COSTMAP_LAYER_, highest_probability, 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_]; +} diff --git a/planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp b/planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp new file mode 100644 index 0000000000000..e875f68973a12 --- /dev/null +++ b/planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp @@ -0,0 +1,142 @@ +// 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. + ********************/ + +#include "costmap_generator/points_to_costmap.hpp" + +#include +#include + +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>> PointsToCostmap::assignPoints2GridCell( + const pcl::PointCloud & 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 z_vec; + std::vector> vec_y_z(y_cell_size, z_vec); + std::vector>> 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>> 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::makeCostmapFromPoints( + 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 & in_sensor_points) +{ + initGridmapParam(gridmap); + std::vector>> grid_vec = assignPoints2GridCell(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; +} diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml new file mode 100644 index 0000000000000..18ce5cfd87de8 --- /dev/null +++ b/planning/costmap_generator/package.xml @@ -0,0 +1,39 @@ + + + costmap_generator + 0.1.0 + The costmap_generator package + + Kenji Miyake + Apache License 2.0 + + ando + BSD-3-Clause + + ament_cmake + ament_cmake_auto + + autoware_auto_mapping_msgs + autoware_auto_perception_msgs + autoware_planning_msgs + grid_map_ros + lanelet2_extension + libpcl-all-dev + pcl_conversions + pcl_ros + rclcpp + rclcpp_components + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + + ament_cmake + +