feat(autoware_objects_of_interest_marker_interface): porting from universe to core

planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_objects_of_interest_marker_interface)
+
+### Compile options
+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 -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(autoware_objects_of_interest_marker_interface SHARED
+  src/coloring.cpp
+  src/objects_of_interest_marker_interface.cpp
+  src/marker_utils.cpp
+)
+
+# Test
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

planning/autoware_objects_of_interest_marker_interface/README.md

@@ -0,0 +1,46 @@
+# Objects Of Interest Marker Interface
+
+## Overview
+
+`autoware_objects_of_interest_marker_interface` is a collection of object visualization function packages.
+
+## Design
+
+This package implement a library to manage and visualize the object information by construct and publish it as marker array to rviz.
+
+For a object to be visualized, it has three import characteristics.
+
+- pose the position of the object
+- shape the shape of the Bounding box of the object
+- color the color of the Bounding box of the object
+
+## Usage
+
+### init
+
+include the header file to use then init the library
+
+```cpp
+#include <autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp>
+
+autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface
+    objects_of_interest_marker_interface_{this, "obstacle_cruise_planner"};
+```
+
+### insert
+
+insert object information to the 'objects_of_interest_marker_interface' manager
+
+```cpp
+using autoware::objects_of_interest_marker_interface::ColorName;
+objects_of_interest_marker_interface_.insertObjectData(
+    stopped_obstacle.pose, stopped_obstacle.shape, ColorName::RED);
+```
+
+### publish
+
+publish object information to the rviz to visualize
+
+```cpp
+objects_of_interest_marker_interface_.publishMarkerArray();
+```

planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp

@@ -0,0 +1,31 @@
+// Copyright 2023 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 AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_
+#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_
+#include "autoware/objects_of_interest_marker_interface/marker_data.hpp"
+
+#include <autoware_utils/ros/marker_helper.hpp>
+
+#include <std_msgs/msg/color_rgba.hpp>
+
+namespace autoware::objects_of_interest_marker_interface::coloring
+{
+std_msgs::msg::ColorRGBA getGreen(const float alpha);
+std_msgs::msg::ColorRGBA getAmber(const float alpha);
+std_msgs::msg::ColorRGBA getRed(const float alpha);
+std_msgs::msg::ColorRGBA getGray(const float alpha);
+}  // namespace autoware::objects_of_interest_marker_interface::coloring
+
+#endif  // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_

planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp

@@ -0,0 +1,34 @@
+// Copyright 2023 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 AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_
+#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_
+
+#include <autoware_perception_msgs/msg/predicted_object.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+
+namespace autoware::objects_of_interest_marker_interface
+{
+struct ObjectMarkerData
+{
+  geometry_msgs::msg::Pose pose{};
+  autoware_perception_msgs::msg::Shape shape{};
+  std_msgs::msg::ColorRGBA color;
+};
+
+enum class ColorName { GRAY, GREEN, AMBER, RED };
+}  // namespace autoware::objects_of_interest_marker_interface
+
+#endif  // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_

planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp

@@ -0,0 +1,86 @@
+// Copyright 2023 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 AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_
+#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_
+#include "autoware/objects_of_interest_marker_interface/coloring.hpp"
+#include "autoware/objects_of_interest_marker_interface/marker_data.hpp"
+
+#include <autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <autoware_utils/geometry/geometry.hpp>
+#include <autoware_utils/math/constants.hpp>
+#include <autoware_utils/math/trigonometry.hpp>
+#include <autoware_utils/ros/marker_helper.hpp>
+
+#include <autoware_perception_msgs/msg/predicted_object.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <string>
+
+namespace autoware::objects_of_interest_marker_interface::marker_utils
+{
+/**
+ * @brief Create arrow marker from object marker data
+ * @param id Marker id
+ * @param data Object marker data
+ * @param name Module name
+ * @param height_offset Height offset of arrow marker
+ * @param arrow_length Length of arrow marker
+ */
+visualization_msgs::msg::Marker createArrowMarker(
+  const size_t id, const ObjectMarkerData & data, const std::string & name,
+  const double height_offset, const double arrow_length = 1.0);
+
+/**
+ * @brief Create circle marker from object marker data
+ * @param id Marker id
+ * @param data Object marker data
+ * @param name Module name
+ * @param radius Radius of circle marker
+ * @param height_offset Height offset of circle marker
+ * @param line_width Line width of circle marker
+ */
+visualization_msgs::msg::Marker createCircleMarker(
+  const size_t id, const ObjectMarkerData & data, const std::string & name, const double radius,
+  const double height_offset, const double line_width = 0.1);
+
+/**
+ * @brief Create text marker visualizing module name
+ * @param id Marker id
+ * @param data Object marker data
+ * @param name Module name
+ * @param height_offset Height offset of target marker
+ * @param text_size Text size
+ */
+visualization_msgs::msg::Marker createNameTextMarker(
+  const size_t id, const ObjectMarkerData & data, const std::string & name,
+  const double height_offset, const double text_size = 0.5);
+
+/**
+ * @brief Create target marker from object marker data
+ * @param id Marker id
+ * @param data Object marker data
+ * @param name Module name
+ * @param height_offset Height offset of target marker
+ * @param arrow_length Length of arrow marker
+ * @param line_width Line width of circle marker
+ */
+visualization_msgs::msg::MarkerArray createTargetMarker(
+  const size_t id, const ObjectMarkerData & data, const std::string & name,
+  const double height_offset, const double arrow_length = 1.0, const double line_width = 0.1);
+}  // namespace autoware::objects_of_interest_marker_interface::marker_utils
+
+#endif  // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_

planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp

@@ -0,0 +1,103 @@
+// Copyright 2023 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 AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_
+#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_
+#include "autoware/objects_of_interest_marker_interface/coloring.hpp"
+#include "autoware/objects_of_interest_marker_interface/marker_data.hpp"
+#include "autoware/objects_of_interest_marker_interface/marker_utils.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_perception_msgs/msg/predicted_object.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <string>
+#include <vector>
+
+namespace autoware::objects_of_interest_marker_interface
+{
+class ObjectsOfInterestMarkerInterface
+{
+public:
+  /**
+   * @brief Constructor
+   * @param node Node that publishes marker
+   * @param name Module name
+   */
+  ObjectsOfInterestMarkerInterface(rclcpp::Node * node, const std::string & name);
+
+  /**
+   * @brief Insert object data to visualize
+   * @param pose Object pose
+   * @param shape Object shape
+   * @param color_name Color name
+   */
+  void insertObjectData(
+    const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape,
+    const ColorName & color_name);
+
+  /**
+   * @brief Insert object data to visualize with custom color data
+   * @param pose Object pose
+   * @param shape Object shape
+   * @param color Color data with alpha
+   */
+  void insertObjectDataWithCustomColor(
+    const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape,
+    const std_msgs::msg::ColorRGBA & color);
+
+  /**
+   * @brief Publish interest objects marker
+   */
+  void publishMarkerArray();
+
+  /**
+   * @brief Set height offset of markers
+   * @param offset Height offset of markers
+   */
+  void setHeightOffset(const double offset);
+
+  /**
+   * @brief Get color data from color name
+   * @param color_name Color name
+   * @param alpha Alpha
+   */
+  static std_msgs::msg::ColorRGBA getColor(const ColorName & color_name, const float alpha = 0.99f);
+
+  /**
+   * @brief Get module name including this interface
+   */
+  std::string getName() const { return name_; }
+
+  /**
+   * @brief Get height offset
+   */
+  double getHeightOffset() const { return height_offset_; }
+
+private:
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_marker_;
+
+  double height_offset_{0.5};
+  std::vector<ObjectMarkerData> obj_marker_data_array_;
+
+  std::string name_;
+  std::string topic_namespace_ = "/planning/debug/objects_of_interest";
+};
+
+}  // namespace autoware::objects_of_interest_marker_interface
+
+#endif  // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_

planning/autoware_objects_of_interest_marker_interface/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>autoware_objects_of_interest_marker_interface</name>
+  <version>0.0.0</version>
+  <description>The autoware_objects_of_interest_marker_interface package</description>
+
+  <maintainer email="[email protected]">Xingang Liu</maintainer>
+  <maintainer email="[email protected]">Fumiya Watanabe</maintainer>
+  <maintainer email="[email protected]">Kosuke Takeuchi</maintainer>
+  <maintainer email="[email protected]">Zulfaqar Azmi</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Fumiya Watanabe</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_perception_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>