diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml index c5c54ba6..613dd9d7 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -1,17 +1,24 @@ /**: ros__parameters: - update_rate: 10.0 + update_rate: 20.0 map_frame_id: "map" costmap_center_frame_id: "base_link" costmap_width: 50.0 costmap_resolution: 0.2 multi_layered_costmap: layers: - - "lanelet_layer" + - "cached_lanelet_layer" - "predicted_object_layer" - lanelet_layer: - type: "lanelet" + cached_lanelet_layer: + type: "cached_lanelet" map_topic: "/map/vector_map" + costmap_topic: "~/debug/cached_costmap" + cached_costmap: + min_x: 89607.0 + max_x: 89687.0 + min_y: 43114.0 + max_y: 43194.0 + resolution: 0.1 predicted_object_layer: type: "predicted_object" predicted_objects_topic: "/perception/object_recognition/objects" diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp index b6ccaddf..8c32ac5c 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp @@ -25,37 +25,49 @@ class OccupancyGridParameters using SharedPtr = std::shared_ptr; static OccupancyGridParameters::SharedPtr create_parameters( - const double width, const double resolution) + const double width, const double height, const double resolution) { - return std::make_shared(width, resolution); + return std::make_shared(width, height, resolution); } - explicit OccupancyGridParameters(const double width, const double resolution) + explicit OccupancyGridParameters(const double width, const double height, const double resolution) { width_ = width; width_2_ = width / 2.0; + height_ = height; + height_2_ = height / 2.0; resolution_ = resolution; resolution_inv_ = 1.0 / resolution; grid_width_2_ = static_cast(width * resolution_inv_) / 2; grid_width_ = grid_width_2_ * 2; - grid_num_ = grid_width_ * grid_width_; + grid_height_2_ = static_cast(height * resolution_inv_) / 2; + grid_height_ = grid_height_2_ * 2; + grid_num_ = grid_width_ * grid_height_; } double width() const { return width_; } double width_2() const { return width_2_; } + double height() const { return height_; } + double height_2() const { return height_2_; } double resolution() const { return resolution_; } double resolution_inv() const { return resolution_inv_; } int grid_width_2() const { return grid_width_2_; } int grid_width() const { return grid_width_; } + int gird_height_2() const { return grid_height_2_; } + int grid_height() const { return grid_height_; } int grid_num() const { return grid_num_; } private: double width_; double width_2_; + double height_; + double height_2_; double resolution_; double resolution_inv_; int grid_width_2_; int grid_width_; + int grid_height_2_; + int grid_height_; int grid_num_; }; diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp index 61af7876..933f3917 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp @@ -29,7 +29,8 @@ OccupancyGridParameters::SharedPtr create_occupancy_grid_parameters( const OccupancyGrid::SharedPtr occupancy_grid) { const double width = occupancy_grid->info.width * occupancy_grid->info.resolution; - return OccupancyGridParameters::create_parameters(width, occupancy_grid->info.resolution); + const double height = occupancy_grid->info.height * occupancy_grid->info.resolution; + return OccupancyGridParameters::create_parameters(width, height, occupancy_grid->info.resolution); } OccupancyGrid::SharedPtr create_occupancy_grid( @@ -37,10 +38,10 @@ OccupancyGrid::SharedPtr create_occupancy_grid( { OccupancyGrid::SharedPtr occupancy_grid = std::make_shared(); occupancy_grid->info.width = parameters->grid_width(); - occupancy_grid->info.height = parameters->grid_width(); + occupancy_grid->info.height = parameters->grid_height(); occupancy_grid->info.resolution = parameters->resolution(); occupancy_grid->info.origin.position.x = -parameters->width_2(); - occupancy_grid->info.origin.position.y = -parameters->width_2(); + occupancy_grid->info.origin.position.y = -parameters->height_2(); occupancy_grid->data.resize(parameters->grid_num(), value); return occupancy_grid; } @@ -50,7 +51,7 @@ void update_origin( const geometry_msgs::msg::Vector3 & translation) { occupancy_grid->info.origin.position.x = -parameters->width_2() + translation.x; - occupancy_grid->info.origin.position.y = -parameters->width_2() + translation.y; + occupancy_grid->info.origin.position.y = -parameters->height_2() + translation.y; occupancy_grid->info.origin.position.z = translation.z; occupancy_grid->info.origin.orientation.x = 0.0; occupancy_grid->info.origin.orientation.y = 0.0; @@ -66,7 +67,7 @@ tier4_autoware_utils::Point2d index_to_point( return { index_x * parameters->resolution() - parameters->width_2(), - index_y * parameters->resolution() - parameters->width_2()}; + index_y * parameters->resolution() - parameters->height_2()}; } std::vector get_index_to_point_table( @@ -80,6 +81,16 @@ std::vector get_index_to_point_table( return table; } +int point_to_index( + const OccupancyGridParameters::SharedPtr parameters, const tier4_autoware_utils::Point2d & point) +{ + const int x = + static_cast((point[0] + parameters->width_2()) * parameters->resolution_inv() + 0.5); + const int y = + static_cast((point[1] + parameters->height_2()) * parameters->resolution_inv() + 0.5); + return y * parameters->grid_width() + x; +} + } // namespace booars_utils::nav::occupancy_grid_utils #endif // BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml index 8173e2a4..3f461d36 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml @@ -1,14 +1,25 @@ /**: ros__parameters: - update_rate: 10.0 + update_rate: 1.0 map_frame_id: "map" costmap_center_frame_id: "base_link" - costmap_width: 20.0 - costmap_resolution: 0.1 + costmap_width: 50.0 + costmap_resolution: 0.2 multi_layered_costmap: layers: + - "cached_lanelet_layer" - "lanelet_layer" - "predicted_object_layer" + cached_lanelet_layer: + type: "cached_lanelet" + map_topic: "/map/vector_map" + costmap_topic: "~/debug/cached_costmap" + cached_costmap: + min_x: 89607.0 + max_x: 89687.0 + min_y: 43114.0 + max_y: 43194.0 + resolution: 0.1 lanelet_layer: type: "lanelet" map_topic: "/map/vector_map" diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp index aa86eee8..60b34007 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp @@ -44,7 +44,7 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & options) double costmap_width = this->declare_parameter("costmap_width", 10.0); double costmap_resolution = this->declare_parameter("costmap_resolution", 0.1); costmap_parameters_ = - OccupancyGridParameters::create_parameters(costmap_width, costmap_resolution); + OccupancyGridParameters::create_parameters(costmap_width, costmap_width, costmap_resolution); index_to_point_table_ = booars_utils::nav::occupancy_grid_utils::get_index_to_point_table(costmap_parameters_); } diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp index dc4ef26d..dabb347c 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp @@ -15,12 +15,14 @@ #ifndef BOOARS_COSTMAP_UTILS__BOOARS_COSTMAP_UTILS_HPP_ #define BOOARS_COSTMAP_UTILS__BOOARS_COSTMAP_UTILS_HPP_ +#include #include #include #include namespace booars_costmap_utils { +using CachedLaneletCostmap = cached_lanelet_costmap::CachedLaneletCostmap; using LaneletCostmap = lanelet_costmap::LaneletCostmap; using MultiLayeredCostmap = multi_layered_costmap::MultiLayeredCostmap; using PredictedObjectCostmap = predicted_object_costmap::PredictedObjectCostmap; @@ -36,7 +38,10 @@ MultiLayeredCostmap::SharedPtr create_multi_layered_costmap( std::string layer_namespace = costmap_namespace + "." + layer; auto type = node.declare_parameter(layer_namespace + ".type", std::string()); - if (type == "lanelet") { + if (type == "cached_lanelet") { + auto cached_lanelet_costmap = CachedLaneletCostmap::create_costmap(node, layer_namespace); + costmap->add_costmap_layer(cached_lanelet_costmap); + } else if (type == "lanelet") { auto lanelet_costmap = LaneletCostmap::create_costmap(node, layer_namespace); costmap->add_costmap_layer(lanelet_costmap); } else if (type == "predicted_object") { diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml index 66e55ae9..fef8a5ab 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml @@ -9,6 +9,7 @@ ament_cmake_auto + cached_lanelet_costmap lanelet_costmap multi_layered_costmap predicted_object_costmap diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/CMakeLists.txt new file mode 100644 index 00000000..dd052e14 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(cached_lanelet_costmap) + +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) + +ament_auto_find_build_dependencies() + +ament_auto_add_library(cached_lanelet_costmap SHARED + src/cached_lanelet_costmap.cpp +) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp new file mode 100644 index 00000000..9bb7f072 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/include/cached_lanelet_costmap/cached_lanelet_costmap.hpp @@ -0,0 +1,75 @@ +// Copyright 2024 Booars +// +// 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 CACHED_LANELET_COSTMAP__CACHED_LANELET_COSTMAP_HPP_ +#define CACHED_LANELET_COSTMAP__CACHED_LANELET_COSTMAP_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include + +#include + +namespace cached_lanelet_costmap +{ + +using OccupancyGrid = nav_msgs::msg::OccupancyGrid; +using OccupancyGridParameters = booars_utils::nav::OccupancyGridParameters; +using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + +class CachedLaneletCostmap : public multi_layered_costmap::CostmapBase +{ +public: + using SharedPtr = std::shared_ptr; + explicit CachedLaneletCostmap(rclcpp::Node & node, const std::string & layer_namespace); + + static CostmapBase::SharedPtr create_costmap( + rclcpp::Node & node, const std::string & layer_namespace) + { + return std::make_shared(node, layer_namespace); + } + + bool is_ready() override; + bool is_occupied(const geometry_msgs::msg::PointStamped & point) override; + +private: + bool try_transform_point( + const geometry_msgs::msg::PointStamped & point, geometry_msgs::msg::Point & transformed_point, + const std::string & target_frame); + void create_cached_costmap( + const std::string & map_frame_id, const lanelet::ConstLanelets & roads_lanelets); + void map_callback(const HADMapBin::SharedPtr msg); + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Publisher::SharedPtr costmap_pub_; + + OccupancyGrid::SharedPtr cached_costmap_; + OccupancyGridParameters::SharedPtr cached_costmap_parameters_; + + double cached_costmap_origin_x_; + double cached_costmap_origin_y_; + bool is_map_ready_ = false; +}; +} // namespace cached_lanelet_costmap + +#endif // CACHED_LANELET_COSTMAP__CACHED_LANELET_COSTMAP_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/package.xml new file mode 100644 index 00000000..83283207 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/package.xml @@ -0,0 +1,25 @@ + + + + cached_lanelet_costmap + 0.0.0 + The cached_lanelet_costmap pkg + Akiro Harada + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_mapping_msgs + booars_utils + geometry_msgs + lanelet2_extension + multi_layered_costmap + rclcpp + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + + + ament_cmake + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp new file mode 100644 index 00000000..8f206d03 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/cached_lanelet_costmap/src/cached_lanelet_costmap.cpp @@ -0,0 +1,144 @@ +// Copyright 2024 Booars +// +// 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 "cached_lanelet_costmap/cached_lanelet_costmap.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include + +namespace cached_lanelet_costmap +{ +CachedLaneletCostmap::CachedLaneletCostmap(rclcpp::Node & node, const std::string & layer_namespace) +: tf_buffer_(node.get_clock()), tf_listener_(tf_buffer_) +{ + std::string map_topic = node.declare_parameter(layer_namespace + ".map_topic", "~/input/map"); + map_sub_ = node.create_subscription( + map_topic, rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(), + std::bind(&CachedLaneletCostmap::map_callback, this, std::placeholders::_1)); + + std::string costmap_topic = + node.declare_parameter(layer_namespace + ".costmap_topic", "~/output/costmap"); + costmap_pub_ = node.create_publisher( + costmap_topic, rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local()); + + { + double cached_costmap_min_x = + node.declare_parameter(layer_namespace + ".cached_costmap.min_x", 0.0); + double cached_costmap_min_y = + node.declare_parameter(layer_namespace + ".cached_costmap.min_y", 0.0); + double cached_costmap_max_x = + node.declare_parameter(layer_namespace + ".cached_costmap.max_x", 0.0); + double cached_costmap_max_y = + node.declare_parameter(layer_namespace + ".cached_costmap.max_y", 0.0); + double cached_costmap_resolution = + node.declare_parameter(layer_namespace + ".cached_costmap.resolution", 0.0); + + cached_costmap_parameters_ = OccupancyGridParameters::create_parameters( + cached_costmap_max_x - cached_costmap_min_x, cached_costmap_max_y - cached_costmap_min_y, + cached_costmap_resolution); + cached_costmap_origin_x_ = (cached_costmap_min_x + cached_costmap_max_x) * 0.5; + cached_costmap_origin_y_ = (cached_costmap_min_y + cached_costmap_max_y) * 0.5; + + cached_costmap_ = + booars_utils::nav::occupancy_grid_utils::create_occupancy_grid(cached_costmap_parameters_); + + geometry_msgs::msg::Vector3 origin; + origin.x = cached_costmap_origin_x_; + origin.y = cached_costmap_origin_y_; + origin.z = 0.0; + booars_utils::nav::occupancy_grid_utils::update_origin( + cached_costmap_, cached_costmap_parameters_, origin); + } +} + +bool CachedLaneletCostmap::is_ready() +{ + return is_map_ready_; +} + +bool CachedLaneletCostmap::is_occupied(const geometry_msgs::msg::PointStamped & point) +{ + if (!is_ready()) return true; + + geometry_msgs::msg::Point transformed_point; + if (!try_transform_point(point, transformed_point, cached_costmap_->header.frame_id)) return true; + + tier4_autoware_utils::Point2d point2d( + transformed_point.x - cached_costmap_origin_x_, transformed_point.y - cached_costmap_origin_y_); + if ( + point2d[0] < -cached_costmap_parameters_->width_2() || + point2d[0] > cached_costmap_parameters_->width_2() || + point2d[1] < -cached_costmap_parameters_->height_2() || + point2d[1] > cached_costmap_parameters_->height_2()) { + return true; + } + int index = + booars_utils::nav::occupancy_grid_utils::point_to_index(cached_costmap_parameters_, point2d); + if (index < 0 || index >= cached_costmap_parameters_->grid_num()) return true; + return cached_costmap_->data[index] > 0; +} + +bool CachedLaneletCostmap::try_transform_point( + const geometry_msgs::msg::PointStamped & point, geometry_msgs::msg::Point & transformed_point, + const std::string & target_frame) +{ + geometry_msgs::msg::PointStamped transformed_point_stamped; + try { + transformed_point_stamped = tf_buffer_.transform(point, target_frame); + } catch (tf2::TransformException & ex) { + return false; + } + + transformed_point = transformed_point_stamped.point; + return true; +} + +void CachedLaneletCostmap::create_cached_costmap( + const std::string & map_frame_id, const lanelet::ConstLanelets & roads_lanelets) +{ + cached_costmap_->header.frame_id = map_frame_id; + for (int i = 0; i < cached_costmap_parameters_->grid_num(); i++) { + tier4_autoware_utils::Point2d point2d = + booars_utils::nav::occupancy_grid_utils::index_to_point(cached_costmap_parameters_, i); + point2d[0] += cached_costmap_origin_x_; + point2d[1] += cached_costmap_origin_y_; + cached_costmap_->data[i] = 100; + for (const auto & road_lanelet : roads_lanelets) { + if (!lanelet::geometry::within(point2d, road_lanelet.polygon2d().basicPolygon())) continue; + cached_costmap_->data[i] = 0; + break; + } + } + costmap_pub_->publish(*cached_costmap_); + + is_map_ready_ = true; +} + +void CachedLaneletCostmap::map_callback(const HADMapBin::SharedPtr msg) +{ + lanelet::LaneletMapPtr map = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*msg, map); + const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map); + const lanelet::ConstLanelets roads_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + create_cached_costmap(msg->header.frame_id, roads_lanelets); +} +} // namespace cached_lanelet_costmap