From 97b59d96335aa6e0e970ca20872c6e75f5787c57 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 24 Nov 2023 22:22:09 +0900 Subject: [PATCH 1/2] feat(objects_of_interest_marker_interface): add objects of interest marker interface (#5564) * feat(interest_objects_marker_interface): add interest objects marker interface Signed-off-by: Fumiya Watanabe * feat(interest_objects_marker_interface): change marker type Signed-off-by: Fumiya Watanabe * feat(interest_objects_marker_interface): change markers Signed-off-by: Fumiya Watanabe * feat(interest_objects_marker_interface): fix lane change interface Signed-off-by: Fumiya Watanabe * feat(interest_objects_marker_interface): fix interface Signed-off-by: Fumiya Watanabe * refactor(interest_objects_marker_interface): fix interface Signed-off-by: Fumiya Watanabe * refactor(interest_objects_marker_interface): refactor and add descriptions Signed-off-by: Fumiya Watanabe * refactor(lane_change): refactor Signed-off-by: Fumiya Watanabe * docs(interest_objects_visualization): add contents (under construction) Signed-off-by: Fumiya Watanabe * chore(interest_objects_visualization): fix includes and package.xml Signed-off-by: Fumiya Watanabe * fix(interest_objects_marker_interface): fix Signed-off-by: Fumiya Watanabe * refactor(objects_of_interest_marker_interface): rename interface Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../scene_module/lane_change/interface.hpp | 11 +++ .../path_safety_checker_parameters.hpp | 1 + planning/behavior_path_planner/package.xml | 1 + .../src/marker_utils/utils.cpp | 1 + .../scene_module/lane_change/interface.cpp | 20 +++- .../path_safety_checker/safety_check.cpp | 2 + .../CMakeLists.txt | 27 ++++++ .../README.md | 15 +++ .../coloring.hpp | 31 ++++++ .../marker_data.hpp | 34 +++++++ .../marker_utils.hpp | 73 ++++++++++++++ .../objects_of_interest_marker_interface.hpp | 93 ++++++++++++++++++ .../package.xml | 28 ++++++ .../src/coloring.cpp | 54 +++++++++++ .../src/marker_utils.cpp | 96 +++++++++++++++++++ .../objects_of_interest_marker_interface.cpp | 90 +++++++++++++++++ 16 files changed, 576 insertions(+), 1 deletion(-) create mode 100644 planning/objects_of_interest_marker_interface/CMakeLists.txt create mode 100644 planning/objects_of_interest_marker_interface/README.md create mode 100644 planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/coloring.hpp create mode 100644 planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp create mode 100644 planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp create mode 100644 planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp create mode 100644 planning/objects_of_interest_marker_interface/package.xml create mode 100644 planning/objects_of_interest_marker_interface/src/coloring.cpp create mode 100644 planning/objects_of_interest_marker_interface/src/marker_utils.cpp create mode 100644 planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index 34ded5c976e3b..d42f8571e8ee4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -25,6 +25,7 @@ #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp" #include @@ -48,6 +49,8 @@ namespace behavior_path_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using objects_of_interest_marker_interface::ColorName; +using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; @@ -107,6 +110,13 @@ class LaneChangeInterface : public SceneModuleInterface void setObjectDebugVisualization() const; + void setObjectsOfInterestData(const bool is_approved); + + void publishObjectsOfInterestData() + { + objects_of_interest_marker_interface_.publishMarkerArray(); + } + void updateSteeringFactorPtr(const BehaviorModuleOutput & output); void updateSteeringFactorPtr( @@ -116,6 +126,7 @@ class LaneChangeInterface : public SceneModuleInterface mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; std::unique_ptr prev_approved_path_; + ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; void clearAbortApproval() { is_abort_path_approved_ = false; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index b2433c076b1a8..8ad057e7073f0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -196,6 +196,7 @@ struct CollisionCheckDebug std::vector obj_predicted_path; ///< object's predicted path. Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. + autoware_auto_perception_msgs::msg::Shape obj_shape{}; ///< object's shape. }; using CollisionCheckDebugPair = std::pair; using CollisionCheckDebugMap = diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 24ee1056e5a47..7358e320e05bd 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -57,6 +57,7 @@ magic_enum motion_utils object_recognition_utils + objects_of_interest_marker_interface planning_test_utils rclcpp rclcpp_components diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index aac4d9235146c..dc36fcd52c278 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -40,6 +40,7 @@ CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) CollisionCheckDebug debug; debug.current_obj_pose = obj.initial_pose.pose; debug.current_twist = obj.initial_twist.twist; + debug.obj_shape = obj.shape; return {tier4_autoware_utils::toHexString(obj.uuid), debug}; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 16527c2886617..b48b543ca9bda 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -41,7 +41,8 @@ LaneChangeInterface::LaneChangeInterface( : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)}, module_type_{std::move(module_type)}, - prev_approved_path_{std::make_unique()} + prev_approved_path_{std::make_unique()}, + objects_of_interest_marker_interface_{&node, name} { steering_factor_interface_ptr_ = std::make_unique(&node, name); } @@ -224,6 +225,9 @@ BehaviorModuleOutput LaneChangeInterface::plan() stop_pose_ = module_type_->getStopPose(); + setObjectsOfInterestData(true); + publishObjectsOfInterestData(); + updateSteeringFactorPtr(output); clearWaitingApproval(); @@ -246,6 +250,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateLaneChangeStatus(); setObjectDebugVisualization(); + setObjectsOfInterestData(false); + publishObjectsOfInterestData(); // change turn signal when the vehicle reaches at the end of the path for waiting lane change out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); @@ -330,6 +336,18 @@ void LaneChangeInterface::setObjectDebugVisualization() const } } +void LaneChangeInterface::setObjectsOfInterestData(const bool is_approved) +{ + const auto debug_data = + is_approved ? module_type_->getAfterApprovalDebugData() : module_type_->getDebugData(); + for (const auto & [uuid, data] : debug_data) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + objects_of_interest_marker_interface_.insertObjectData( + data.current_obj_pose, data.obj_shape, color); + } + return; +} + std::shared_ptr LaneChangeInterface::get_debug_msg_array() const { const auto debug_data = module_type_->getDebugData(); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 90f3eb7a29891..27ebd8f7043df 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -274,6 +274,8 @@ bool checkCollision( const auto interpolated_data = getInterpolatedPoseWithVelocityAndPolygonStamped( predicted_ego_path, current_time, ego_vehicle_info); if (!interpolated_data) { + debug.expected_obj_pose = obj_pose; + debug.extended_obj_polygon = obj_polygon; continue; } const auto & ego_pose = interpolated_data->pose; diff --git a/planning/objects_of_interest_marker_interface/CMakeLists.txt b/planning/objects_of_interest_marker_interface/CMakeLists.txt new file mode 100644 index 0000000000000..eec17f8d9218b --- /dev/null +++ b/planning/objects_of_interest_marker_interface/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.5) +project(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(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/objects_of_interest_marker_interface/README.md b/planning/objects_of_interest_marker_interface/README.md new file mode 100644 index 0000000000000..636e4fde32ca2 --- /dev/null +++ b/planning/objects_of_interest_marker_interface/README.md @@ -0,0 +1,15 @@ +# 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/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/coloring.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/coloring.hpp new file mode 100644 index 0000000000000..041a6a454d0dd --- /dev/null +++ b/planning/objects_of_interest_marker_interface/include/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 OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ +#define OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ +#include "objects_of_interest_marker_interface/marker_data.hpp" + +#include + +#include + +namespace 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 objects_of_interest_marker_interface::coloring + +#endif // OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp new file mode 100644 index 0000000000000..32262ab7eec35 --- /dev/null +++ b/planning/objects_of_interest_marker_interface/include/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 OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ +#define OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ + +#include +#include +#include + +namespace objects_of_interest_marker_interface +{ +struct ObjectMarkerData +{ + geometry_msgs::msg::Pose pose{}; + autoware_auto_perception_msgs::msg::Shape shape{}; + std_msgs::msg::ColorRGBA color; +}; + +enum class ColorName { GRAY, GREEN, AMBER, RED }; +} // namespace objects_of_interest_marker_interface + +#endif // OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp new file mode 100644 index 0000000000000..cff45e5c1a9eb --- /dev/null +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp @@ -0,0 +1,73 @@ +// 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 OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ +#define OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ +#include "objects_of_interest_marker_interface/marker_data.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace 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 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 objects_of_interest_marker_interface::marker_utils + +#endif // OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp new file mode 100644 index 0000000000000..79bab6d5faf69 --- /dev/null +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp @@ -0,0 +1,93 @@ +// 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 OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ +#define OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ +#include "objects_of_interest_marker_interface/coloring.hpp" +#include "objects_of_interest_marker_interface/marker_data.hpp" +#include "objects_of_interest_marker_interface/marker_utils.hpp" + +#include + +#include +#include +#include +#include + +#include +#include + +namespace 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_auto_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_auto_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); + +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 objects_of_interest_marker_interface + +#endif // OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ diff --git a/planning/objects_of_interest_marker_interface/package.xml b/planning/objects_of_interest_marker_interface/package.xml new file mode 100644 index 0000000000000..0aebdd3b099b1 --- /dev/null +++ b/planning/objects_of_interest_marker_interface/package.xml @@ -0,0 +1,28 @@ + + + objects_of_interest_marker_interface + 0.1.0 + The objects_of_interest_marker_interface package + + Fumiya Watanabe + + Apache License 2.0 + + Fumiya Watanabe + + ament_cmake_auto + + autoware_auto_perception_msgs + geometry_msgs + rclcpp + std_msgs + tier4_autoware_utils + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/objects_of_interest_marker_interface/src/coloring.cpp b/planning/objects_of_interest_marker_interface/src/coloring.cpp new file mode 100644 index 0000000000000..6c566474e4c9e --- /dev/null +++ b/planning/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 "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 tier4_autoware_utils::createMarkerColor(r, g, b, alpha); +} +} // namespace + +namespace 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 objects_of_interest_marker_interface::coloring diff --git a/planning/objects_of_interest_marker_interface/src/marker_utils.cpp b/planning/objects_of_interest_marker_interface/src/marker_utils.cpp new file mode 100644 index 0000000000000..71414eae4dbe7 --- /dev/null +++ b/planning/objects_of_interest_marker_interface/src/marker_utils.cpp @@ -0,0 +1,96 @@ +// 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 "objects_of_interest_marker_interface/marker_utils.hpp" + +namespace objects_of_interest_marker_interface::marker_utils +{ +using geometry_msgs::msg::Point; + +using std_msgs::msg::ColorRGBA; + +using tier4_autoware_utils::createDefaultMarker; +using tier4_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, 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 * tier4_autoware_utils::pi * ratio; + point.x = data.pose.position.x + radius * tier4_autoware_utils::cos(theta); + point.y = data.pose.position.y + radius * tier4_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; +} + +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 + "_arrow", 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)); + + return marker_array; +} +} // namespace objects_of_interest_marker_interface::marker_utils diff --git a/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp new file mode 100644 index 0000000000000..6261b4791be48 --- /dev/null +++ b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -0,0 +1,90 @@ +// 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 "objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp" + +#include +#include +#include + +namespace objects_of_interest_marker_interface +{ +using autoware_auto_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, name_, height_offset_); + 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::GREEN: + return coloring::getGreen(alpha); + case ColorName::AMBER: + return coloring::getAmber(alpha); + case ColorName::RED: + return coloring::getRed(alpha); + case ColorName::GRAY: + return coloring::getGray(alpha); + default: + return coloring::getGray(alpha); + } +} + +} // namespace objects_of_interest_marker_interface From 7b45fab8a6150b5325c79f835e882c00ff6194d8 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 27 Nov 2023 17:45:46 +0900 Subject: [PATCH 2/2] feat(objects_of_interest_marker_interface): add name text marker (#5690) Signed-off-by: Fumiya Watanabe --- .../marker_utils.hpp | 13 ++++++++++ .../objects_of_interest_marker_interface.hpp | 10 ++++++++ .../src/marker_utils.cpp | 24 ++++++++++++++++--- .../objects_of_interest_marker_interface.cpp | 2 +- 4 files changed, 45 insertions(+), 4 deletions(-) diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp index cff45e5c1a9eb..c1fae5ecc4bec 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp @@ -14,6 +14,7 @@ #ifndef OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ #define OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ +#include "objects_of_interest_marker_interface/coloring.hpp" #include "objects_of_interest_marker_interface/marker_data.hpp" #include @@ -56,6 +57,18 @@ 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 diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp index 79bab6d5faf69..d5a19fede4fc2 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp @@ -78,6 +78,16 @@ class ObjectsOfInterestMarkerInterface */ 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_; diff --git a/planning/objects_of_interest_marker_interface/src/marker_utils.cpp b/planning/objects_of_interest_marker_interface/src/marker_utils.cpp index 71414eae4dbe7..23f33c0b8115a 100644 --- a/planning/objects_of_interest_marker_interface/src/marker_utils.cpp +++ b/planning/objects_of_interest_marker_interface/src/marker_utils.cpp @@ -35,7 +35,7 @@ Marker createArrowMarker( const double head_height = 0.5 * arrow_length; Marker marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, Marker::ARROW, + "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; @@ -77,19 +77,37 @@ Marker createCircleMarker( 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 + "_arrow", height_offset, arrow_length)); + 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; } diff --git a/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp index 6261b4791be48..03e71193cc5a5 100644 --- a/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp +++ b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -57,7 +57,7 @@ void ObjectsOfInterestMarkerInterface::publishMarkerArray() 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, name_, height_offset_); + marker_utils::createTargetMarker(i, data, getName(), getHeightOffset()); marker_array.markers.insert( marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end()); }