feat(interest_objects_marker_interface): add interest objects marker …

…interface

Signed-off-by: Fumiya Watanabe <[email protected]>

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 "interest_objects_marker_interface/interest_objects_marker_interface.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -48,6 +49,7 @@ namespace behavior_path_planner
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
+using interest_objects_marker_interface::InterestObjectsMarkerInterface;
 using tier4_planning_msgs::msg::LaneChangeDebugMsg;
 using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 
@@ -133,6 +135,8 @@ class LaneChangeInterface : public SceneModuleInterface
 
   void setObjectDebugVisualization() const;
 
+  void setObjectDebugVisualizationTmp();
+
   void updateSteeringFactorPtr(const BehaviorModuleOutput & output);
 
   void updateSteeringFactorPtr(
@@ -142,6 +146,7 @@ class LaneChangeInterface : public SceneModuleInterface
   mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_;
 
   std::unique_ptr<PathWithLaneId> prev_approved_path_;
+  InterestObjectsMarkerInterface interest_objects_marker_interface_;
 
   void clearAbortApproval() { is_abort_path_approved_ = false; }
 

planning/behavior_path_planner/package.xml

@@ -53,6 +53,7 @@
   <depend>behaviortree_cpp_v3</depend>
   <depend>freespace_planning_algorithms</depend>
   <depend>geometry_msgs</depend>
+  <depend>interest_objects_marker_interface</depend>
   <depend>interpolation</depend>
   <depend>lane_departure_checker</depend>
   <depend>lanelet2_extension</depend>

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -40,7 +40,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>()},
+  interest_objects_marker_interface_{&node, name}
 {
   steering_factor_interface_ptr_ = std::make_unique<SteeringFactorInterface>(&node, name);
 }
@@ -220,6 +221,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
     getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
   module_type_->updateLaneChangeStatus();
   setObjectDebugVisualization();
+  setObjectDebugVisualizationTmp();
+  interest_objects_marker_interface_.publishMarkerArray();
 
   // 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);
@@ -308,6 +311,15 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   }
 }
 
+void LaneChangeInterface::setObjectDebugVisualizationTmp()
+{
+  const auto debug_data = module_type_->getDebugData();
+  for (const auto & [uuid, data] : debug_data) {
+    interest_objects_marker_interface_.insertObjectStatus(data.current_obj_pose, 1.5, data.is_safe);
+  }
+  return;
+}
+
 std::shared_ptr<LaneChangeDebugMsgArray> LaneChangeInterface::get_debug_msg_array() const
 {
   const auto debug_data = module_type_->getDebugData();

planning/interest_objects_marker_interface/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.5)
+project(interest_objects_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(interest_objects_marker_interface SHARED
+  src/interest_objects_marker_interface.cpp
+)
+
+# Test
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

planning/interest_objects_marker_interface/README.md

@@ -0,0 +1,11 @@
+# Interest Objects Marker Interface
+
+## Purpose
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+## Assumptions / Known limits
+
+## Future extensions / Unimplemented parts

planning/interest_objects_marker_interface/include/interest_objects_marker_interface/interest_objects_marker_interface.hpp

@@ -0,0 +1,61 @@
+// 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 INTEREST_OBJECTS_MARKER_INTERFACE__INTEREST_OBJECTS_MARKER_INTERFACE_HPP_
+#define INTEREST_OBJECTS_MARKER_INTERFACE__INTEREST_OBJECTS_MARKER_INTERFACE_HPP_
+#include "rclcpp/rclcpp.hpp"
+
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <mutex>
+#include <string>
+#include <vector>
+
+namespace interest_objects_marker_interface
+{
+using geometry_msgs::msg::Pose;
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+
+struct ObjectStatus
+{
+  Pose pose{};
+  double height{0.0};
+  bool safe{false};
+};
+
+class InterestObjectsMarkerInterface
+{
+public:
+  InterestObjectsMarkerInterface(rclcpp::Node * node, const std::string & name);
+  void insertObjectStatus(const Pose & pose, const double obj_height, const bool safe);
+  void publishMarkerArray();
+
+private:
+  rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_;
+
+  std::vector<ObjectStatus> obj_status_array_;
+
+  std::string name_;
+  std::string topic_namespace_ = "/planning/interest_objects_marker";
+
+  mutable std::mutex mutex_;
+};
+
+}  // namespace interest_objects_marker_interface
+
+#endif  // INTEREST_OBJECTS_MARKER_INTERFACE__INTEREST_OBJECTS_MARKER_INTERFACE_HPP_

planning/interest_objects_marker_interface/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>interest_objects_marker_interface</name>
+  <version>0.1.0</version>
+  <description>The interest_objects_marker_interface package</description>
+
+  <maintainer email="[email protected]">Fumiya Watanabe</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Fumiya Watanabe</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

planning/interest_objects_marker_interface/src/interest_objects_marker_interface.cpp

@@ -0,0 +1,85 @@
+// 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 "interest_objects_marker_interface/interest_objects_marker_interface.hpp"
+namespace
+{
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+using interest_objects_marker_interface::ObjectStatus;
+using visualization_msgs::msg::Marker;
+
+using tier4_autoware_utils::createDefaultMarker;
+using tier4_autoware_utils::createMarkerColor;
+using tier4_autoware_utils::createMarkerScale;
+
+Marker createArrowMarker(const size_t & id, const ObjectStatus & status, const std::string & name)
+{
+  const float r = status.safe ? 0.0f : 0.75f;
+  const float g = status.safe ? 0.75f : 0.0f;
+  const float b = 0.0f;
+
+  Marker marker = createDefaultMarker(
+    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, Marker::ARROW,
+    createMarkerScale(0.25, 0.5, 0.5), createMarkerColor(r, g, b, 0.999));
+
+  Point src, dst;
+  src = status.pose.position;
+  src.z += status.height + 1.0;
+  dst = status.pose.position;
+  dst.z += status.height;
+
+  marker.points.push_back(src);
+  marker.points.push_back(dst);
+
+  return marker;
+}
+}  // namespace
+
+namespace interest_objects_marker_interface
+{
+
+InterestObjectsMarkerInterface::InterestObjectsMarkerInterface(
+  rclcpp::Node * node, const std::string & name)
+: name_{name}
+{
+  // Publisher
+  pub_marker_ = node->create_publisher<MarkerArray>(topic_namespace_ + "/" + name, 1);
+}
+
+void InterestObjectsMarkerInterface::insertObjectStatus(
+  const Pose & pose, const double obj_height, const bool safe)
+{
+  ObjectStatus status;
+  status.pose = pose;
+  status.height = obj_height;
+  status.safe = safe;
+
+  obj_status_array_.push_back(status);
+}
+
+void InterestObjectsMarkerInterface::publishMarkerArray()
+{
+  MarkerArray marker_array;
+  for (size_t i = 0; i < obj_status_array_.size(); ++i) {
+    const auto obj = obj_status_array_.at(i);
+    const Marker marker = createArrowMarker(i, obj, name_);
+    marker_array.markers.push_back(marker);
+  }
+
+  pub_marker_->publish(marker_array);
+  obj_status_array_.clear();
+}
+
+}  // namespace interest_objects_marker_interface