From 87e8bd25dd0069c887031cd3b0358dd898d87e3e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 13 May 2024 17:13:13 +0000 Subject: [PATCH 1/7] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/igorshubovych/markdownlint-cli: v0.39.0 → v0.40.0](https://github.com/igorshubovych/markdownlint-cli/compare/v0.39.0...v0.40.0) - [github.com/psf/black: 24.4.0 → 24.4.2](https://github.com/psf/black/compare/24.4.0...24.4.2) - [github.com/pre-commit/mirrors-clang-format: v17.0.6 → v18.1.5](https://github.com/pre-commit/mirrors-clang-format/compare/v17.0.6...v18.1.5) --- .pre-commit-config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b4a3e6557..c5bc520b6 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -18,7 +18,7 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.39.0 + rev: v0.40.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] @@ -60,13 +60,13 @@ repos: # - id: isort - repo: https://github.com/psf/black - rev: 24.4.0 + rev: 24.4.2 hooks: - id: black args: [--line-length=99] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v17.0.6 + rev: v18.1.5 hooks: - id: clang-format types_or: [c++, c, cuda] From 9ca1ece1c631c4b72c7ab0f980672bce7052feb5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 13 May 2024 17:13:26 +0000 Subject: [PATCH 2/7] style(pre-commit): autofix --- utility/crane_msg_wrappers/src/play_situation_wrapper.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp b/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp index 6917772fa..a6f6d0002 100644 --- a/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp +++ b/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp @@ -14,10 +14,7 @@ namespace crane { -#define CMD_STRING_MAPPING(TYPE, CMD) \ - { \ - TYPE::CMD, #CMD \ - } +#define CMD_STRING_MAPPING(TYPE, CMD) {TYPE::CMD, #CMD} static std::map referee_command_map = { CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, COMMAND_HALT), From be3025ad083d07c434845e4490cc962c9a41b9ca Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 14 May 2024 09:38:48 +0900 Subject: [PATCH 3/7] =?UTF-8?q?clang-format=E3=82=92=E3=83=AD=E3=83=BC?= =?UTF-8?q?=E3=83=AB=E3=83=90=E3=83=83=E3=82=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c5bc520b6..a6a480105 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -66,7 +66,7 @@ repos: args: [--line-length=99] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.5 + rev: v17.0.6 hooks: - id: clang-format types_or: [c++, c, cuda] From 680831c03df543fae8177d651c1101cc9eecd739 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 May 2024 00:38:59 +0000 Subject: [PATCH 4/7] style(pre-commit): autofix --- utility/crane_msg_wrappers/src/play_situation_wrapper.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp b/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp index a6f6d0002..6917772fa 100644 --- a/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp +++ b/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp @@ -14,7 +14,10 @@ namespace crane { -#define CMD_STRING_MAPPING(TYPE, CMD) {TYPE::CMD, #CMD} +#define CMD_STRING_MAPPING(TYPE, CMD) \ + { \ + TYPE::CMD, #CMD \ + } static std::map referee_command_map = { CMD_STRING_MAPPING(robocup_ssl_msgs::msg::Referee, COMMAND_HALT), From 8ccff0ef90ce066f5b828b2f0a437a423983586d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 23 May 2024 00:16:04 +0900 Subject: [PATCH 5/7] =?UTF-8?q?GridMap=E3=82=92publish?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_local_planner/src/gridmap_planner.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/crane_local_planner/src/gridmap_planner.cpp b/crane_local_planner/src/gridmap_planner.cpp index 0722f50ff..cf7a50dab 100644 --- a/crane_local_planner/src/gridmap_planner.cpp +++ b/crane_local_planner/src/gridmap_planner.cpp @@ -406,11 +406,12 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand( // ball_pos += ball_vel_unit; // time += TIME_STEP; // } - // std::unique_ptr message; - // message = grid_map::GridMapRosConverter::toMessage(map); - // message->header.stamp = rclcpp::Clock().now(); - // - // gridmap_publisher->publish(std::move(message)); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(map); + message->header.stamp = rclcpp::Clock().now(); + message->header.frame_id = "map"; + + gridmap_publisher->publish(std::move(message)); crane_msgs::msg::RobotCommands commands = msg; for (auto & command : commands.robot_commands) { From 65f37ad436925a3e7f98190557f6be99f2abc9a0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 23 May 2024 00:17:06 +0900 Subject: [PATCH 6/7] =?UTF-8?q?=E6=96=B0=E3=81=9F=E3=81=AA=E3=83=91?= =?UTF-8?q?=E3=83=83=E3=82=B1=E3=83=BC=E3=82=B8=EF=BC=81=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- utility/gridmap_to_image/.clang-format | 17 +++++++++++++++++ utility/gridmap_to_image/CMakeLists.txt | 24 ++++++++++++++++++++++++ utility/gridmap_to_image/package.xml | 24 ++++++++++++++++++++++++ 3 files changed, 65 insertions(+) create mode 100755 utility/gridmap_to_image/.clang-format create mode 100755 utility/gridmap_to_image/CMakeLists.txt create mode 100755 utility/gridmap_to_image/package.xml diff --git a/utility/gridmap_to_image/.clang-format b/utility/gridmap_to_image/.clang-format new file mode 100755 index 000000000..1db9fb9f5 --- /dev/null +++ b/utility/gridmap_to_image/.clang-format @@ -0,0 +1,17 @@ +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false diff --git a/utility/gridmap_to_image/CMakeLists.txt b/utility/gridmap_to_image/CMakeLists.txt new file mode 100755 index 000000000..70576e602 --- /dev/null +++ b/utility/gridmap_to_image/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.5) +project(gridmap_to_image) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -g) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/utility/gridmap_to_image/package.xml b/utility/gridmap_to_image/package.xml new file mode 100755 index 000000000..af15384cb --- /dev/null +++ b/utility/gridmap_to_image/package.xml @@ -0,0 +1,24 @@ + + + + gridmap_to_image + 0.0.0 + TODO: Package description + ibis + TODO: License declaration + + ament_cmake_auto + + grid_map_core + grid_map_cv + grid_map_ros + rclcpp + sensor_msgs + + ament_lint_auto + crane_lint_common + + + ament_cmake + + From d21a5e7966623309630dd4a17b20e3697a29d8d2 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 23 May 2024 00:18:02 +0900 Subject: [PATCH 7/7] =?UTF-8?q?=E4=B8=80=E4=B8=81=E4=B8=8A=E3=81=8C?= =?UTF-8?q?=E3=82=8A=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/gridmap_to_image_node.cpp | 66 +++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100644 utility/gridmap_to_image/src/gridmap_to_image_node.cpp diff --git a/utility/gridmap_to_image/src/gridmap_to_image_node.cpp b/utility/gridmap_to_image/src/gridmap_to_image_node.cpp new file mode 100644 index 000000000..487a7cd16 --- /dev/null +++ b/utility/gridmap_to_image/src/gridmap_to_image_node.cpp @@ -0,0 +1,66 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +#include +#include +#include +#include + +class GridMapToImageNode : public rclcpp::Node +{ +public: + GridMapToImageNode() : Node("grid_map_to_image_node") + { + // grid_mapの購読 + grid_map_sub_ = this->create_subscription( + "/local_planner/grid_map", 10, + std::bind(&GridMapToImageNode::gridMapCallback, this, std::placeholders::_1)); + } + +private: + void gridMapCallback(const grid_map_msgs::msg::GridMap::SharedPtr msg) + { + grid_map::GridMap map; + grid_map::GridMapRosConverter::fromMessage(*msg, map); + + // GridMap内の全てのレイヤーを取得 + for (const auto & layer : map.getLayers()) { + // GridMapをcv::Matに変換 + cv::Mat image; + grid_map::GridMapCvConverter::toImage(map, layer, CV_8UC1, 0.0, 1.0, image); + + // cv::Matをsensor_msgs/Imageに変換 + auto image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", image).toImageMsg(); + image_msg->header.stamp = this->get_clock()->now(); + + // トピック名をレイヤー名に基づいて生成し、「/」を「_」に置換 + std::string topic_name = "grid_map_image_" + layer; + std::replace(topic_name.begin(), topic_name.end(), '/', '_'); + + // 該当トピックのパブリッシャーが存在しない場合は作成 + if (image_pubs_.find(topic_name) == image_pubs_.end()) { + image_pubs_[topic_name] = this->create_publisher(topic_name, 10); + } + + // Imageメッセージの発行 + image_pubs_[topic_name]->publish(*image_msg); + } + } + + rclcpp::Subscription::SharedPtr grid_map_sub_; + std::unordered_map::SharedPtr> + image_pubs_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}