refactor(autoware_autonomous_emergency_braking): add getObjectOnPathD…

…ata and getLongitudinalOffset functions (#9462) * feat(aeb): add getObjectOnPathData and getLongitudinalOffset functions Signed-off-by: kyoichi-sugahara <[email protected]> --------- Signed-off-by: Kyoichi Sugahara <[email protected]>
autowarefoundation · Nov 28, 2024 · 710dacc · 710dacc
1 parent ecdb3f5
commit 710dacc
Show file tree

Hide file tree

Showing 4 changed files with 166 additions and 48 deletions.
diff --git a/...ware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/...ware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp
@@ -15,9 +15,12 @@
 #ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_
 #define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_
 
+#include "autoware/autonomous_emergency_braking/node.hpp"
+
 #include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
 
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
+#include <geometry_msgs/msg/detail/point__struct.hpp>
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
@@ -49,6 +52,26 @@ using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::TransformStamped;
 
+/**
+ * @brief Get the Object data of an obstacle inside the ego path
+ * @param path the ego path
+ * @param front_offset offset from ego baselink to front
+ * @param rear_offset offset from ego baselink to back
+ */
+std::optional<ObjectData> getObjectOnPathData(
+  const std::vector<Pose> & ego_path, const geometry_msgs::msg::Point & obj_position,
+  const rclcpp::Time & stamp, const double path_length, const double path_width,
+  const double path_expansion, const double longitudinal_offset, const double object_speed = 0.0);
+
+/**
+ * @brief Get the longitudinal offset based on vehicle traveling direction
+ * @param path the ego path
+ * @param front_offset offset from ego baselink to front
+ * @param rear_offset offset from ego baselink to back
+ */
+std::optional<double> getLongitudinalOffset(
+  const std::vector<Pose> & path, const double front_offset, const double rear_offset);
+
 /**
  * @brief Apply a transform to a predicted object
  * @param input the predicted object

diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp
@@ -815,6 +815,13 @@ void AEB::createObjectDataUsingPredictedObjects(
   const auto transform_stamped_opt =
     utils::getTransform("base_link", predicted_objects_ptr_->header.frame_id, tf_buffer_, logger);
   if (!transform_stamped_opt.has_value()) return;
+
+  const auto longitudinal_offset_opt = utils::getLongitudinalOffset(
+    ego_path, vehicle_info_.max_longitudinal_offset_m, vehicle_info_.rear_overhang_m);
+
+  if (!longitudinal_offset_opt.has_value()) return;
+  const auto longitudinal_offset = longitudinal_offset_opt.value();
+
   // Check which objects collide with the ego footprints
   std::for_each(objects.begin(), objects.end(), [&](const auto & predicted_object) {
     // get objects in base_link frame
@@ -842,10 +849,7 @@ void AEB::createObjectDataUsingPredictedObjects(
 
         // If the object is behind the ego, we need to use the backward long offset. The
         // distance should be a positive number in any case
-        const bool is_object_in_front_of_ego = obj_arc_length > 0.0;
-        const double dist_ego_to_object =
-          (is_object_in_front_of_ego) ? obj_arc_length - vehicle_info_.max_longitudinal_offset_m
-                                      : obj_arc_length + vehicle_info_.min_longitudinal_offset_m;
+        const double dist_ego_to_object = obj_arc_length - longitudinal_offset;
 
         ObjectData obj;
         obj.stamp = stamp;
@@ -928,53 +932,21 @@ void AEB::getClosestObjectsOnPath(
   if (ego_path.size() < 2 || points_belonging_to_cluster_hulls->empty()) {
     return;
   }
-  const auto ego_is_driving_forward_opt = autoware::motion_utils::isDrivingForward(ego_path);
-  if (!ego_is_driving_forward_opt.has_value()) {
-    return;
-  }
-  const bool ego_is_driving_forward = ego_is_driving_forward_opt.value();
-  // select points inside the ego footprint path
-  const auto current_p = [&]() {
-    const auto & first_point_of_path = ego_path.front();
-    const auto & p = first_point_of_path.position;
-    return autoware::universe_utils::createPoint(p.x, p.y, p.z);
-  }();
 
+  const auto longitudinal_offset_opt = utils::getLongitudinalOffset(
+    ego_path, vehicle_info_.max_longitudinal_offset_m, vehicle_info_.rear_overhang_m);
+
+  if (!longitudinal_offset_opt.has_value()) return;
+  const auto longitudinal_offset = longitudinal_offset_opt.value();
   const auto path_length = autoware::motion_utils::calcArcLength(ego_path);
+  const auto path_width = vehicle_info_.vehicle_width_m / 2.0 + expand_width_;
+  // select points inside the ego footprint path
   for (const auto & p : *points_belonging_to_cluster_hulls) {
     const auto obj_position = autoware::universe_utils::createPoint(p.x, p.y, p.z);
-    const double obj_arc_length =
-      autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position);
-    if (
-      std::isnan(obj_arc_length) ||
-      obj_arc_length > path_length + vehicle_info_.max_longitudinal_offset_m)
-      continue;
-
-    // calculate the lateral offset between the ego vehicle and the object
-    const double lateral_offset =
-      std::abs(autoware::motion_utils::calcLateralOffset(ego_path, obj_position));
-
-    if (std::isnan(lateral_offset)) continue;
-
-    // object is outside region of interest
-    if (
-      lateral_offset >
-      vehicle_info_.vehicle_width_m / 2.0 + expand_width_ + speed_calculation_expansion_margin_) {
-      continue;
-    }
-
-    // If the object is behind the ego, we need to use the backward long offset. The distance should
-    // be a positive number in any case
-    const double dist_ego_to_object = (ego_is_driving_forward)
-                                        ? obj_arc_length - vehicle_info_.max_longitudinal_offset_m
-                                        : obj_arc_length + vehicle_info_.min_longitudinal_offset_m;
-    ObjectData obj;
-    obj.stamp = stamp;
-    obj.position = obj_position;
-    obj.velocity = 0.0;
-    obj.distance_to_object = std::abs(dist_ego_to_object);
-    obj.is_target = (lateral_offset < vehicle_info_.vehicle_width_m / 2.0 + expand_width_);
-    objects.push_back(obj);
+    auto obj_data_opt = utils::getObjectOnPathData(
+      ego_path, obj_position, stamp, path_length, path_width, speed_calculation_expansion_margin_,
+      longitudinal_offset, 0.0);
+    if (obj_data_opt.has_value()) objects.push_back(obj_data_opt.value());
   }
 }
 

diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp
@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <autoware/autonomous_emergency_braking/utils.hpp>
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
 #include <optional>
@@ -27,6 +28,55 @@ using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::TransformStamped;
 using geometry_msgs::msg::Vector3;
 
+std::optional<ObjectData> getObjectOnPathData(
+  const std::vector<Pose> & ego_path, const geometry_msgs::msg::Point & obj_position,
+  const rclcpp::Time & stamp, const double path_length, const double path_width,
+  const double path_expansion, const double longitudinal_offset, const double object_speed)
+{
+  const auto current_p = [&]() {
+    const auto & p = ego_path.front().position;
+    return autoware::universe_utils::createPoint(p.x, p.y, p.z);
+  }();
+  const double obj_arc_length =
+    autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position);
+  if (
+    std::isnan(obj_arc_length) || obj_arc_length < 0.0 ||
+    obj_arc_length > path_length + longitudinal_offset) {
+    return {};
+  }
+
+  // calculate the lateral offset between the ego vehicle and the object
+  const double lateral_offset =
+    std::abs(autoware::motion_utils::calcLateralOffset(ego_path, obj_position));
+
+  // object is outside region of interest
+  if (std::isnan(lateral_offset) || lateral_offset > path_width + path_expansion) {
+    return {};
+  }
+
+  // If the object is behind the ego, we need to use the backward long offset. The distance should
+  // be a positive number in any case
+  const double dist_ego_to_object = obj_arc_length - longitudinal_offset;
+  ObjectData obj;
+  obj.stamp = stamp;
+  obj.position = obj_position;
+  obj.velocity = object_speed;
+  obj.distance_to_object = std::abs(dist_ego_to_object);
+  obj.is_target = (lateral_offset < path_width);
+  return obj;
+}
+
+std::optional<double> getLongitudinalOffset(
+  const std::vector<Pose> & ego_path, const double front_offset, const double rear_offset)
+{
+  const auto ego_is_driving_forward_opt = autoware::motion_utils::isDrivingForward(ego_path);
+  if (!ego_is_driving_forward_opt.has_value()) {
+    return {};
+  }
+  const bool ego_is_driving_forward = ego_is_driving_forward_opt.value();
+  return (ego_is_driving_forward) ? front_offset : rear_offset;
+}
+
 PredictedObject transformObjectFrame(
   const PredictedObject & input, const geometry_msgs::msg::TransformStamped & transform_stamped)
 {

diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp
@@ -21,12 +21,14 @@
 #include <rclcpp/time.hpp>
 
 #include <autoware_perception_msgs/msg/detail/shape__struct.hpp>
+#include <geometry_msgs/msg/detail/point__struct.hpp>
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
 
 #include <gtest/gtest.h>
 #include <pcl/memory.h>
 #include <tf2/LinearMath/Transform.h>
 
+#include <limits>
 #include <memory>
 #include <thread>
 
@@ -126,6 +128,76 @@ TEST_F(TestAEB, checkCollision)
   ASSERT_FALSE(aeb_node_->hasCollision(longitudinal_velocity, object_no_collision));
 }
 
+TEST_F(TestAEB, getObjectOnPathData)
+{
+  constexpr double longitudinal_velocity = 3.0;
+  constexpr double yaw_rate = 0.0;
+  const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate);
+  ASSERT_FALSE(imu_path.empty());
+
+  const double path_width = 2.0;
+  const auto path_length = autoware::motion_utils::calcArcLength(imu_path);
+  ASSERT_TRUE(
+    path_length < aeb_node_->max_generated_imu_path_length_ +
+                    aeb_node_->imu_prediction_time_interval_ * longitudinal_velocity +
+                    std::numeric_limits<double>::epsilon());
+
+  const auto stamp = rclcpp::Time();
+
+  const auto longitudinal_offset_opt = utils::getLongitudinalOffset(imu_path, 1.0, -1.0);
+  ASSERT_TRUE(longitudinal_offset_opt.has_value());
+  const auto longitudinal_offset = longitudinal_offset_opt.value();
+  ASSERT_DOUBLE_EQ(longitudinal_offset, 1.0);
+
+  // Object in path if longitudinal_offset is considered
+  Point obj_position;
+  double path_expansion;
+
+  {
+    obj_position.x = path_length + std::numeric_limits<double>::epsilon();
+    obj_position.y = 1.0;
+    path_expansion = 0.0;
+    auto obj_data_opt = utils::getObjectOnPathData(
+      imu_path, obj_position, stamp, path_length, path_width, path_expansion, longitudinal_offset,
+      0.0);
+    ASSERT_TRUE(obj_data_opt.has_value());
+    ASSERT_TRUE(obj_data_opt.value().is_target);
+  }
+
+  // Object outside of path
+  {
+    obj_position.x = path_length + std::numeric_limits<double>::epsilon();
+    obj_position.y = 3.0;
+    path_expansion = 0.0;
+    auto obj_data_opt = utils::getObjectOnPathData(
+      imu_path, obj_position, stamp, path_length, path_width, path_expansion, longitudinal_offset,
+      0.0);
+    ASSERT_FALSE(obj_data_opt.has_value());
+  }
+
+  // Object outside of path
+  {
+    obj_position.x = -1.0;
+    obj_position.y = 0.0;
+    path_expansion = 0.0;
+    auto obj_data_opt = utils::getObjectOnPathData(
+      imu_path, obj_position, stamp, path_length, path_width, path_expansion, longitudinal_offset,
+      0.0);
+    ASSERT_FALSE(obj_data_opt.has_value());
+  }
+
+  // Object is covered by path expansion
+  {
+    obj_position.x = path_length + std::numeric_limits<double>::epsilon();
+    obj_position.y = 3.0;
+    path_expansion = 2.0;
+    auto obj_data_opt = utils::getObjectOnPathData(
+      imu_path, obj_position, stamp, path_length, path_width, 2.0, longitudinal_offset, 0.0);
+    ASSERT_TRUE(obj_data_opt.has_value());
+    ASSERT_FALSE(obj_data_opt.value().is_target);
+  }
+}
+
 TEST_F(TestAEB, checkImuPathGeneration)
 {
   constexpr double longitudinal_velocity = 3.0;
@@ -145,7 +217,7 @@ TEST_F(TestAEB, checkImuPathGeneration)
   pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_ptr =
     pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
   {
-    const double x_start{0.0};
+    const double x_start{0.5};
     const double y_start{0.0};
 
     for (size_t i = 0; i < 15; ++i) {
@@ -162,6 +234,7 @@ TEST_F(TestAEB, checkImuPathGeneration)
   MarkerArray debug_markers;
   aeb_node_->getPointsBelongingToClusterHulls(
     obstacle_points_ptr, points_belonging_to_cluster_hulls, debug_markers);
+  ASSERT_FALSE(points_belonging_to_cluster_hulls->empty());
   std::vector<ObjectData> objects;
   aeb_node_->getClosestObjectsOnPath(imu_path, stamp, points_belonging_to_cluster_hulls, objects);
   ASSERT_FALSE(objects.empty());