Merge branch 'develop' into feature/scenario_test

.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,7 +60,7 @@ repos:
 #      - id: isort
 
   - repo: https://github.com/psf/black
-    rev: 24.4.0
+    rev: 24.4.2
     hooks:
       - id: black
         args: [--line-length=99]

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<grid_map_msgs::msg::GridMap> message;
-  //  message = grid_map::GridMapRosConverter::toMessage(map);
-  //  message->header.stamp = rclcpp::Clock().now();
-  //
-  //  gridmap_publisher->publish(std::move(message));
+  std::unique_ptr<grid_map_msgs::msg::GridMap> 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) {

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

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()

utility/gridmap_to_image/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gridmap_to_image</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">ibis</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>grid_map_core</depend>
+  <depend>grid_map_cv</depend>
+  <depend>grid_map_ros</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>crane_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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 <cv_bridge/cv_bridge.h>
+
+#include <grid_map_cv/grid_map_cv.hpp>
+#include <grid_map_ros/grid_map_ros.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+class GridMapToImageNode : public rclcpp::Node
+{
+public:
+  GridMapToImageNode() : Node("grid_map_to_image_node")
+  {
+    // grid_mapの購読
+    grid_map_sub_ = this->create_subscription<grid_map_msgs::msg::GridMap>(
+      "/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<unsigned char, 1>(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<sensor_msgs::msg::Image>(topic_name, 10);
+      }
+
+      // Imageメッセージの発行
+      image_pubs_[topic_name]->publish(*image_msg);
+    }
+  }
+
+  rclcpp::Subscription<grid_map_msgs::msg::GridMap>::SharedPtr grid_map_sub_;
+  std::unordered_map<std::string, rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr>
+    image_pubs_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<GridMapToImageNode>());
+  rclcpp::shutdown();
+  return 0;
+}