feat(heatmap_visualizer): add heatmap visualizer (#925)

* feat(heatmap_visualizer): add heatmap visualizer components

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

* docs(heatmap_visualizer): add readme and image

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

perception/heatmap_visualizer/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.14)
+project(heatmap_visualizer)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(heatmap_visualizer_component SHARED
+    src/heatmap_visualizer_node.cpp
+    src/utils.cpp
+)
+
+rclcpp_components_register_node(heatmap_visualizer_component
+    PLUGIN "heatmap_visualizer::HeatmapVisualizerNode"
+    EXECUTABLE heatmap_visualizer
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+    launch
+)

perception/heatmap_visualizer/README.md

@@ -0,0 +1,91 @@
+# heatmap_visualizer
+
+## Purpose
+
+heatmap_visualizer is a package for visualizing heatmap of detected 3D objects' positions on the BEV space.
+
+This package is used for qualitative evaluation and trend analysis of the detector, it means, for instance, the heatmap shows "This detector performs good for near around of our vehicle, but far is bad".
+
+### How to run
+
+```shell
+ros2 launch heatmap_visualizer heatmap_visualizer.launch.xml input/objects:=<DETECTED_OBJECTS_TOPIC>
+```
+
+## Inner-workings / Algorithms
+
+In this implementation, create heatmap of the center position of detected objects for each classes, for instance, CAR, PEDESTRIAN, etc, and publish them as occupancy grid maps.
+
+![heatmap_visualizer_sample](./image/heatmap_sample.png)
+
+In the above figure, the pink represents high detection frequency area and blue one is low, or black represents there is no detection.
+
+![heatmap_visualizer](./image/heatmap_visualizer.png)
+
+As inner-workings, add center positions of detected objects to index of each corresponding grid map cell in a buffer.
+The created heatmap will be published by each specific frame, which can be specified with `frame_count`. Note that the buffer to be add the positions is not reset per publishing.
+When publishing, firstly these values are normalized to [0, 1] using maximum and minimum values in the buffer. Secondly, they are scaled to integer in [0, 100] because `nav_msgs::msg::OccupancyGrid` only allow the value in [0, 100].
+
+## Inputs / Outputs
+
+### Input
+
+| Name              | Type                                                  | Description      |
+| ----------------- | ----------------------------------------------------- | ---------------- |
+| `~/input/objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects |
+
+### Output
+
+| Name                            | Type                           | Description        |
+| ------------------------------- | ------------------------------ | ------------------ |
+| `~/output/objects/<CLASS_NAME>` | `nav_msgs::msg::OccupancyGrid` | visualized heatmap |
+
+## Parameters
+
+### Core Parameters
+
+| Name                          | Type   | Default Value | Description                                       |
+| ----------------------------- | ------ | ------------- | ------------------------------------------------- |
+| `frame_count`                 | int    | `50`          | The number of frames to publish heatmap           |
+| `map_frame`                   | string | `base_link`   | the frame of heatmap to be respected              |
+| `map_length`                  | float  | `200.0`       | the length of map in meter                        |
+| `map_resolution`              | float  | `0.8`         | the resolution of map                             |
+| `use_confidence`              | bool   | `false`       | the flag if use confidence score as heatmap value |
+| `rename_car_to_truck_and_bus` | bool   | `true`        | the flag if rename car to truck or bus            |
+
+## Assumptions / Known limits
+
+The heatmap depends on the data to be used, so if the objects in data are sparse the heatmap will be sparse.
+
+## (Optional) Error detection and handling
+
+<!-- Write how to detect errors and how to recover from them.
+
+Example:
+  This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
+-->
+
+## (Optional) Performance characterization
+
+<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary.
+
+Example:
+  ### Complexity
+
+  This algorithm is O(N).
+
+  ### Processing time
+
+  ...
+-->
+
+## References/External links
+
+## (Optional) Future extensions / Unimplemented parts
+
+<!-- Write future extensions of this package.
+
+Example:
+  Currently, this package can't handle the chattering obstacles well. We plan to add some probabilistic filters in the perception layer to improve it.
+  Also, there are some parameters that should be global(e.g. vehicle size, max steering, etc.). These will be refactored and defined as global parameters so that we can share the same parameters between different nodes.
+-->

perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp

@@ -0,0 +1,81 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// 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 HEATMAP_VISUALIZER__HEATMAP_VISUALIZER_NODE_HPP_
+#define HEATMAP_VISUALIZER__HEATMAP_VISUALIZER_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <map>
+#include <string>
+#include <vector>
+
+namespace heatmap_visualizer
+{
+using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
+
+class HeatmapVisualizerNode : public rclcpp::Node
+{
+public:
+  /**
+   * @brief Construct a new Heatmap Visualizer Node object
+   *
+   * @param node_options
+   */
+  explicit HeatmapVisualizerNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  /**
+   * @brief Callback function
+   *
+   * @param msg
+   */
+  void detectedObjectsCallback(
+    const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
+
+  // Subscriber
+  rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr objects_sub_;
+
+  // Publishers
+  std::map<uint8_t, rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr> heatmap_pubs_;
+
+  std::map<uint8_t, nav_msgs::msg::OccupancyGrid> heatmaps_;
+
+  uint32_t frame_count_;
+
+  // ROS params
+  uint32_t total_frame_;
+  std::string target_frame_;
+  std::string src_frame_;
+  std::string map_frame_;
+  float map_length_;
+  float map_resolution_;
+  bool use_confidence_;
+  std::vector<std::string> class_names_{"CAR",     "TRUCK",     "BUS",       "TRAILER",
+                                        "BICYCLE", "MOTORBIKE", "PEDESTRIAN"};
+  bool rename_car_to_truck_and_bus_;
+
+  // Number of width and height cells
+  uint32_t width_;
+  uint32_t height_;
+  std::map<uint8_t, std::vector<float>> data_buffers_;
+};  // class HeatmapVisualizerNode
+
+}  // namespace heatmap_visualizer
+
+#endif  // HEATMAP_VISUALIZER__HEATMAP_VISUALIZER_NODE_HPP_

perception/heatmap_visualizer/include/heatmap_visualizer/utils.hpp

@@ -0,0 +1,117 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// 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 HEATMAP_VISUALIZER__UTILS_HPP_
+#define HEATMAP_VISUALIZER__UTILS_HPP_
+
+#include <autoware_auto_perception_msgs/msg/detected_object.hpp>
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <string>
+#include <vector>
+
+namespace heatmap_visualizer
+{
+using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
+
+/**
+ * @brief IndexXYT object
+ *
+ */
+struct IndexXYT
+{
+  int x;
+  int y;
+  int theta;
+};  // struct IndexXYT
+
+/**
+ * @brief
+ *
+ * @param theta
+ * @param theta_size
+ * @return int
+ */
+int discretizeAngle(const double theta, const int theta_size);
+
+/**
+ * @brief
+ *
+ * @param costmap
+ * @param pose_local
+ * @param theta_size
+ * @return IndexXYT
+ */
+IndexXYT pose2index(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local,
+  const int theta_size);
+
+/**
+ * @brief Set confidence score or 1 to box position to data buffer.
+ *
+ * @param obj
+ * @param heatmap
+ * @param data_buffer
+ * @param use_confidence
+ */
+void setHeatmapToBuffer(
+  const autoware_auto_perception_msgs::msg::DetectedObject & obj,
+  const nav_msgs::msg::OccupancyGrid & heatmap, std::vector<float> * data_buffer,
+  const bool use_confidence);
+
+/**
+ * @brief Set the Heatmap To Occupancy Grid object
+ *
+ * @param data_buffer
+ * @param heatmap
+ */
+void setHeatmapToOccupancyGrid(
+  const std::vector<float> & data_buffer, nav_msgs::msg::OccupancyGrid * heatmap);
+
+/**
+ * @brief Get the Semantic Type object
+ *
+ * @param class_name
+ * @return uint8_t
+ */
+uint8_t getSemanticType(const std::string & class_name);
+
+/**
+ * @brief Get the Class Name object
+ *
+ * @param label
+ * @return std::string
+ */
+std::string getClassName(const uint8_t label);
+
+/**
+ * @brief
+ *
+ * @param label
+ * @return true
+ * @return false
+ */
+bool isCarLikeVehicleLabel(const uint8_t label);
+}  // namespace heatmap_visualizer
+
+#endif  // HEATMAP_VISUALIZER__UTILS_HPP_

perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <!-- heatmap visualizer-->
+  <arg name="input/objects" default="objects"/>
+  <arg name="frame_count" default="50"/>
+  <arg name="map_frame" default="base_link"/>
+  <arg name="map_length" default="200.0"/>
+  <arg name="map_resolution" default="0.5"/>
+  <arg name="use_confidence" default="false"/>
+  <arg name="rename_car_to_truck_and_bus" default="true"/>
+
+  <node pkg="heatmap_visualizer" exec="heatmap_visualizer" output="screen">
+    <remap from="~/input/objects" to="$(var input/objects)"/>
+    <param name="frame_count" value="$(var frame_count)"/>
+    <param name="map_frame" value="$(var map_frame)"/>
+    <param name="map_length" value="$(var map_length)"/>
+    <param name="map_resolution" value="$(var map_resolution)"/>
+    <param name="use_confidence" value="$(var use_confidence)"/>
+    <param name="rename_car_to_truck_and_bus" value="$(var rename_car_to_truck_and_bus)"/>
+  </node>
+</launch>

perception/heatmap_visualizer/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>heatmap_visualizer</name>
+  <version>1.0.0</version>
+  <description>The heatmap_visualizer package</description>
+  <maintainer email="kotaro.uetake@tier4.jp">Kotaro Uetake</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="kotaro.uetake@tier4.jp">Kotaro Uetake</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>