Merge pull request autowarefoundation#1048 from tier4/feat/objects_of…

…_interest_marker

feat(objects_of_interest_marker_interface): add objects of interest marker interface

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 <rclcpp/rclcpp.hpp>
 
@@ -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<PathWithLaneId> prev_approved_path_;
+  ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_;
 
   void clearAbortApproval() { is_abort_path_approved_ = false; }
 

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<PoseWithVelocityAndPolygonStamped> 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<std::string, CollisionCheckDebug>;
 using CollisionCheckDebugMap =

planning/behavior_path_planner/package.xml

@@ -57,6 +57,7 @@
   <depend>magic_enum</depend>
   <depend>motion_utils</depend>
   <depend>object_recognition_utils</depend>
+  <depend>objects_of_interest_marker_interface</depend>
   <depend>planning_test_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

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};
 }
 

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<PathWithLaneId>()}
+  prev_approved_path_{std::make_unique<PathWithLaneId>()},
+  objects_of_interest_marker_interface_{&node, name}
 {
   steering_factor_interface_ptr_ = std::make_unique<SteeringFactorInterface>(&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<LaneChangeDebugMsgArray> LaneChangeInterface::get_debug_msg_array() const
 {
   const auto debug_data = module_type_->getDebugData();

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;

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()

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

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 <tier4_autoware_utils/ros/marker_helper.hpp>
+
+#include <std_msgs/msg/color_rgba.hpp>
+
+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_

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 <autoware_auto_perception_msgs/msg/predicted_object.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+
+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_

planning/objects_of_interest_marker_interface/include/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 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 <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/math/constants.hpp>
+#include <tier4_autoware_utils/math/trigonometry.hpp>
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
+
+#include <autoware_auto_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 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 objects_of_interest_marker_interface::marker_utils
+
+#endif  // OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_