From ad0dfa973b4c32f52107ca6ba3125a41976d11af Mon Sep 17 00:00:00 2001 From: liuXinGangChina Date: Wed, 15 Jan 2025 17:55:51 +0800 Subject: [PATCH] feat(autoware_objects_of_interest_marker_interface): porting from universe to core, autoware_objects_of_interest_marker_interface, add this package to core repo and replace universe_utils dependency with autoware_utils dependency: v0.0 Signed-off-by: liuXinGangChina --- .../CMakeLists.txt | 27 ++++ .../README.md | 41 +++++++ .../coloring.hpp | 31 +++++ .../marker_data.hpp | 34 +++++ .../marker_utils.hpp | 86 +++++++++++++ .../objects_of_interest_marker_interface.hpp | 103 ++++++++++++++++ .../package.xml | 31 +++++ .../src/coloring.cpp | 54 ++++++++ .../src/marker_utils.cpp | 116 ++++++++++++++++++ .../objects_of_interest_marker_interface.cpp | 92 ++++++++++++++ 10 files changed, 615 insertions(+) create mode 100644 planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt create mode 100644 planning/autoware_objects_of_interest_marker_interface/README.md create mode 100644 planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/package.xml create mode 100644 planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp diff --git a/planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt b/planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt new file mode 100644 index 00000000..da1161c0 --- /dev/null +++ b/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() diff --git a/planning/autoware_objects_of_interest_marker_interface/README.md b/planning/autoware_objects_of_interest_marker_interface/README.md new file mode 100644 index 00000000..9260ac01 --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/README.md @@ -0,0 +1,41 @@ +# 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 visualizaed, 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::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); +``` + +### pubish +pubish object information to the rviz to visualize +```cpp +objects_of_interest_marker_interface_.publishMarkerArray(); +``` \ No newline at end of file diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp new file mode 100644 index 00000000..b1551918 --- /dev/null +++ b/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 + +#include + +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_ diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp new file mode 100644 index 00000000..53a5cb09 --- /dev/null +++ b/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 +#include +#include + +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_ diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp new file mode 100644 index 00000000..1e484ca8 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +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_ diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp new file mode 100644 index 00000000..619df5f7 --- /dev/null +++ b/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 + +#include +#include +#include +#include + +#include +#include + +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::SharedPtr pub_marker_; + + double height_offset_{0.5}; + std::vector 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_ diff --git a/planning/autoware_objects_of_interest_marker_interface/package.xml b/planning/autoware_objects_of_interest_marker_interface/package.xml new file mode 100644 index 00000000..27b07972 --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/package.xml @@ -0,0 +1,31 @@ + + + autoware_objects_of_interest_marker_interface + 0.0.0 + The autoware_objects_of_interest_marker_interface package + + Xingang Liu + Fumiya Watanabe + Kosuke Takeuchi + Zulfaqar Azmi + + Apache License 2.0 + + Fumiya Watanabe + + ament_cmake_auto + + autoware_perception_msgs + autoware_utils + geometry_msgs + rclcpp + std_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp b/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp new file mode 100644 index 00000000..3a1cfb96 --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp @@ -0,0 +1,54 @@ +// 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. + +#include "autoware/objects_of_interest_marker_interface/coloring.hpp" + +namespace +{ +std_msgs::msg::ColorRGBA convertFromColorCode(const uint64_t code, const float alpha) +{ + const float r = static_cast(code >> 16) / 255.0; + const float g = static_cast((code << 48) >> 56) / 255.0; + const float b = static_cast((code << 56) >> 56) / 255.0; + + return autoware_utils::createMarkerColor(r, g, b, alpha); +} +} // namespace + +namespace autoware::objects_of_interest_marker_interface::coloring +{ +std_msgs::msg::ColorRGBA getGreen(const float alpha) +{ + constexpr uint64_t code = 0x00e676; + return convertFromColorCode(code, alpha); +} + +std_msgs::msg::ColorRGBA getAmber(const float alpha) +{ + constexpr uint64_t code = 0xffea00; + return convertFromColorCode(code, alpha); +} + +std_msgs::msg::ColorRGBA getRed(const float alpha) +{ + constexpr uint64_t code = 0xff3d00; + return convertFromColorCode(code, alpha); +} + +std_msgs::msg::ColorRGBA getGray(const float alpha) +{ + constexpr uint64_t code = 0xbdbdbd; + return convertFromColorCode(code, alpha); +} +} // namespace autoware::objects_of_interest_marker_interface::coloring diff --git a/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp new file mode 100644 index 00000000..ea8cbb9c --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp @@ -0,0 +1,116 @@ +// 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. + +#include "autoware/objects_of_interest_marker_interface/marker_utils.hpp" + +#include + +namespace autoware::objects_of_interest_marker_interface::marker_utils +{ +using geometry_msgs::msg::Point; + +using std_msgs::msg::ColorRGBA; + +using autoware_utils::createDefaultMarker; +using autoware_utils::createMarkerScale; + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +Marker createArrowMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, + const double height_offset, const double arrow_length) +{ + const double line_width = 0.25 * arrow_length; + const double head_width = 0.5 * arrow_length; + const double head_height = 0.5 * arrow_length; + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_arrow", id, Marker::ARROW, + createMarkerScale(line_width, head_width, head_height), data.color); + + const double height = 0.5 * data.shape.dimensions.z; + + Point src, dst; + src = data.pose.position; + src.z += height + height_offset + arrow_length; + dst = data.pose.position; + dst.z += height + height_offset; + + marker.points.push_back(src); + marker.points.push_back(dst); + + return marker; +} + +Marker createCircleMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, const double radius, + const double height_offset, const double line_width) +{ + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, Marker::LINE_STRIP, + createMarkerScale(line_width, 0.0, 0.0), data.color); + + const double height = 0.5 * data.shape.dimensions.z; + + constexpr size_t num_points = 20; + for (size_t i = 0; i < num_points; ++i) { + Point point; + const double ratio = static_cast(i) / static_cast(num_points); + const double theta = 2 * autoware_utils::pi * ratio; + point.x = data.pose.position.x + radius * autoware_utils::cos(theta); + point.y = data.pose.position.y + radius * autoware_utils::sin(theta); + point.z = data.pose.position.z + height + height_offset; + marker.points.push_back(point); + } + marker.points.push_back(marker.points.front()); + + return marker; +} + +visualization_msgs::msg::Marker createNameTextMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, + const double height_offset, const double text_size) +{ + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_name_text", id, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, text_size), coloring::getGray(data.color.a)); + + marker.text = name; + + const double height = 0.5 * data.shape.dimensions.z; + marker.pose = data.pose; + marker.pose.position.z += height + height_offset; + + return marker; +} + +MarkerArray createTargetMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, + const double height_offset, const double arrow_length, const double line_width) +{ + MarkerArray marker_array; + marker_array.markers.push_back(createArrowMarker(id, data, name, height_offset, arrow_length)); + marker_array.markers.push_back(createCircleMarker( + id, data, name + "_circle1", 0.5 * arrow_length, height_offset + 0.75 * arrow_length, + line_width)); + marker_array.markers.push_back(createCircleMarker( + id, data, name + "_circle2", 0.75 * arrow_length, height_offset + 0.75 * arrow_length, + line_width)); + marker_array.markers.push_back( + createNameTextMarker(id, data, name, height_offset + 1.5 * arrow_length, 0.5 * arrow_length)); + + return marker_array; +} +} // namespace autoware::objects_of_interest_marker_interface::marker_utils diff --git a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp new file mode 100644 index 00000000..055a836f --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -0,0 +1,92 @@ +// 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. + +#include "autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp" + +#include +#include +#include + +#include + +namespace autoware::objects_of_interest_marker_interface +{ +using autoware_perception_msgs::msg::Shape; +using geometry_msgs::msg::Pose; +using std_msgs::msg::ColorRGBA; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +ObjectsOfInterestMarkerInterface::ObjectsOfInterestMarkerInterface( + rclcpp::Node * node, const std::string & name) +: name_{name} +{ + // Publisher + pub_marker_ = node->create_publisher(topic_namespace_ + "/" + name, 1); +} + +void ObjectsOfInterestMarkerInterface::insertObjectData( + const Pose & pose, const Shape & shape, const ColorName & color_name) +{ + insertObjectDataWithCustomColor(pose, shape, getColor(color_name)); +} + +void ObjectsOfInterestMarkerInterface::insertObjectDataWithCustomColor( + const Pose & pose, const Shape & shape, const ColorRGBA & color) +{ + ObjectMarkerData data; + data.pose = pose; + data.shape = shape; + data.color = color; + + obj_marker_data_array_.push_back(data); +} + +void ObjectsOfInterestMarkerInterface::publishMarkerArray() +{ + MarkerArray marker_array; + for (size_t i = 0; i < obj_marker_data_array_.size(); ++i) { + const auto data = obj_marker_data_array_.at(i); + const MarkerArray target_marker = + marker_utils::createTargetMarker(i, data, getName(), getHeightOffset()); + marker_array.markers.insert( + marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end()); + } + pub_marker_->publish(marker_array); + obj_marker_data_array_.clear(); +} + +void ObjectsOfInterestMarkerInterface::setHeightOffset(const double offset) +{ + height_offset_ = offset; +} + +ColorRGBA ObjectsOfInterestMarkerInterface::getColor( + const ColorName & color_name, const float alpha) +{ + switch (color_name) { + case ColorName::GRAY: + return coloring::getGray(alpha); + case ColorName::GREEN: + return coloring::getGreen(alpha); + case ColorName::AMBER: + return coloring::getAmber(alpha); + case ColorName::RED: + return coloring::getRed(alpha); + default: + return coloring::getGray(alpha); + } +} + +} // namespace autoware::objects_of_interest_marker_interface