diff --git a/planning/autoware_objects_of_interest_marker_interface/CHANGELOG.rst b/planning/autoware_objects_of_interest_marker_interface/CHANGELOG.rst deleted file mode 100644 index 16b583990ec3e..0000000000000 --- a/planning/autoware_objects_of_interest_marker_interface/CHANGELOG.rst +++ /dev/null @@ -1,54 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_objects_of_interest_marker_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - planning (`#9570 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(universe_utils/motion_utils)!: add autoware namespace (`#7594 `_) -* feat(autoware_universe_utils)!: rename from tier4_autoware_utils (`#7538 `_) - Co-authored-by: kosuke55 -* refactor(objects_of_interest_marker_interface): rename to include/autoware/{package_name} (`#7535 `_) -* refactor(objects_of_interest_marker_interface)!: prefix package and namespace with autoware (`#7413 `_) -* Contributors: Fumiya Watanabe, Kosuke Takeuchi, Takayuki Murooka, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt b/planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt deleted file mode 100644 index da1161c007bae..0000000000000 --- a/planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -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 deleted file mode 100644 index 636e4fde32ca2..0000000000000 --- a/planning/autoware_objects_of_interest_marker_interface/README.md +++ /dev/null @@ -1,15 +0,0 @@ -# Objects Of Interest Marker Interface - -!!! warning - - Under Construction - -## Purpose - -## Inner-workings / Algorithms - -## Inputs / Outputs - -## Assumptions / Known limits - -## Future extensions / Unimplemented parts 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 deleted file mode 100644 index e228b34cb9f5d..0000000000000 --- a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// 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 deleted file mode 100644 index 53a5cb091d1ea..0000000000000 --- a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// 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 deleted file mode 100644 index ace32e91287a4..0000000000000 --- a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp +++ /dev/null @@ -1,86 +0,0 @@ -// 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 deleted file mode 100644 index 619df5f719df9..0000000000000 --- a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp +++ /dev/null @@ -1,103 +0,0 @@ -// 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 deleted file mode 100644 index 6d179f7872dc1..0000000000000 --- a/planning/autoware_objects_of_interest_marker_interface/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - autoware_objects_of_interest_marker_interface - 0.40.0 - The autoware_objects_of_interest_marker_interface package - - Fumiya Watanabe - Kosuke Takeuchi - Zulfaqar Azmi - - Apache License 2.0 - - Fumiya Watanabe - - ament_cmake_auto - - autoware_perception_msgs - autoware_universe_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 deleted file mode 100644 index 038006517f45a..0000000000000 --- a/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// 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::universe_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 deleted file mode 100644 index f2f076869e2ff..0000000000000 --- a/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// 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::universe_utils::createDefaultMarker; -using autoware::universe_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::universe_utils::pi * ratio; - point.x = data.pose.position.x + radius * autoware::universe_utils::cos(theta); - point.y = data.pose.position.y + radius * autoware::universe_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 deleted file mode 100644 index e18190a8bf9e7..0000000000000 --- a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// 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