From a04db19c664827f580f881b4ab1f7f8a3258e873 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 12:31:37 +0900 Subject: [PATCH 01/21] create pkg directory Signed-off-by: Autumn60 --- .../costmap_generator/CMakeLists.txt | 20 +++++++++++++++++ .../costmap_generator/package.xml | 22 +++++++++++++++++++ 2 files changed, 42 insertions(+) create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt new file mode 100644 index 00000000..ede007ad --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.8) +project(goal_pose_setter) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +# ament_auto_add_executable(goal_pose_setter_node +# src/goal_pose_setter_node.cpp +# ) + +# ament_auto_package( +# INSTALL_TO_SHARE +# config +# ) \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml new file mode 100644 index 00000000..6a42e834 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml @@ -0,0 +1,22 @@ + + + + costmap_generator + 0.0.0 + The costmap_generator pkg + Akiro Harada + TODO: License declaration + + ament_cmake_auto + + + + + ament_cmake + + From 733d519d72836598cb3b5e24c7ea15b9bae5922a Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 12:44:54 +0900 Subject: [PATCH 02/21] update .gitignore Signed-off-by: Autumn60 --- aichallenge/workspace/.gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/aichallenge/workspace/.gitignore b/aichallenge/workspace/.gitignore index 4c288e3e..ab2524be 100644 --- a/aichallenge/workspace/.gitignore +++ b/aichallenge/workspace/.gitignore @@ -1,3 +1,6 @@ /build /install /log + +.vscode +*.code-workspace \ No newline at end of file From 9e85fbd958e1437e1e5f8e61b19a8b48fd95e825 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 12:45:22 +0900 Subject: [PATCH 03/21] add .clang-format Signed-off-by: Autumn60 --- aichallenge/workspace/.clang-format | 47 +++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 aichallenge/workspace/.clang-format diff --git a/aichallenge/workspace/.clang-format b/aichallenge/workspace/.clang-format new file mode 100644 index 00000000..b41fae91 --- /dev/null +++ b/aichallenge/workspace/.clang-format @@ -0,0 +1,47 @@ +# Modified from https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +AllowShortFunctionsOnASingleLine: InlineOnly +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: true +IncludeCategories: + # C++ system headers + - Regex: <[a-z_]*> + Priority: 6 + CaseSensitive: true + # C system headers + - Regex: <.*\.h> + Priority: 5 + CaseSensitive: true + # Boost headers + - Regex: boost/.* + Priority: 4 + CaseSensitive: true + # Message headers + - Regex: .*_msgs/.* + Priority: 3 + CaseSensitive: true + - Regex: .*_srvs/.* + Priority: 3 + CaseSensitive: true + # Other Package headers + - Regex: <.*> + Priority: 2 + CaseSensitive: true + # Local package headers + - Regex: '".*"' + Priority: 1 + CaseSensitive: true From a0381943704baf42f63b2b2f5a68a133c0ef6d4d Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 13:09:18 +0900 Subject: [PATCH 04/21] implement map subscription Signed-off-by: Autumn60 --- .../costmap_generator/CMakeLists.txt | 22 ++++++---- .../costmap_generator/config/.gitkeep | 0 .../global_costmap_generator.hpp | 41 +++++++++++++++++++ .../costmap_generator/launch/.gitkeep | 0 .../costmap_generator/package.xml | 7 +--- .../src/global_costmap_generator.cpp | 35 ++++++++++++++++ 6 files changed, 92 insertions(+), 13 deletions(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/.gitkeep create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/.gitkeep create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt index ede007ad..7a374285 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(goal_pose_setter) +project(costmap_generator) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -10,11 +10,17 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -# ament_auto_add_executable(goal_pose_setter_node -# src/goal_pose_setter_node.cpp -# ) +ament_auto_add_library(costmap_generator SHARED + src/global_costmap_generator.cpp +) -# ament_auto_package( -# INSTALL_TO_SHARE -# config -# ) \ No newline at end of file +rclcpp_components_register_node(costmap_generator + PLUGIN "costmap_generator::GlobalCostmapGenerator" + EXECUTABLE global_costmap_generator_node +) + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/.gitkeep b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp new file mode 100644 index 00000000..264f4099 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp @@ -0,0 +1,41 @@ +// Copyright 2024 Fool Stuck Engineers +// +// 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__GLOBAL_COSTMAP_GENERATOR_HPP_ +#define COSTMAP_GENERATOR__GLOBAL_COSTMAP_GENERATOR_HPP_ + +#include + +#include + +namespace costmap_generator +{ + +class GlobalCostmapGenerator : public rclcpp::Node +{ + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBinSubscription = rclcpp::Subscription; + +public: + explicit GlobalCostmapGenerator(const rclcpp::NodeOptions & options); + +private: + void map_callback(const HADMapBin::SharedPtr msg); + + HADMapBinSubscription::SharedPtr had_map_bin_sub_; + HADMapBin::SharedPtr map_; +}; +} // namespace costmap_generator + +#endif // COSTMAP_GENERATOR__GLOBAL_COSTMAP_GENERATOR_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/.gitkeep b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml index 6a42e834..79de08a4 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml @@ -9,12 +9,9 @@ ament_cmake_auto - + rclcpp_components ament_cmake diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp new file mode 100644 index 00000000..a9864272 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp @@ -0,0 +1,35 @@ +// Copyright 2024 Fool Stuck Engineers +// +// 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/global_costmap_generator.hpp" + +namespace costmap_generator +{ +GlobalCostmapGenerator::GlobalCostmapGenerator(const rclcpp::NodeOptions & options) +: Node("global_costmap_generator", options) +{ + had_map_bin_sub_ = this->create_subscription( + "~/input/map", 1, + std::bind(&GlobalCostmapGenerator::map_callback, this, std::placeholders::_1)); +} + +void GlobalCostmapGenerator::map_callback(const HADMapBin::SharedPtr msg) +{ + map_ = msg; +} + +} // namespace costmap_generator + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(costmap_generator::GlobalCostmapGenerator) From c2443489738351aede4a25a7c6199a14f3f58532 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 13:32:08 +0900 Subject: [PATCH 05/21] create pkg directory Signed-off-by: Autumn60 --- .../booars_utils/CMakeLists.txt | 19 ++++++++++++++++++ .../booars_utils/package.xml | 20 +++++++++++++++++++ 2 files changed, 39 insertions(+) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_utils/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/booars_utils/CMakeLists.txt new file mode 100644 index 00000000..693dcc12 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(booars_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() + +include_directories(include) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml b/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml new file mode 100644 index 00000000..5bb22bbe --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/package.xml @@ -0,0 +1,20 @@ + + + + booars_utils + 0.1.0 + The booars_utils package + Akiro Harada + + Apache 2 + + ament_cmake_auto + + rclcpp + + ament_lint_auto + + + ament_cmake + + From 0f914b618779a03f8d414f995449e709c2e435aa Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 13:32:28 +0900 Subject: [PATCH 06/21] create function_timer.hpp Signed-off-by: Autumn60 --- .../booars_utils/ros/function_timer.hpp | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/ros/function_timer.hpp diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/ros/function_timer.hpp b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/ros/function_timer.hpp new file mode 100644 index 00000000..56935e23 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/ros/function_timer.hpp @@ -0,0 +1,52 @@ +// 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__ROS__FUNCTION_TIMER_HPP_ +#define BOOARS_UTILS__ROS__FUNCTION_TIMER_HPP_ + +#include +#include +#include + +namespace booars_utils::ros { + +class FunctionTimer { + private: + rclcpp::TimerBase::SharedPtr timer_; + + public: + using SharedPtr = std::shared_ptr; + + static SharedPtr create_function_timer(rclcpp::Node* node, + const double update_rate_hz, + std::function callback) { + return std::make_shared(node, update_rate_hz, callback); + } + + explicit FunctionTimer(rclcpp::Node* node, const double update_rate_hz, + std::function callback) { + const double dt = 1.0 / update_rate_hz; + + auto period = std::chrono::duration_cast( + std::chrono::duration(dt)); + timer_ = std::make_shared>( + node->get_clock(), period, std::move(callback), + node->get_node_base_interface()->get_context()); + node->get_node_timers_interface()->add_timer(timer_, nullptr); + } +}; + +} // namespace booars_utils::ros + +#endif // BOOARS_UTILS__ROS__FUNCTION_TIMER_HPP_ From 6e95ffe05bed3ad84dcc57987ffbb6341dff99c0 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 13:48:37 +0900 Subject: [PATCH 07/21] create timer driven function Signed-off-by: Autumn60 --- .../global_costmap_generator.hpp | 5 ++++ .../costmap_generator/package.xml | 1 + .../src/global_costmap_generator.cpp | 29 +++++++++++++++++-- 3 files changed, 32 insertions(+), 3 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp index 264f4099..8b25e42a 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp @@ -15,6 +15,7 @@ #ifndef COSTMAP_GENERATOR__GLOBAL_COSTMAP_GENERATOR_HPP_ #define COSTMAP_GENERATOR__GLOBAL_COSTMAP_GENERATOR_HPP_ +#include #include #include @@ -24,6 +25,7 @@ namespace costmap_generator class GlobalCostmapGenerator : public rclcpp::Node { + using FunctionTimer = booars_utils::ros::FunctionTimer; using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using HADMapBinSubscription = rclcpp::Subscription; @@ -31,8 +33,11 @@ class GlobalCostmapGenerator : public rclcpp::Node explicit GlobalCostmapGenerator(const rclcpp::NodeOptions & options); private: + void update(); void map_callback(const HADMapBin::SharedPtr msg); + FunctionTimer::SharedPtr update_timer_; + HADMapBinSubscription::SharedPtr had_map_bin_sub_; HADMapBin::SharedPtr map_; }; diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml index 79de08a4..ee5ac60f 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_auto_mapping_msgs + booars_utils rclcpp rclcpp_components diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp index a9864272..e82b4f0b 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp @@ -19,9 +19,32 @@ namespace costmap_generator GlobalCostmapGenerator::GlobalCostmapGenerator(const rclcpp::NodeOptions & options) : Node("global_costmap_generator", options) { - had_map_bin_sub_ = this->create_subscription( - "~/input/map", 1, - std::bind(&GlobalCostmapGenerator::map_callback, this, std::placeholders::_1)); + // Declare parameters + double update_rate; + { + update_rate = this->declare_parameter("update_rate", 20.0); + } + + // Create subscriptions + { + had_map_bin_sub_ = this->create_subscription( + "~/input/map", 1, + std::bind(&GlobalCostmapGenerator::map_callback, this, std::placeholders::_1)); + } + + // Create function timers + { + update_timer_ = FunctionTimer::create_function_timer( + this, update_rate, std::bind(&GlobalCostmapGenerator::update, this)); + } +} + +void GlobalCostmapGenerator::update() +{ + if (!map_) { + RCLCPP_WARN(get_logger(), "No map received yet"); + return; + } } void GlobalCostmapGenerator::map_callback(const HADMapBin::SharedPtr msg) From 4ad269c8ec2fa146639c9813900259293a4f7050 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 13:51:49 +0900 Subject: [PATCH 08/21] add costmap publisher Signed-off-by: Autumn60 --- .../costmap_generator/global_costmap_generator.hpp | 7 ++++++- .../src/aichallenge_submit/costmap_generator/package.xml | 1 + .../costmap_generator/src/global_costmap_generator.cpp | 8 +++++++- 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp index 8b25e42a..a9840826 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp @@ -19,6 +19,7 @@ #include #include +#include namespace costmap_generator { @@ -28,6 +29,8 @@ class GlobalCostmapGenerator : public rclcpp::Node using FunctionTimer = booars_utils::ros::FunctionTimer; using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using HADMapBinSubscription = rclcpp::Subscription; + using OccupancyGrid = nav_msgs::msg::OccupancyGrid; + using OccupancyGridPublisher = rclcpp::Publisher; public: explicit GlobalCostmapGenerator(const rclcpp::NodeOptions & options); @@ -38,8 +41,10 @@ class GlobalCostmapGenerator : public rclcpp::Node FunctionTimer::SharedPtr update_timer_; - HADMapBinSubscription::SharedPtr had_map_bin_sub_; + HADMapBinSubscription::SharedPtr map_sub_; HADMapBin::SharedPtr map_; + + OccupancyGridPublisher::SharedPtr costmap_pub_; }; } // namespace costmap_generator diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml index ee5ac60f..4cdc9968 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml @@ -11,6 +11,7 @@ autoware_auto_mapping_msgs booars_utils + nav_msgs rclcpp rclcpp_components diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp index e82b4f0b..2de68d04 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp @@ -27,11 +27,16 @@ GlobalCostmapGenerator::GlobalCostmapGenerator(const rclcpp::NodeOptions & optio // Create subscriptions { - had_map_bin_sub_ = this->create_subscription( + map_sub_ = this->create_subscription( "~/input/map", 1, std::bind(&GlobalCostmapGenerator::map_callback, this, std::placeholders::_1)); } + // Create publishers + { + costmap_pub_ = this->create_publisher("~/output/costmap", 1); + } + // Create function timers { update_timer_ = FunctionTimer::create_function_timer( @@ -41,6 +46,7 @@ GlobalCostmapGenerator::GlobalCostmapGenerator(const rclcpp::NodeOptions & optio void GlobalCostmapGenerator::update() { + // Check if map is available if (!map_) { RCLCPP_WARN(get_logger(), "No map received yet"); return; From 8b0089cb4449d357cf95b83c63dfe9e6dad8fcc0 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 14:03:25 +0900 Subject: [PATCH 09/21] create launch and config Signed-off-by: Autumn60 --- .../costmap_generator/config/.gitkeep | 0 .../global_costmap_generator.param.yaml | 3 +++ .../costmap_generator/launch/.gitkeep | 0 .../launch/costmap_generator.launch.xml | 6 ++++++ .../global_costmap_generator.launch.xml | 12 ++++++++++++ .../aichallenge_submit/costmap_generator/package.xml | 2 +- 6 files changed, 22 insertions(+), 1 deletion(-) delete mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/.gitkeep create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml delete mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/.gitkeep create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator.launch.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator/global_costmap_generator.launch.xml diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/.gitkeep b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml new file mode 100644 index 00000000..ae77ec56 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + update_rate: 20.0 # Hz \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/.gitkeep b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator.launch.xml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator.launch.xml new file mode 100644 index 00000000..a25b910d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator/global_costmap_generator.launch.xml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator/global_costmap_generator.launch.xml new file mode 100644 index 00000000..65989621 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator/global_costmap_generator.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml index 4cdc9968..61443a29 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_auto_mapping_msgs - booars_utils + booars_utils nav_msgs rclcpp rclcpp_components From 7c1553a642468338dba7b50a20bfdc4d0d78574e Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 20:33:26 +0900 Subject: [PATCH 10/21] add lanelet conversion Signed-off-by: Autumn60 --- .../costmap_generator/global_costmap_generator.hpp | 4 +++- .../src/aichallenge_submit/costmap_generator/package.xml | 1 + .../costmap_generator/src/global_costmap_generator.cpp | 8 ++++++-- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp index a9840826..337ba6d0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp @@ -21,6 +21,8 @@ #include #include +#include + namespace costmap_generator { @@ -42,7 +44,7 @@ class GlobalCostmapGenerator : public rclcpp::Node FunctionTimer::SharedPtr update_timer_; HADMapBinSubscription::SharedPtr map_sub_; - HADMapBin::SharedPtr map_; + lanelet::LaneletMapPtr map_; OccupancyGridPublisher::SharedPtr costmap_pub_; }; diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml index 61443a29..bdd457f1 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_auto_mapping_msgs + lanelet2_extension booars_utils nav_msgs rclcpp diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp index 2de68d04..654d298f 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp @@ -14,6 +14,8 @@ #include "costmap_generator/global_costmap_generator.hpp" +#include + namespace costmap_generator { GlobalCostmapGenerator::GlobalCostmapGenerator(const rclcpp::NodeOptions & options) @@ -48,14 +50,16 @@ void GlobalCostmapGenerator::update() { // Check if map is available if (!map_) { - RCLCPP_WARN(get_logger(), "No map received yet"); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "No map received yet"); return; } } void GlobalCostmapGenerator::map_callback(const HADMapBin::SharedPtr msg) { - map_ = msg; + RCLCPP_INFO(get_logger(), "Received map"); + map_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*msg, map_); } } // namespace costmap_generator From b4ca7b4e11c77071329f0137d11d20d8b3af5a59 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 23:21:06 +0900 Subject: [PATCH 11/21] create costmap_parameters.hpp Signed-off-by: Autumn60 --- .../costmap_generator/costmap_parameters.hpp | 54 +++++++++++++++++++ 1 file changed, 54 insertions(+) create mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp new file mode 100644 index 00000000..04bcdc79 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 Fool Stuck Engineers +// +// 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__COSTMAP_PARAMETERS_HPP_ +#define COSTMAP_GENERATOR__COSTMAP_PARAMETERS_HPP_ + +#include + +namespace costmap_generator +{ +class CostmapParameters +{ +public: + using SharedPtr = std::shared_ptr; + + static CostmapParameters::SharedPtr create_parameters(const double width, const double resolution) + { + return std::make_shared(width, resolution); + } + + explicit CostmapParameters(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_; + } + +private: + double width_; + double width_2_; + double resolution_; + double resolution_inv_; + int grid_width_2_; + int grid_width_; + int grid_num_; +}; +} // namespace costmap_generator + +#endif // COSTMAP_GENERATOR__COSTMAP_PARAMETERS_HPP_ From e12496057c22b1ab98dfbf318d2ffa94a528f9c9 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 23:35:45 +0900 Subject: [PATCH 12/21] add getter funcs to CostmapParameters Signed-off-by: Autumn60 --- .../include/costmap_generator/costmap_parameters.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp index 04bcdc79..17c5e669 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp @@ -40,6 +40,14 @@ class CostmapParameters 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_; From 1ab97c98f39bd5a66b8b86f1bb9842d12d7ce74f Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sun, 11 Aug 2024 23:43:54 +0900 Subject: [PATCH 13/21] add intersected lanelet extraction Signed-off-by: Autumn60 --- .../global_costmap_generator.hpp | 18 ++++++ .../costmap_generator/package.xml | 1 + .../src/global_costmap_generator.cpp | 61 ++++++++++++++++++- 3 files changed, 78 insertions(+), 2 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp index 337ba6d0..b7449c67 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp @@ -15,13 +15,20 @@ #ifndef COSTMAP_GENERATOR__GLOBAL_COSTMAP_GENERATOR_HPP_ #define COSTMAP_GENERATOR__GLOBAL_COSTMAP_GENERATOR_HPP_ +#include "costmap_generator/costmap_parameters.hpp" + #include #include +#include #include #include #include +#include +#include + +#include namespace costmap_generator { @@ -31,6 +38,7 @@ class GlobalCostmapGenerator : public rclcpp::Node using FunctionTimer = booars_utils::ros::FunctionTimer; using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using HADMapBinSubscription = rclcpp::Subscription; + using LinearRing2d = tier4_autoware_utils::LinearRing2d; using OccupancyGrid = nav_msgs::msg::OccupancyGrid; using OccupancyGridPublisher = rclcpp::Publisher; @@ -39,14 +47,24 @@ class GlobalCostmapGenerator : public rclcpp::Node private: void update(); + lanelet::ConstLanelets get_intersected_lanelets(const geometry_msgs::msg::Vector3 & center); + LinearRing2d get_costmap_contour(const geometry_msgs::msg::Vector3 & center); + void map_callback(const HADMapBin::SharedPtr msg); FunctionTimer::SharedPtr update_timer_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + HADMapBinSubscription::SharedPtr map_sub_; lanelet::LaneletMapPtr map_; + lanelet::ConstLanelets roads_; OccupancyGridPublisher::SharedPtr costmap_pub_; + + std::string costmap_center_frame_id_; + CostmapParameters::SharedPtr costmap_parameters_; }; } // namespace costmap_generator diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml index bdd457f1..d5132030 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/package.xml @@ -15,6 +15,7 @@ nav_msgs rclcpp rclcpp_components + tier4_autoware_utils ament_cmake diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp index 654d298f..080185b7 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp @@ -15,16 +15,26 @@ #include "costmap_generator/global_costmap_generator.hpp" #include +#include + +#include + +#include namespace costmap_generator { GlobalCostmapGenerator::GlobalCostmapGenerator(const rclcpp::NodeOptions & options) -: Node("global_costmap_generator", options) +: Node("global_costmap_generator", options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) { // Declare parameters double update_rate; { update_rate = this->declare_parameter("update_rate", 20.0); + costmap_center_frame_id_ = this->declare_parameter("costmap_frame_id", "base_link"); + + double costmap_width = this->declare_parameter("costmap_width", 20.0); + double costmap_resolution = this->declare_parameter("costmap_resolution", 0.1); + costmap_parameters_ = CostmapParameters::create_parameters(costmap_width, costmap_resolution); } // Create subscriptions @@ -53,13 +63,60 @@ void GlobalCostmapGenerator::update() RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "No map received yet"); return; } + + // Get the transform from the map frame to the costmap frame + geometry_msgs::msg::Vector3 costmap_center_position; + { + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_.lookupTransform(costmap_center_frame_id_, "map", tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "Could not get transform: %s", ex.what()); + return; + } + costmap_center_position = transform.transform.translation; + } + + // Get the intersected lanelets + lanelet::ConstLanelets intersected_lanelets = get_intersected_lanelets(costmap_center_position); +} + +lanelet::ConstLanelets GlobalCostmapGenerator::get_intersected_lanelets( + const geometry_msgs::msg::Vector3 & center) +{ + LinearRing2d costmap_contour = get_costmap_contour(center); + lanelet::ConstLanelets intersected_lanelets; + for (const auto & road : roads_) { + if (boost::geometry::intersects(costmap_contour, road.polygon2d().basicPolygon())) { + intersected_lanelets.push_back(road); + } + } + return intersected_lanelets; +} + +tier4_autoware_utils::LinearRing2d GlobalCostmapGenerator::get_costmap_contour( + const geometry_msgs::msg::Vector3 & center) +{ + LinearRing2d costmap_contour; + costmap_contour.push_back( + {center.x - costmap_parameters_->width_2(), center.y - costmap_parameters_->width_2()}); + costmap_contour.push_back( + {center.x + costmap_parameters_->width_2(), center.y - costmap_parameters_->width_2()}); + costmap_contour.push_back( + {center.x + costmap_parameters_->width_2(), center.y + costmap_parameters_->width_2()}); + costmap_contour.push_back( + {center.x - costmap_parameters_->width_2(), center.y + costmap_parameters_->width_2()}); + costmap_contour.push_back( + {center.x - costmap_parameters_->width_2(), center.y - costmap_parameters_->width_2()}); + return costmap_contour; } void GlobalCostmapGenerator::map_callback(const HADMapBin::SharedPtr msg) { - RCLCPP_INFO(get_logger(), "Received map"); 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 costmap_generator From dc97c5c4e2a720ff318e2d9fdc54f3d43a319305 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Mon, 12 Aug 2024 00:49:52 +0900 Subject: [PATCH 14/21] implement costmap calculation Signed-off-by: Autumn60 --- .../global_costmap_generator.param.yaml | 5 +- .../global_costmap_generator.hpp | 7 ++- .../src/global_costmap_generator.cpp | 60 ++++++++++++++++--- 3 files changed, 62 insertions(+), 10 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml index ae77ec56..dbfe38df 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml @@ -1,3 +1,6 @@ /**: ros__parameters: - update_rate: 20.0 # Hz \ No newline at end of file + update_rate: 20.0 # Hz + costmap_center_frame_id: "base_link" + costmap_width: 20.0 # m + costmap_resolution: 0.2 # m/cell diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp index b7449c67..5b15d1d5 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp @@ -41,14 +41,17 @@ class GlobalCostmapGenerator : public rclcpp::Node using LinearRing2d = tier4_autoware_utils::LinearRing2d; using OccupancyGrid = nav_msgs::msg::OccupancyGrid; using OccupancyGridPublisher = rclcpp::Publisher; + using Point2d = tier4_autoware_utils::Point2d; + using Vector3 = geometry_msgs::msg::Vector3; public: explicit GlobalCostmapGenerator(const rclcpp::NodeOptions & options); private: void update(); - lanelet::ConstLanelets get_intersected_lanelets(const geometry_msgs::msg::Vector3 & center); - LinearRing2d get_costmap_contour(const geometry_msgs::msg::Vector3 & center); + lanelet::ConstLanelets get_intersected_lanelets(const Vector3 & center); + LinearRing2d get_costmap_contour(const Vector3 & center); + Point2d get_cell_position(const Vector3 & costmap_origin, const int & index); void map_callback(const HADMapBin::SharedPtr msg); diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp index 080185b7..0b40a94c 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp @@ -30,7 +30,7 @@ GlobalCostmapGenerator::GlobalCostmapGenerator(const rclcpp::NodeOptions & optio double update_rate; { update_rate = this->declare_parameter("update_rate", 20.0); - costmap_center_frame_id_ = this->declare_parameter("costmap_frame_id", "base_link"); + costmap_center_frame_id_ = this->declare_parameter("costmap_center_frame_id", "base_link"); double costmap_width = this->declare_parameter("costmap_width", 20.0); double costmap_resolution = this->declare_parameter("costmap_resolution", 0.1); @@ -65,13 +65,14 @@ void GlobalCostmapGenerator::update() } // Get the transform from the map frame to the costmap frame - geometry_msgs::msg::Vector3 costmap_center_position; + Vector3 costmap_center_position; { geometry_msgs::msg::TransformStamped transform; try { - transform = tf_buffer_.lookupTransform(costmap_center_frame_id_, "map", tf2::TimePointZero); + transform = tf_buffer_.lookupTransform("map", costmap_center_frame_id_, tf2::TimePointZero); } catch (tf2::TransformException & ex) { - RCLCPP_WARN(get_logger(), "Could not get transform: %s", ex.what()); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "Could not get transform %s", ex.what()); return; } costmap_center_position = transform.transform.translation; @@ -79,10 +80,45 @@ void GlobalCostmapGenerator::update() // Get the intersected lanelets lanelet::ConstLanelets intersected_lanelets = get_intersected_lanelets(costmap_center_position); + + // Create the costmap + OccupancyGrid costmap; + { + costmap.info.width = costmap_parameters_->grid_width(); + costmap.info.height = costmap_parameters_->grid_width(); + costmap.info.resolution = costmap_parameters_->resolution(); + costmap.info.origin.position.x = costmap_center_position.x - costmap_parameters_->width_2(); + costmap.info.origin.position.y = costmap_center_position.y - costmap_parameters_->width_2(); + costmap.info.origin.position.z = 0.0; + costmap.info.origin.orientation.x = 0.0; + costmap.info.origin.orientation.y = 0.0; + costmap.info.origin.orientation.z = 0.0; + costmap.info.origin.orientation.w = 1.0; + } + + // Fill the costmap + { + costmap.data.resize(costmap_parameters_->grid_num(), 100); + for (int i = 0; i < costmap_parameters_->grid_num(); i++) { + Point2d cell_position = get_cell_position(costmap_center_position, i); + for (const auto & lanelet : intersected_lanelets) { + if (boost::geometry::within(cell_position, lanelet.polygon2d().basicPolygon())) { + costmap.data[i] = 0; + break; + } + } + } + } + + // Publish the costmap + { + costmap.header.stamp = now(); + costmap.header.frame_id = "map"; + costmap_pub_->publish(costmap); + } } -lanelet::ConstLanelets GlobalCostmapGenerator::get_intersected_lanelets( - const geometry_msgs::msg::Vector3 & center) +lanelet::ConstLanelets GlobalCostmapGenerator::get_intersected_lanelets(const Vector3 & center) { LinearRing2d costmap_contour = get_costmap_contour(center); lanelet::ConstLanelets intersected_lanelets; @@ -95,7 +131,7 @@ lanelet::ConstLanelets GlobalCostmapGenerator::get_intersected_lanelets( } tier4_autoware_utils::LinearRing2d GlobalCostmapGenerator::get_costmap_contour( - const geometry_msgs::msg::Vector3 & center) + const Vector3 & center) { LinearRing2d costmap_contour; costmap_contour.push_back( @@ -111,6 +147,16 @@ tier4_autoware_utils::LinearRing2d GlobalCostmapGenerator::get_costmap_contour( return costmap_contour; } +tier4_autoware_utils::Point2d GlobalCostmapGenerator::get_cell_position( + const Vector3 & costmap_origin, const int & index) +{ + const int x = index % costmap_parameters_->grid_width(); + const int y = index / costmap_parameters_->grid_width(); + return { + costmap_origin.x + x * costmap_parameters_->resolution() - costmap_parameters_->width_2(), + costmap_origin.y + y * costmap_parameters_->resolution() - costmap_parameters_->width_2()}; +} + void GlobalCostmapGenerator::map_callback(const HADMapBin::SharedPtr msg) { map_ = std::make_shared(); From b6809d79de3df87862813717ccf2677fe87dbc38 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 21 Aug 2024 18:49:34 +0900 Subject: [PATCH 15/21] create costmap tf frame Signed-off-by: Autumn60 --- .../global_costmap_generator.param.yaml | 3 +- .../global_costmap_generator.hpp | 5 ++- .../src/global_costmap_generator.cpp | 39 ++++++++++++------- 3 files changed, 31 insertions(+), 16 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml index dbfe38df..93e74b99 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: update_rate: 20.0 # Hz - costmap_center_frame_id: "base_link" + costmap_target_frame_id: "base_link" + costmap_frame_id: "costmap" costmap_width: 20.0 # m costmap_resolution: 0.2 # m/cell diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp index 5b15d1d5..bc3824d9 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -59,6 +60,7 @@ class GlobalCostmapGenerator : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; + tf2_ros::TransformBroadcaster tf_broadcaster_; HADMapBinSubscription::SharedPtr map_sub_; lanelet::LaneletMapPtr map_; @@ -66,7 +68,8 @@ class GlobalCostmapGenerator : public rclcpp::Node OccupancyGridPublisher::SharedPtr costmap_pub_; - std::string costmap_center_frame_id_; + std::string costmap_target_frame_id_; + std::string costmap_frame_id_; CostmapParameters::SharedPtr costmap_parameters_; }; } // namespace costmap_generator diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp index 0b40a94c..a20885a4 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp @@ -24,13 +24,17 @@ namespace costmap_generator { GlobalCostmapGenerator::GlobalCostmapGenerator(const rclcpp::NodeOptions & options) -: Node("global_costmap_generator", options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) +: Node("global_costmap_generator", options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_), + tf_broadcaster_(this) { // Declare parameters double update_rate; { update_rate = this->declare_parameter("update_rate", 20.0); - costmap_center_frame_id_ = this->declare_parameter("costmap_center_frame_id", "base_link"); + costmap_target_frame_id_ = this->declare_parameter("costmap_target_frame_id", "base_link"); + costmap_frame_id_ = this->declare_parameter("costmap_frame_id", "costmap"); double costmap_width = this->declare_parameter("costmap_width", 20.0); double costmap_resolution = this->declare_parameter("costmap_resolution", 0.1); @@ -64,20 +68,19 @@ void GlobalCostmapGenerator::update() return; } - // Get the transform from the map frame to the costmap frame - Vector3 costmap_center_position; - { - geometry_msgs::msg::TransformStamped transform; - try { - transform = tf_buffer_.lookupTransform("map", costmap_center_frame_id_, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 1000, "Could not get transform %s", ex.what()); - return; - } - costmap_center_position = transform.transform.translation; + // Get the transform from the map frame to the costmap target frame + geometry_msgs::msg::TransformStamped target_transform; + try { + target_transform = + tf_buffer_.lookupTransform("map", costmap_target_frame_id_, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Could not get transform %s", ex.what()); + return; } + // Cache the current position of the costmap center + Vector3 costmap_center_position = target_transform.transform.translation; + // Get the intersected lanelets lanelet::ConstLanelets intersected_lanelets = get_intersected_lanelets(costmap_center_position); @@ -110,6 +113,14 @@ void GlobalCostmapGenerator::update() } } + // Publish the costmap TF + { + geometry_msgs::msg::TransformStamped costmap_transform = target_transform; + costmap_transform.header.stamp = now(); + costmap_transform.child_frame_id = costmap_frame_id_; + tf_broadcaster_.sendTransform(costmap_transform); + } + // Publish the costmap { costmap.header.stamp = now(); From d0ec088ac41515f8e73d65c8f1d31cdbfa7fbad2 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 21 Aug 2024 19:31:56 +0900 Subject: [PATCH 16/21] create occupancy_grid utils Signed-off-by: Autumn60 --- .../nav/occupancy_grid_parameters.hpp | 64 +++++++++++++++++++ .../booars_utils/nav/occupancy_grid_utils.hpp | 61 ++++++++++++++++++ .../booars_utils/package.xml | 2 + 3 files changed, 127 insertions(+) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_parameters.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp 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..eef6e32f --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_utils/include/booars_utils/nav/occupancy_grid_utils.hpp @@ -0,0 +1,61 @@ +// 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; +} + +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()}; +} + +} // 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 From ce6856efd0bed0c6d21484c02222fd98b5eddb55 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 21 Aug 2024 19:40:46 +0900 Subject: [PATCH 17/21] replace CostmapParameters to OccupancyGridParameters Signed-off-by: Autumn60 --- .../costmap_generator/costmap_parameters.hpp | 62 ------------------- .../global_costmap_generator.hpp | 3 +- 2 files changed, 2 insertions(+), 63 deletions(-) delete mode 100644 aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp deleted file mode 100644 index 17c5e669..00000000 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/costmap_parameters.hpp +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright 2024 Fool Stuck Engineers -// -// 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__COSTMAP_PARAMETERS_HPP_ -#define COSTMAP_GENERATOR__COSTMAP_PARAMETERS_HPP_ - -#include - -namespace costmap_generator -{ -class CostmapParameters -{ -public: - using SharedPtr = std::shared_ptr; - - static CostmapParameters::SharedPtr create_parameters(const double width, const double resolution) - { - return std::make_shared(width, resolution); - } - - explicit CostmapParameters(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 costmap_generator - -#endif // COSTMAP_GENERATOR__COSTMAP_PARAMETERS_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp index bc3824d9..f2eb6e69 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp @@ -15,7 +15,7 @@ #ifndef COSTMAP_GENERATOR__GLOBAL_COSTMAP_GENERATOR_HPP_ #define COSTMAP_GENERATOR__GLOBAL_COSTMAP_GENERATOR_HPP_ -#include "costmap_generator/costmap_parameters.hpp" +#include "booars_utils/nav/occupancy_grid_parameters.hpp" #include #include @@ -44,6 +44,7 @@ class GlobalCostmapGenerator : public rclcpp::Node using OccupancyGridPublisher = rclcpp::Publisher; using Point2d = tier4_autoware_utils::Point2d; using Vector3 = geometry_msgs::msg::Vector3; + using CostmapParameters = booars_utils::nav::OccupancyGridParameters; public: explicit GlobalCostmapGenerator(const rclcpp::NodeOptions & options); From 140dcb42bc0497bd10bacd28e3c73aa5cafe6ae9 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 21 Aug 2024 23:08:49 +0900 Subject: [PATCH 18/21] add update_origin func to occupancy_grid_utils.hpp Signed-off-by: Autumn60 (cherry picked from commit 96000cbb96377321fee819f19033eea061c37c3c) --- .../booars_utils/nav/occupancy_grid_utils.hpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) 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 eef6e32f..dd1519d9 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 @@ -45,6 +45,19 @@ OccupancyGrid::SharedPtr create_occupancy_grid( 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) { From 849d020e107c61000fb0ed9feff6b88b2b5b91dc Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 21 Aug 2024 23:19:28 +0900 Subject: [PATCH 19/21] refactor with booars_utils Signed-off-by: Autumn60 --- .../costmap_generator/CMakeLists.txt | 4 +- .../global_costmap_generator.param.yaml | 4 +- .../global_costmap_generator.hpp | 17 ++- .../global_costmap_generator.launch.xml | 18 +-- .../src/global_costmap_generator.cpp | 110 +++++++++--------- 5 files changed, 74 insertions(+), 79 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt index 7a374285..85aa0f44 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/CMakeLists.txt @@ -10,11 +10,11 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(costmap_generator SHARED +ament_auto_add_library(global_costmap_generator SHARED src/global_costmap_generator.cpp ) -rclcpp_components_register_node(costmap_generator +rclcpp_components_register_node(global_costmap_generator PLUGIN "costmap_generator::GlobalCostmapGenerator" EXECUTABLE global_costmap_generator_node ) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml index 93e74b99..bf4b14d0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/config/costmap_generator/global_costmap_generator.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - update_rate: 20.0 # Hz + update_rate: 5.0 # Hz + map_frame_id: "map" costmap_target_frame_id: "base_link" - costmap_frame_id: "costmap" costmap_width: 20.0 # m costmap_resolution: 0.2 # m/cell diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp index f2eb6e69..286f873a 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/include/costmap_generator/global_costmap_generator.hpp @@ -26,7 +26,6 @@ #include #include -#include #include #include @@ -41,19 +40,19 @@ class GlobalCostmapGenerator : public rclcpp::Node using HADMapBinSubscription = rclcpp::Subscription; using LinearRing2d = tier4_autoware_utils::LinearRing2d; using OccupancyGrid = nav_msgs::msg::OccupancyGrid; + using OccupancyGridParameters = booars_utils::nav::OccupancyGridParameters; using OccupancyGridPublisher = rclcpp::Publisher; using Point2d = tier4_autoware_utils::Point2d; using Vector3 = geometry_msgs::msg::Vector3; - using CostmapParameters = booars_utils::nav::OccupancyGridParameters; public: explicit GlobalCostmapGenerator(const rclcpp::NodeOptions & options); private: void update(); - lanelet::ConstLanelets get_intersected_lanelets(const Vector3 & center); - LinearRing2d get_costmap_contour(const Vector3 & center); - Point2d get_cell_position(const Vector3 & costmap_origin, const int & index); + lanelet::ConstLanelets get_intersected_lanelets(const Vector3 & costmap_center); + LinearRing2d get_costmap_contour(const Vector3 & costmap_center); + Point2d get_global_cell_position(const Vector3 & costmap_center, const int & index); void map_callback(const HADMapBin::SharedPtr msg); @@ -61,17 +60,17 @@ class GlobalCostmapGenerator : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - tf2_ros::TransformBroadcaster tf_broadcaster_; HADMapBinSubscription::SharedPtr map_sub_; lanelet::LaneletMapPtr map_; lanelet::ConstLanelets roads_; OccupancyGridPublisher::SharedPtr costmap_pub_; + OccupancyGrid::SharedPtr costmap_; + OccupancyGridParameters::SharedPtr costmap_parameters_; - std::string costmap_target_frame_id_; - std::string costmap_frame_id_; - CostmapParameters::SharedPtr costmap_parameters_; + std::string map_frame_; + std::string target_frame_; }; } // namespace costmap_generator diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator/global_costmap_generator.launch.xml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator/global_costmap_generator.launch.xml index 65989621..369bcc0d 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator/global_costmap_generator.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator/global_costmap_generator.launch.xml @@ -1,12 +1,14 @@ - - + + + - + - - - - - + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp index a20885a4..84c90c26 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/src/global_costmap_generator.cpp @@ -14,6 +14,7 @@ #include "costmap_generator/global_costmap_generator.hpp" +#include #include #include @@ -23,22 +24,26 @@ namespace costmap_generator { + GlobalCostmapGenerator::GlobalCostmapGenerator(const rclcpp::NodeOptions & options) -: Node("global_costmap_generator", options), - tf_buffer_(get_clock()), - tf_listener_(tf_buffer_), - tf_broadcaster_(this) +: Node("global_costmap_generator", options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) { // Declare parameters double update_rate; { update_rate = this->declare_parameter("update_rate", 20.0); - costmap_target_frame_id_ = this->declare_parameter("costmap_target_frame_id", "base_link"); - costmap_frame_id_ = this->declare_parameter("costmap_frame_id", "costmap"); + map_frame_ = this->declare_parameter("map_frame_id", "map"); + target_frame_ = this->declare_parameter("costmap_frame_id", "base_link"); + } + // Declare costmap parameters and setup costmap + { double costmap_width = this->declare_parameter("costmap_width", 20.0); double costmap_resolution = this->declare_parameter("costmap_resolution", 0.1); - costmap_parameters_ = CostmapParameters::create_parameters(costmap_width, costmap_resolution); + costmap_parameters_ = + OccupancyGridParameters::create_parameters(costmap_width, costmap_resolution); + costmap_ = booars_utils::nav::occupancy_grid_utils::create_occupancy_grid(costmap_parameters_); + costmap_->header.frame_id = map_frame_; } // Create subscriptions @@ -69,69 +74,55 @@ void GlobalCostmapGenerator::update() } // Get the transform from the map frame to the costmap target frame - geometry_msgs::msg::TransformStamped target_transform; - try { - target_transform = - tf_buffer_.lookupTransform("map", costmap_target_frame_id_, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Could not get transform %s", ex.what()); - return; + rclcpp::Time costmap_frame_time; + Vector3 costmap_center_position; + { + geometry_msgs::msg::TransformStamped target_transform; + try { + target_transform = tf_buffer_.lookupTransform(map_frame_, target_frame_, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "Could not get transform %s", ex.what()); + return; + } + costmap_frame_time = target_transform.header.stamp; + costmap_center_position = target_transform.transform.translation; } - // Cache the current position of the costmap center - Vector3 costmap_center_position = target_transform.transform.translation; - // Get the intersected lanelets lanelet::ConstLanelets intersected_lanelets = get_intersected_lanelets(costmap_center_position); - // Create the costmap - OccupancyGrid costmap; - { - costmap.info.width = costmap_parameters_->grid_width(); - costmap.info.height = costmap_parameters_->grid_width(); - costmap.info.resolution = costmap_parameters_->resolution(); - costmap.info.origin.position.x = costmap_center_position.x - costmap_parameters_->width_2(); - costmap.info.origin.position.y = costmap_center_position.y - costmap_parameters_->width_2(); - costmap.info.origin.position.z = 0.0; - costmap.info.origin.orientation.x = 0.0; - costmap.info.origin.orientation.y = 0.0; - costmap.info.origin.orientation.z = 0.0; - costmap.info.origin.orientation.w = 1.0; - } - // Fill the costmap { - costmap.data.resize(costmap_parameters_->grid_num(), 100); for (int i = 0; i < costmap_parameters_->grid_num(); i++) { - Point2d cell_position = get_cell_position(costmap_center_position, i); + Point2d cell_position = this->get_global_cell_position(costmap_center_position, i); for (const auto & lanelet : intersected_lanelets) { if (boost::geometry::within(cell_position, lanelet.polygon2d().basicPolygon())) { - costmap.data[i] = 0; + costmap_->data[i] = 0; break; } + costmap_->data[i] = 100; } } } - // Publish the costmap TF + // Update the costmap origin { - geometry_msgs::msg::TransformStamped costmap_transform = target_transform; - costmap_transform.header.stamp = now(); - costmap_transform.child_frame_id = costmap_frame_id_; - tf_broadcaster_.sendTransform(costmap_transform); + booars_utils::nav::occupancy_grid_utils::update_origin( + costmap_, costmap_parameters_, costmap_center_position); } // Publish the costmap { - costmap.header.stamp = now(); - costmap.header.frame_id = "map"; - costmap_pub_->publish(costmap); + costmap_->header.stamp = costmap_frame_time; + costmap_pub_->publish(*costmap_); } } -lanelet::ConstLanelets GlobalCostmapGenerator::get_intersected_lanelets(const Vector3 & center) +lanelet::ConstLanelets GlobalCostmapGenerator::get_intersected_lanelets( + const Vector3 & costmap_center) { - LinearRing2d costmap_contour = get_costmap_contour(center); + LinearRing2d costmap_contour = get_costmap_contour(costmap_center); lanelet::ConstLanelets intersected_lanelets; for (const auto & road : roads_) { if (boost::geometry::intersects(costmap_contour, road.polygon2d().basicPolygon())) { @@ -142,30 +133,33 @@ lanelet::ConstLanelets GlobalCostmapGenerator::get_intersected_lanelets(const Ve } tier4_autoware_utils::LinearRing2d GlobalCostmapGenerator::get_costmap_contour( - const Vector3 & center) + const Vector3 & costmap_center) { LinearRing2d costmap_contour; costmap_contour.push_back( - {center.x - costmap_parameters_->width_2(), center.y - costmap_parameters_->width_2()}); + {costmap_center.x - costmap_parameters_->width_2(), + costmap_center.y - costmap_parameters_->width_2()}); costmap_contour.push_back( - {center.x + costmap_parameters_->width_2(), center.y - costmap_parameters_->width_2()}); + {costmap_center.x + costmap_parameters_->width_2(), + costmap_center.y - costmap_parameters_->width_2()}); costmap_contour.push_back( - {center.x + costmap_parameters_->width_2(), center.y + costmap_parameters_->width_2()}); + {costmap_center.x + costmap_parameters_->width_2(), + costmap_center.y + costmap_parameters_->width_2()}); costmap_contour.push_back( - {center.x - costmap_parameters_->width_2(), center.y + costmap_parameters_->width_2()}); + {costmap_center.x - costmap_parameters_->width_2(), + costmap_center.y + costmap_parameters_->width_2()}); costmap_contour.push_back( - {center.x - costmap_parameters_->width_2(), center.y - costmap_parameters_->width_2()}); + {costmap_center.x - costmap_parameters_->width_2(), + costmap_center.y - costmap_parameters_->width_2()}); return costmap_contour; } -tier4_autoware_utils::Point2d GlobalCostmapGenerator::get_cell_position( - const Vector3 & costmap_origin, const int & index) +tier4_autoware_utils::Point2d GlobalCostmapGenerator::get_global_cell_position( + const Vector3 & costmap_center, const int & index) { - const int x = index % costmap_parameters_->grid_width(); - const int y = index / costmap_parameters_->grid_width(); - return { - costmap_origin.x + x * costmap_parameters_->resolution() - costmap_parameters_->width_2(), - costmap_origin.y + y * costmap_parameters_->resolution() - costmap_parameters_->width_2()}; + auto local_cell_position = + booars_utils::nav::occupancy_grid_utils::index_to_point(costmap_parameters_, index); + return {costmap_center.x + local_cell_position(0), costmap_center.y + local_cell_position(1)}; } void GlobalCostmapGenerator::map_callback(const HADMapBin::SharedPtr msg) From 718a1b2518f381bbc3529e5323306371ff2bb34c Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 21 Aug 2024 23:23:17 +0900 Subject: [PATCH 20/21] add config_file arg to costmap_generator.launch.xml Signed-off-by: Autumn60 --- .../costmap_generator/launch/costmap_generator.launch.xml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator.launch.xml b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator.launch.xml index a25b910d..a1ddc7cd 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap_generator/launch/costmap_generator.launch.xml @@ -1,6 +1,10 @@ + + - + + + From a43535113d43e414603bbc0051aa850c97b30a1b Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 21 Aug 2024 23:32:51 +0900 Subject: [PATCH 21/21] add CODEOWNERS Signed-off-by: Autumn60 --- .github/CODEOWNERS | 1 + 1 file changed, 1 insertion(+) create mode 100644 .github/CODEOWNERS diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 00000000..14353d6c --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1 @@ +aichallenge/workspace/src/aichallenge_submit/costmap_generator/** @Autumn60 \ No newline at end of file