diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index b0417909..379992ee 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -2,6 +2,7 @@ aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/** @booar aichallenge/workspace/src/aichallenge_submit/booars_dummy_perception_publisher/** @Autumn60 aichallenge/workspace/src/aichallenge_submit/booars_launch/** @Autumn60 aichallenge/workspace/src/aichallenge_submit/booars_utils/** @Autumn60 +aichallenge/workspace/src/aichallenge_submit/costmap/** @Autumn60 aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/** @hrjp aichallenge/workspace/src/aichallenge_submit/gyro_odometer/** @booars/aic2024-developers aichallenge/workspace/src/aichallenge_submit/imu_corrector/** @booars/aic2024-developers 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 new file mode 100644 index 00000000..b6ccaddf --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp @@ -0,0 +1,64 @@ +// 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 BOOARS_UTILS__NAV__OCCUPANCY_GRID_PARAMETERS_HPP_ +#define BOOARS_UTILS__NAV__OCCUPANCY_GRID_PARAMETERS_HPP_ + +#include + +namespace booars_utils::nav +{ +class OccupancyGridParameters +{ +public: + using SharedPtr = std::shared_ptr; + + static OccupancyGridParameters::SharedPtr create_parameters( + const double width, const double resolution) + { + return std::make_shared(width, resolution); + } + + explicit OccupancyGridParameters(const double width, const double resolution) + { + width_ = width; + width_2_ = width / 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_; + } + + double width() const { return width_; } + double width_2() const { return width_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 grid_num() const { return grid_num_; } + +private: + double width_; + double width_2_; + double resolution_; + double resolution_inv_; + int grid_width_2_; + int grid_width_; + int grid_num_; +}; + +} // namespace booars_utils::nav + +#endif // BOOARS_UTILS__NAV__OCCUPANCY_GRID_PARAMETERS_HPP_ 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 new file mode 100644 index 00000000..61af7876 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp @@ -0,0 +1,85 @@ +// 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 BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_ +#define BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_ + +#include "booars_utils/nav/occupancy_grid_parameters.hpp" + +#include + +#include + +namespace booars_utils::nav::occupancy_grid_utils +{ +using OccupancyGrid = nav_msgs::msg::OccupancyGrid; + +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); +} + +OccupancyGrid::SharedPtr create_occupancy_grid( + const OccupancyGridParameters::SharedPtr parameters, const int8_t value = 0) +{ + OccupancyGrid::SharedPtr occupancy_grid = std::make_shared(); + occupancy_grid->info.width = parameters->grid_width(); + occupancy_grid->info.height = parameters->grid_width(); + 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->data.resize(parameters->grid_num(), value); + return occupancy_grid; +} + +void update_origin( + OccupancyGrid::SharedPtr occupancy_grid, const OccupancyGridParameters::SharedPtr parameters, + 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.z = translation.z; + occupancy_grid->info.origin.orientation.x = 0.0; + occupancy_grid->info.origin.orientation.y = 0.0; + occupancy_grid->info.origin.orientation.z = 0.0; + occupancy_grid->info.origin.orientation.w = 1.0; +} + +tier4_autoware_utils::Point2d index_to_point( + const OccupancyGridParameters::SharedPtr parameters, const int index) +{ + const int index_x = index % parameters->grid_width(); + const int index_y = index / parameters->grid_width(); + + return { + index_x * parameters->resolution() - parameters->width_2(), + index_y * parameters->resolution() - parameters->width_2()}; +} + +std::vector get_index_to_point_table( + const OccupancyGridParameters::SharedPtr parameters) +{ + std::vector table; + table.reserve(parameters->grid_num()); + for (int i = 0; i < parameters->grid_num(); ++i) { + table.push_back(index_to_point(parameters, i)); + } + return table; +} + +} // namespace booars_utils::nav::occupancy_grid_utils + +#endif // BOOARS_UTILS__NAV__OCCUPANCY_GRID_UTILS_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml b/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml index 5bb22bbe..0db5a693 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml @@ -10,7 +10,9 @@ ament_cmake_auto + nav_msgs rclcpp + tier4_autoware_utils ament_lint_auto diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/CMakeLists.txt new file mode 100644 index 00000000..b1158743 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.5) +project(booars_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) +ament_auto_find_build_dependencies() + +ament_auto_add_library(costmap_generator SHARED + src/costmap_generator.cpp +) + +rclcpp_components_register_node(costmap_generator + PLUGIN "booars_costmap_generator::CostmapGenerator" + EXECUTABLE costmap_generator_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + config + launch +) \ No newline at end of file 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 new file mode 100644 index 00000000..8173e2a4 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + update_rate: 10.0 + map_frame_id: "map" + costmap_center_frame_id: "base_link" + costmap_width: 20.0 + costmap_resolution: 0.1 + multi_layered_costmap: + layers: + - "lanelet_layer" + - "predicted_object_layer" + lanelet_layer: + type: "lanelet" + map_topic: "/map/vector_map" + predicted_object_layer: + type: "predicted_object" + predicted_objects_topic: "/perception/object_recognition/objects" + distance_threshold: 3.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/include/booars_costmap_generator/costmap_generator.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/include/booars_costmap_generator/costmap_generator.hpp new file mode 100644 index 00000000..ce942902 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/include/booars_costmap_generator/costmap_generator.hpp @@ -0,0 +1,68 @@ +// 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 BOOARS_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#define BOOARS_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace booars_costmap_generator +{ + +using FunctionTimer = booars_utils::ros::FunctionTimer; +using MultiLayeredCostmap = multi_layered_costmap::MultiLayeredCostmap; +using OccupancyGrid = nav_msgs::msg::OccupancyGrid; +using OccupancyGridParameters = booars_utils::nav::OccupancyGridParameters; +using PointStamped = geometry_msgs::msg::PointStamped; +using Point2d = tier4_autoware_utils::Point2d; +using Vector3 = geometry_msgs::msg::Vector3; + +class CostmapGenerator : public rclcpp::Node +{ +public: + explicit CostmapGenerator(const rclcpp::NodeOptions & options); + +private: + void update(); + bool try_get_transform(geometry_msgs::msg::TransformStamped & transform); + + FunctionTimer::SharedPtr update_timer_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Publisher::SharedPtr costmap_pub_; + MultiLayeredCostmap::SharedPtr multi_layered_costmap_; + + OccupancyGrid::SharedPtr costmap_; + OccupancyGridParameters::SharedPtr costmap_parameters_; + + std::string map_frame_; + std::string target_frame_; + + std::vector index_to_point_table_; +}; +}; // namespace booars_costmap_generator + +#endif // BOOARS_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml new file mode 100644 index 00000000..9501d94e --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/package.xml new file mode 100644 index 00000000..03a0188d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/package.xml @@ -0,0 +1,25 @@ + + + + booars_costmap_generator + 0.1.0 + The booars_costmap_generator package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + booars_costmap_utils + booars_utils + rclcpp + rclcpp_components + tf2_ros + tier4_autoware_utils + + ament_lint_auto + + + ament_cmake + + 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 new file mode 100644 index 00000000..aa86eee8 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp @@ -0,0 +1,122 @@ +// 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 "booars_costmap_generator/costmap_generator.hpp" + +#include + +namespace booars_costmap_generator +{ +CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & options) +: Node("costmap_generator", options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) +{ + // Declare parameters + { + map_frame_ = this->declare_parameter("map_frame_id", "map"); + target_frame_ = this->declare_parameter("costmap_center_frame_id", "base_link"); + } + + // Create publisher + { + costmap_pub_ = this->create_publisher("~/output/costmap", 1); + } + + // Crate update() timer + { + double update_rate = this->declare_parameter("update_rate", 10.0); + update_timer_ = FunctionTimer::create_function_timer( + this, update_rate, std::bind(&CostmapGenerator::update, this)); + } + + // Declare costmap parameters + { + 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); + index_to_point_table_ = + booars_utils::nav::occupancy_grid_utils::get_index_to_point_table(costmap_parameters_); + } + + // Create costmap + { + costmap_ = booars_utils::nav::occupancy_grid_utils::create_occupancy_grid(costmap_parameters_); + costmap_->header.frame_id = map_frame_; + } + + // Crate multi layered costmap + { + multi_layered_costmap_ = + booars_costmap_utils::create_multi_layered_costmap(*this, "multi_layered_costmap"); + } +} + +void CostmapGenerator::update() +{ + if (!multi_layered_costmap_->is_ready()) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "MultiLayeredCostmap is not ready"); + return; + } + + rclcpp::Time costmap_time; + Vector3 costmap_center_position; + { + geometry_msgs::msg::TransformStamped transform; + if (!try_get_transform(transform)) return; + costmap_time = transform.header.stamp; + costmap_center_position = transform.transform.translation; + } + + // Fill the costmap data + { + for (int i = 0; i < costmap_parameters_->grid_num(); i++) { + Point2d local_point = index_to_point_table_[i]; + PointStamped global_point; + global_point.header.frame_id = map_frame_; + global_point.header.stamp = costmap_time; + global_point.point.x = local_point[0] + costmap_center_position.x; + global_point.point.y = local_point[1] + costmap_center_position.y; + global_point.point.z = 0.0; + costmap_->data[i] = multi_layered_costmap_->is_occupied(global_point) ? 100 : 0; + } + } + + // Update the costmap origin + { + booars_utils::nav::occupancy_grid_utils::update_origin( + costmap_, costmap_parameters_, costmap_center_position); + } + + // Publish the costmap + { + costmap_->header.stamp = costmap_time; + costmap_pub_->publish(*costmap_); + } +} + +bool CostmapGenerator::try_get_transform(geometry_msgs::msg::TransformStamped & transform) +{ + try { + transform = tf_buffer_.lookupTransform(map_frame_, target_frame_, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "Failed to get transform: %s", ex.what()); + return false; + } + return true; +} +} // namespace booars_costmap_generator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(booars_costmap_generator::CostmapGenerator) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/CMakeLists.txt new file mode 100644 index 00000000..44ecaf10 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.5) +project(booars_costmap_utils) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + 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) +ament_auto_find_build_dependencies() + +ament_auto_package() 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 new file mode 100644 index 00000000..dc4ef26d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/include/booars_costmap_utils/booars_costmap_utils.hpp @@ -0,0 +1,55 @@ +// 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 BOOARS_COSTMAP_UTILS__BOOARS_COSTMAP_UTILS_HPP_ +#define BOOARS_COSTMAP_UTILS__BOOARS_COSTMAP_UTILS_HPP_ + +#include +#include +#include + +namespace booars_costmap_utils +{ +using LaneletCostmap = lanelet_costmap::LaneletCostmap; +using MultiLayeredCostmap = multi_layered_costmap::MultiLayeredCostmap; +using PredictedObjectCostmap = predicted_object_costmap::PredictedObjectCostmap; + +MultiLayeredCostmap::SharedPtr create_multi_layered_costmap( + rclcpp::Node & node, const std::string & costmap_namespace) +{ + auto costmap = std::make_shared(); + + auto layers = node.declare_parameter(costmap_namespace + ".layers", std::vector()); + + for (const auto & layer : layers) { + std::string layer_namespace = costmap_namespace + "." + layer; + auto type = node.declare_parameter(layer_namespace + ".type", std::string()); + + if (type == "lanelet") { + auto lanelet_costmap = LaneletCostmap::create_costmap(node, layer_namespace); + costmap->add_costmap_layer(lanelet_costmap); + } else if (type == "predicted_object") { + auto predicted_object_costmap = PredictedObjectCostmap::create_costmap(node, layer_namespace); + costmap->add_costmap_layer(predicted_object_costmap); + } else { + RCLCPP_ERROR(node.get_logger(), "Unknown layer type: %s", type.c_str()); + } + } + + return costmap; +} + +} // namespace booars_costmap_utils + +#endif 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 new file mode 100644 index 00000000..66e55ae9 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_utils/package.xml @@ -0,0 +1,20 @@ + + + + booars_costmap_utils + 0.0.0 + The booars_costmap_utils pkg + Akiro Harada + Apache License 2.0 + + ament_cmake_auto + + lanelet_costmap + multi_layered_costmap + predicted_object_costmap + rclcpp + + + ament_cmake + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/CMakeLists.txt new file mode 100644 index 00000000..27b2ef37 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(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(lanelet_costmap SHARED + src/lanelet_costmap.cpp +) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/include/lanelet_costmap/lanelet_costmap.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/include/lanelet_costmap/lanelet_costmap.hpp new file mode 100644 index 00000000..b7185e9e --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/include/lanelet_costmap/lanelet_costmap.hpp @@ -0,0 +1,65 @@ +// 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 LANELET_COSTMAP__LANELET_COSTMAP_HPP_ +#define LANELET_COSTMAP__LANELET_COSTMAP_HPP_ + +#include + +#include + +#include +#include +#include + +#include + +namespace lanelet_costmap +{ + +using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + +class LaneletCostmap : public multi_layered_costmap::CostmapBase +{ +public: + using SharedPtr = std::shared_ptr; + explicit LaneletCostmap(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 map_callback(const HADMapBin::SharedPtr msg); + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr map_sub_; + + std::string map_frame_id_; + lanelet::LaneletMapPtr map_; + lanelet::ConstLanelets roads_; +}; +} // namespace lanelet_costmap + +#endif // LANELET_COSTMAP__LANELET_COSTMAP_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/package.xml new file mode 100644 index 00000000..c695f51b --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/package.xml @@ -0,0 +1,24 @@ + + + + lanelet_costmap + 0.0.0 + The lanelet_costmap pkg + Akiro Harada + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_mapping_msgs + 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/lanelet_costmap/src/lanelet_costmap.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/src/lanelet_costmap.cpp new file mode 100644 index 00000000..3ce26508 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/lanelet_costmap/src/lanelet_costmap.cpp @@ -0,0 +1,84 @@ +// 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 "lanelet_costmap/lanelet_costmap.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include + +namespace lanelet_costmap +{ +LaneletCostmap::LaneletCostmap(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(&LaneletCostmap::map_callback, this, std::placeholders::_1)); +} + +bool LaneletCostmap::is_ready() +{ + return map_ != nullptr; +} + +bool LaneletCostmap::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, map_frame_id_)) { + return true; + } + + tier4_autoware_utils::Point2d point2d(transformed_point.x, transformed_point.y); + for (const auto & road : roads_) { + if (!lanelet::geometry::within(point2d, road.polygon2d().basicPolygon())) continue; + return false; + } + return true; +} + +bool LaneletCostmap::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 LaneletCostmap::map_callback(const HADMapBin::SharedPtr msg) +{ + map_frame_id_ = msg->header.frame_id; + map_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*msg, map_); + const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map_); + roads_ = lanelet::utils::query::roadLanelets(all_lanelets); +} +} // namespace lanelet_costmap diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/CMakeLists.txt new file mode 100644 index 00000000..666ac04f --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(multi_layered_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(multi_layered_costmap SHARED + src/multi_layered_costmap.cpp +) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/costmap_base.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/costmap_base.hpp new file mode 100644 index 00000000..12adc217 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/costmap_base.hpp @@ -0,0 +1,35 @@ +// 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 MULTI_LAYERED_COSTMAP__COSTMAP_BASE_HPP_ +#define MULTI_LAYERED_COSTMAP__COSTMAP_BASE_HPP_ + +#include + +#include + +#include + +namespace multi_layered_costmap +{ +class CostmapBase +{ +public: + using SharedPtr = std::shared_ptr; + virtual bool is_ready() = 0; + virtual bool is_occupied(const geometry_msgs::msg::PointStamped & point) = 0; +}; +} // namespace multi_layered_costmap + +#endif // MULTI_LAYERED_COSTMAP__COSTMAP_BASE_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/multi_layered_costmap.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/multi_layered_costmap.hpp new file mode 100644 index 00000000..395ec450 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/include/multi_layered_costmap/multi_layered_costmap.hpp @@ -0,0 +1,37 @@ +// 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 MULTI_LAYERED_COSTMAP__MULTI_LAYERED_COSTMAP_HPP_ +#define MULTI_LAYERED_COSTMAP__MULTI_LAYERED_COSTMAP_HPP_ + +#include "multi_layered_costmap/costmap_base.hpp" + +#include + +namespace multi_layered_costmap +{ +class MultiLayeredCostmap +{ +public: + using SharedPtr = std::shared_ptr; + void add_costmap_layer(CostmapBase::SharedPtr costmap_layer); + bool is_ready(); + bool is_occupied(const geometry_msgs::msg::PointStamped & point); + +private: + std::vector costmap_layers_; +}; +} // namespace multi_layered_costmap + +#endif // MULTI_LAYERED_COSTMAP__MULTI_LAYERED_COSTMAP_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/package.xml new file mode 100644 index 00000000..d5ee5091 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/package.xml @@ -0,0 +1,18 @@ + + + + multi_layered_costmap + 0.0.0 + The multi_layered_costmap pkg + Akiro Harada + Apache License 2.0 + + ament_cmake_auto + + rclcpp + geometry_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/src/multi_layered_costmap.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/src/multi_layered_costmap.cpp new file mode 100644 index 00000000..10ed829d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/multi_layered_costmap/src/multi_layered_costmap.cpp @@ -0,0 +1,40 @@ +// 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 "multi_layered_costmap/multi_layered_costmap.hpp" + +namespace multi_layered_costmap +{ +void MultiLayeredCostmap::add_costmap_layer(CostmapBase::SharedPtr costmap_layer) +{ + costmap_layers_.push_back(costmap_layer); +} + +bool MultiLayeredCostmap::is_ready() +{ + for (const auto & costmap_layer : costmap_layers_) { + if (!costmap_layer->is_ready()) return false; + } + return true; +} + +bool MultiLayeredCostmap::is_occupied(const geometry_msgs::msg::PointStamped & point) +{ + for (const auto & costmap_layer : costmap_layers_) { + if (!costmap_layer->is_occupied(point)) continue; + return true; + } + return false; +} +} // namespace multi_layered_costmap diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/CMakeLists.txt new file mode 100644 index 00000000..db44765b --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(predicted_object_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(predicted_object_costmap SHARED + src/predicted_object_costmap.cpp +) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/include/predicted_object_costmap/predicted_object_costmap.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/include/predicted_object_costmap/predicted_object_costmap.hpp new file mode 100644 index 00000000..e2c161c7 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/include/predicted_object_costmap/predicted_object_costmap.hpp @@ -0,0 +1,65 @@ +// 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 PREDICTED_OBJECT_COSTMAP__PREDICTED_OBJECT_COSTMAP_HPP_ +#define PREDICTED_OBJECT_COSTMAP__PREDICTED_OBJECT_COSTMAP_HPP_ + +#include + +#include + +#include +#include + +#include + +namespace predicted_object_costmap +{ + +using PredictedObject = autoware_auto_perception_msgs::msg::PredictedObject; +using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; + +class PredictedObjectCostmap : public multi_layered_costmap::CostmapBase +{ +public: + using SharedPtr = std::shared_ptr; + explicit PredictedObjectCostmap(rclcpp::Node & node, const std::string & layer_namespace); + static multi_layered_costmap::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); + bool is_intersected(const PredictedObject & object, const geometry_msgs::msg::Point & point); + + void objects_callback(const PredictedObjects::SharedPtr msg); + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr objects_sub_; + + PredictedObjects::SharedPtr objects_; + double distance_threshold_; +}; +} // namespace predicted_object_costmap + +#endif // PREDICTED_OBJECT_COSTMAP__PREDICTED_OBJECT_COSTMAP_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/package.xml new file mode 100644 index 00000000..541840d1 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/package.xml @@ -0,0 +1,21 @@ + + + + predicted_object_costmap + 0.0.0 + The predicted_object_costmap pkg + Akiro Harada + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_perception_msgs + multi_layered_costmap + rclcpp + tf2_geometry_msgs + tf2_ros + + + ament_cmake + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/src/predicted_object_costmap.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/src/predicted_object_costmap.cpp new file mode 100644 index 00000000..04c01ded --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/predicted_object_costmap/src/predicted_object_costmap.cpp @@ -0,0 +1,95 @@ +// 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 "predicted_object_costmap/predicted_object_costmap.hpp" + +#include + +#include + +namespace predicted_object_costmap +{ +PredictedObjectCostmap::PredictedObjectCostmap( + rclcpp::Node & node, const std::string & layer_namespace) +: tf_buffer_(node.get_clock()), tf_listener_(tf_buffer_) +{ + std::string objects_topic = + node.declare_parameter(layer_namespace + ".predicted_objects_topic", "~/input/objects"); + objects_sub_ = node.create_subscription( + objects_topic, 1, + std::bind(&PredictedObjectCostmap::objects_callback, this, std::placeholders::_1)); + + distance_threshold_ = node.declare_parameter(layer_namespace + ".distance_threshold", 0.0); +} + +bool PredictedObjectCostmap::is_ready() +{ + return objects_ != nullptr; +} + +bool PredictedObjectCostmap::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, objects_->header.frame_id)) { + return true; + } + + for (const auto & object : objects_->objects) { + if (!is_intersected(object, transformed_point)) continue; + return true; + } + return false; +} + +bool PredictedObjectCostmap::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; +} + +bool PredictedObjectCostmap::is_intersected( + const PredictedObject & object, const geometry_msgs::msg::Point & point) +{ + if (object.shape.type != object.shape.CYLINDER) return false; + + const geometry_msgs::msg::Point center = + object.kinematics.initial_pose_with_covariance.pose.position; + const double radius = + std::max(object.shape.dimensions.x, object.shape.dimensions.y) * 0.5 + distance_threshold_; + const double sqr_radius = radius * radius; + + const double dx = point.x - center.x; + const double dy = point.y - center.y; + const double sqr_distance = dx * dx + dy * dy; + + return sqr_distance < sqr_radius; +} + +void PredictedObjectCostmap::objects_callback(const PredictedObjects::SharedPtr msg) +{ + objects_ = msg; +} + +} // namespace predicted_object_costmap