visualization

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

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -23,6 +23,11 @@
 
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/logging.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+#include <geometry_msgs/msg/detail/point32__struct.hpp>
+#include <geometry_msgs/msg/detail/polygon__struct.hpp>
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
 
 #include <boost/geometry/algorithms/centroid.hpp>
 #include <boost/geometry/strategies/cartesian/centroid_bashein_detmer.hpp>
@@ -114,31 +119,6 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
     sampleLongitudinalAccValues(status_.current_lanes, status_.target_lanes);
   const auto prepare_durations = calcPrepareDuration(status_.current_lanes, status_.target_lanes);
 
-  const auto create_point32 = [](const double x, const double y, const double z) {
-    geometry_msgs::msg::Point32 p;
-    p.x = x;
-    p.y = y;
-    p.z = z;
-    return p;
-  };
-
-  const auto create_vehicle_polygon =
-    [create_point32](const vehicle_info_util::VehicleInfo & vehicle_info, const double offset) {
-      const auto & i = vehicle_info;
-      const auto & front_m = i.max_longitudinal_offset_m;
-      const auto & width_m = i.vehicle_width_m / 2.0 + offset;
-      const auto & back_m = i.rear_overhang_m;
-
-      geometry_msgs::msg::Polygon polygon{};
-
-      polygon.points.push_back(create_point32(front_m, -width_m, 0.0));
-      polygon.points.push_back(create_point32(front_m, width_m, 0.0));
-      polygon.points.push_back(create_point32(-back_m, width_m, 0.0));
-      polygon.points.push_back(create_point32(-back_m, -width_m, 0.0));
-
-      return polygon;
-    };
-
   const auto max_offset = std::invoke([&]() -> double {
     auto max_offset{0.0};
     for (const auto & [type, p] : avoidance_parameters_->object_parameters) {
@@ -149,9 +129,55 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
     return max_offset;
   });
 
+  auto create_point32 = [](const geometry_msgs::msg::Pose & pose) -> geometry_msgs::msg::Point32 {
+    geometry_msgs::msg::Point32 p;
+    p.x = static_cast<float>(pose.position.x);
+    p.y = static_cast<float>(pose.position.y);
+    p.z = static_cast<float>(pose.position.z);
+    return p;
+  };
+
+  const auto create_vehicle_polygon =
+    [&](const vehicle_info_util::VehicleInfo & vehicle_info, const double offset) {
+      const double & base_to_front = vehicle_info.max_longitudinal_offset_m;
+      const double & width = vehicle_info.vehicle_width_m;
+      const double & base_to_rear = vehicle_info.rear_overhang_m;
+
+      // if stationary object, extend forward and backward by the half of lon length
+      const double forward_lon_offset =
+        base_to_front + std::max(minimum_lane_change_length, minimum_avoid_distance);
+      const double backward_lon_offset = -base_to_rear;
+      const double lat_offset = width / 2.0 + offset;
+
+      const auto & base_link_pose = getEgoPose();
+
+      const auto p1 =
+        tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0);
+      const auto p2 =
+        tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0);
+      const auto p3 =
+        tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0);
+      const auto p4 =
+        tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0);
+      geometry_msgs::msg::Polygon polygon;
+
+      polygon.points.push_back(create_point32(p1));
+      polygon.points.push_back(create_point32(p2));
+      polygon.points.push_back(create_point32(p3));
+      polygon.points.push_back(create_point32(p4));
+
+      return polygon;
+    };
+
   debug_execution_.effective_area =
     create_vehicle_polygon(getCommonParam().vehicle_info, max_offset);
 
+  RCLCPP_DEBUG(logger_, "ego pose.x %.3f pose.y %.3f", getEgoPosition().x, getEgoPosition().y);
+  RCLCPP_DEBUG(
+    logger_, "effective_area p1.x %.3f, p1.y %.3f",
+    debug_execution_.effective_area.points.front().x,
+    debug_execution_.effective_area.points.front().y);
+
   RCLCPP_DEBUG(
     logger_,
     "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, minimum_avoid_distance "

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -343,8 +343,6 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   const auto debug_filtered_objects = module_type_->getDebugFilteredObjects();
   const auto debug_execution = module_type_->getDebugExecution();
 
-  RCLCPP_WARN(logger_, "%ld", debug_execution.effective_area.points.size());
-
   debug_marker_.markers.clear();
   const auto add = [this](const MarkerArray & added) {
     tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);