feat(behavior_path_planner): lane change revised visualization

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_planner/CMakeLists.txt

@@ -22,6 +22,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/scene_module/lane_following/lane_following_module.cpp
   src/scene_module/lane_change/lane_change_module.cpp
   src/scene_module/lane_change/util.cpp
+  src/scene_module/lane_change/debug.cpp
   src/scene_module/pull_over/pull_over_module.cpp
   src/scene_module/pull_over/util.cpp
   src/scene_module/pull_out/pull_out_module.cpp

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -24,3 +24,4 @@
       lateral_distance_threshold: 5.0
       expected_front_deceleration: -1.0
       expected_rear_deceleration: -1.0
+      publish_debug_marker: true

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/debug.hpp

@@ -0,0 +1,88 @@
+// Copyright 2022 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_
+
+#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
+
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace marker_utils
+{
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_perception_msgs::msg::PredictedPath;
+using behavior_path_planner::LaneChangePath;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::Twist;
+using tier4_autoware_utils::Polygon2d;
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+namespace lane_change_markers
+{
+
+struct LaneChangeObjectDebug
+{
+  std::string failed_reason;
+  std::size_t lane_id;
+  Pose current_pose;
+  Twist current_twist;
+  Pose relative_to_ego;
+  bool is_front;
+  bool allow_lane_change;
+  std::vector<Pose> lerped_path;
+  std::vector<PredictedPath> ego_predicted_path;
+  Polygon2d ego_polygon;
+};
+
+constexpr std::array<std::array<float, 3>, 10> colorsList()
+{
+  constexpr std::array<float, 3> red = {1., 0., 0.};
+  constexpr std::array<float, 3> green = {0., 1., 0.};
+  constexpr std::array<float, 3> blue = {0., 0., 1.};
+  constexpr std::array<float, 3> yellow = {1., 1., 0.};
+  constexpr std::array<float, 3> aqua = {0., 1., 1.};
+  constexpr std::array<float, 3> magenta = {1., 0., 1.};
+  constexpr std::array<float, 3> medium_orchid = {0.729, 0.333, 0.827};
+  constexpr std::array<float, 3> light_pink = {1, 0.713, 0.756};
+  constexpr std::array<float, 3> light_yellow = {1, 1, 0.878};
+  constexpr std::array<float, 3> light_steel_blue = {0.690, 0.768, 0.870};
+  return {red,     green,         blue,       yellow,       aqua,
+          magenta, medium_orchid, light_pink, light_yellow, light_steel_blue};
+}
+
+Marker initializeWithFrameStampAndNamespace(std::string && frame_id, std::string && ns);
+MarkerArray showObjectInfo(
+  const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns);
+MarkerArray showAllLaneChangeLanes(const std::vector<LaneChangePath> & lanes, std::string && ns);
+MarkerArray showLerpedPose(
+  const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns);
+MarkerArray showEgoPredictedPaths(
+  const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns);
+MarkerArray showEgoPolygon(
+  const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns);
+}  // namespace lane_change_markers
+}  // namespace marker_utils
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp

@@ -15,6 +15,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_
 
+#include "behavior_path_planner/scene_module/lane_change/debug.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/utilities.hpp"
@@ -31,12 +32,14 @@
 
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using marker_utils::lane_change_markers::LaneChangeObjectDebug;
 
 struct LaneChangeParameters
 {
@@ -66,6 +69,7 @@ struct LaneChangeParameters
   double lateral_distance_threshold;
   double expected_front_deceleration;
   double expected_rear_deceleration;
+  bool publish_debug_marker;
 };
 
 struct LaneChangeStatus
@@ -181,6 +185,10 @@ class LaneChangeModule : public SceneModuleInterface
   bool isCurrentSpeedLow() const;
   bool isAbortConditionSatisfied() const;
   bool hasFinishedLaneChange() const;
+
+  void setObjectDebugVisualization() const;
+  mutable std::unordered_map<std::string, LaneChangeObjectDebug> object_debug_;
+  mutable std::vector<LaneChangePath> candidate_path_;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp

@@ -29,6 +29,8 @@
 #include <lanelet2_core/primitives/Primitive.h>
 
 #include <memory>
+#include <string>
+#include <unordered_map>
 #include <vector>
 
 namespace behavior_path_planner::lane_change_utils
@@ -61,13 +63,15 @@ bool selectSafePath(
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
   const Twist & current_twist, const double & vehicle_width, const double & vehicle_length,
   const behavior_path_planner::LaneChangeParameters & ros_parameters,
-  LaneChangePath * selected_path);
+  LaneChangePath * selected_path,
+  std::unordered_map<std::string, LaneChangeObjectDebug> & debug_data);
 bool isLaneChangePathSafe(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
   const Twist & current_twist, const double & vehicle_width, const double & vehicle_length,
-  const behavior_path_planner::LaneChangeParameters & ros_parameters, const bool use_buffer = true,
+  const behavior_path_planner::LaneChangeParameters & ros_parameters,
+  std::unordered_map<std::string, LaneChangeObjectDebug> & debug_data, const bool use_buffer = true,
   const double acceleration = 0.0);
 bool hasEnoughDistance(
   const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -241,7 +241,7 @@ class SceneModuleInterface
   rclcpp::Logger logger_;
 
 protected:
-  MarkerArray debug_marker_;
+  mutable MarkerArray debug_marker_;
   rclcpp::Clock::SharedPtr clock_;
   mutable AvoidanceDebugMsgArray::SharedPtr debug_avoidance_msg_array_ptr_{};
 

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -365,6 +365,8 @@ Polygon2d convertCylindricalObjectToGeometryPolygon(
   const Pose & current_pose, const Shape & obj_shape);
 
 Polygon2d convertPolygonObjectToGeometryPolygon(const Pose & current_pose, const Shape & obj_shape);
+
+std::string getUuidStr(const PredictedObject & obj);
 }  // namespace util
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -339,6 +339,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.lateral_distance_threshold = dp("lateral_distance_threshold", 5.0);
   p.expected_front_deceleration = dp("expected_front_deceleration", -1.0);
   p.expected_rear_deceleration = dp("expected_rear_deceleration", -1.0);
+  p.publish_debug_marker = dp("publish_debug_marker", true);
 
   // validation of parameters
   if (p.lane_change_sampling_num < 1) {

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

@@ -0,0 +1,188 @@
+// Copyright 2022 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 "behavior_path_planner/path_shifter/path_shifter.hpp"
+
+#include <behavior_path_planner/scene_module/lane_change/debug.hpp>
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+
+#include <cstdlib>
+#include <iomanip>
+#include <string>
+#include <vector>
+
+namespace marker_utils
+{
+namespace lane_change_markers
+{
+using geometry_msgs::msg::Point;
+
+Marker initializeWithFrameStampAndNamespace(std::string && frame_id, std::string && ns)
+{
+  Marker marker{};
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now();
+  marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+  marker.ns = ns.c_str();
+  return marker;
+}
+
+MarkerArray showObjectInfo(
+  const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns)
+{
+  MarkerArray marker_array;
+
+  constexpr float r{1.0};
+  constexpr float g{0.3};
+  constexpr float b{1.0};
+  constexpr float alpha{0.99};
+  int32_t id{0};
+  for (const auto & obj : obj_debug_vec) {
+    Marker marker = initializeWithFrameStampAndNamespace("map", std::move(ns));
+    marker.id = ++id;
+    marker.type = Marker::TEXT_VIEW_FACING;
+    marker.action = Marker::ADD;
+    marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0);
+    marker.pose = obj.second.current_pose;
+    marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, alpha);
+    std::ostringstream ss;
+    ss << obj.second.failed_reason << "\nLon: " << std::setprecision(4)
+       << obj.second.relative_to_ego.position.x
+       << "\nLat: " << obj.second.relative_to_ego.position.y;
+    marker.text = ss.str();
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+}
+
+MarkerArray showAllLaneChangeLanes(const std::vector<LaneChangePath> & lanes, std::string && ns)
+{
+  MarkerArray marker_array;
+  int32_t id{0};
+
+  constexpr auto colors = colorsList();
+  const auto loop_size = std::min(lanes.size(), colors.size());
+
+  float scale_val = 0.1;
+  // float minus_res = 0.1;
+
+  for (std::size_t idx = 0; idx < loop_size; ++idx) {
+    Marker marker = initializeWithFrameStampAndNamespace("map", std::move(ns));
+    marker.id = ++id;
+    marker.type = Marker::LINE_STRIP;
+    marker.action = Marker::ADD;
+    marker.scale = tier4_autoware_utils::createMarkerScale(scale_val, scale_val, 0.0);
+    marker.color =
+      tier4_autoware_utils::createMarkerColor(colors[idx][0], colors[idx][1], colors[idx][2], 0.9);
+    for (const auto & point : lanes.at(idx).path.points) {
+      marker.points.push_back(point.point.pose.position);
+    }
+    // scale_val = scale_val - minus_res;
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+}
+
+MarkerArray showLerpedPose(
+  const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns)
+{
+  MarkerArray marker_array;
+
+  constexpr float r{1.0};
+  constexpr float g{0.3};
+  constexpr float b{1.0};
+  constexpr float alpha{0.99};
+  constexpr float scale = 0.5;
+  int32_t id{0};
+  for (const auto & obj : obj_debug_vec) {
+    Marker marker = initializeWithFrameStampAndNamespace("map", std::move(ns));
+    marker.id = ++id;
+    marker.type = Marker::POINTS;
+    marker.action = Marker::ADD;
+    marker.scale = tier4_autoware_utils::createMarkerScale(scale, scale, scale);
+    marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, alpha);
+    for (const auto & point : obj.second.lerped_path) {
+      marker.points.push_back(point.position);
+    }
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+}
+
+MarkerArray showEgoPredictedPaths(
+  const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns)
+{
+  MarkerArray marker_array;
+  constexpr auto colors = colorsList();
+
+  constexpr float scale_val = 0.2;
+  // float minus_res = 0.1;
+
+  int32_t id{0};
+  for (const auto & obj : obj_debug_vec) {
+    const auto loop_size = std::min(obj.second.ego_predicted_path.size(), colors.size());
+    for (std::size_t idx = 0; idx < loop_size; ++idx) {
+      const auto lane = obj.second.ego_predicted_path.at(idx);
+      Marker marker = initializeWithFrameStampAndNamespace("map", std::move(ns));
+      marker.id = ++id;
+      marker.type = Marker::POINTS;
+      marker.action = Marker::ADD;
+      marker.scale = tier4_autoware_utils::createMarkerScale(scale_val, scale_val, 0.0);
+      marker.color = tier4_autoware_utils::createMarkerColor(
+        colors[idx][0], colors[idx][1], colors[idx][2], 0.9);
+      for (const auto & point : lane.path) {
+        marker.points.push_back(point.position);
+      }
+      // scale_val = scale_val - minus_res;
+      marker_array.markers.push_back(marker);
+    }
+  }
+  return marker_array;
+}
+
+MarkerArray showEgoPolygon(
+  const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns)
+{
+  if (obj_debug_vec.empty()) {
+    return MarkerArray{};
+  }
+  Marker marker = initializeWithFrameStampAndNamespace("map", std::move(ns));
+  marker.type = Marker::LINE_STRIP;
+  marker.action = Marker::ADD;
+  marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.0, 0.0);
+  marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.8);
+  int32_t id{0};
+
+  MarkerArray marker_array;
+  marker_array.markers.reserve(obj_debug_vec.size());
+  for (const auto & obj_datum : obj_debug_vec) {
+    const auto & ego_polygon = obj_datum.second.ego_polygon.outer();
+    if (ego_polygon.empty()) {
+      continue;
+    }
+    marker.id = ++id;
+    marker.points.reserve(ego_polygon.size());
+    for (const auto & p : ego_polygon) {
+      marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0));
+    }
+    marker_array.markers.push_back(marker);
+  }
+
+  return marker_array;
+}
+}  // namespace lane_change_markers
+}  // namespace marker_utils