Merge branch 'main' into feature/smooth_ndt_map_update

Author: anhnv3991

planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp

@@ -26,6 +26,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <tf2/utils.h>
 
+#include <optional>
 #include <vector>
 
 namespace motion_planning
@@ -190,12 +191,12 @@ class AdaptiveCruiseController
     const rclcpp::Time & nearest_collision_point_time, double * distance,
     const std_msgs::msg::Header & trajectory_header);
   double calcTrajYaw(const TrajectoryPoints & trajectory, const int collision_point_idx);
-  bool estimatePointVelocityFromObject(
+  std::optional<double> estimatePointVelocityFromObject(
     const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr,
-    const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, double * velocity);
-  bool estimatePointVelocityFromPcl(
+    const double traj_yaw, const pcl::PointXYZ & nearest_collision_point);
+  std::optional<double> estimatePointVelocityFromPcl(
     const double traj_yaw, const pcl::PointXYZ & nearest_collision_point,
-    const rclcpp::Time & nearest_collision_point_time, double * velocity);
+    const rclcpp::Time & nearest_collision_point_time);
   void calculateProjectedVelocityFromObject(
     const PredictedObject & object, const double traj_yaw, double * velocity);
   double estimateRoughPointVelocity(double current_vel);

planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp

@@ -207,8 +207,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity(
 
   const double current_velocity = current_odometry_ptr->twist.twist.linear.x;
   double col_point_distance;
-  double point_velocity;
-  bool success_estimate_vel = false;
+  std::optional<double> point_velocity;
   /*
    * calc distance to collision point
    */
@@ -225,25 +224,19 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity(
    * estimate velocity of collision point
    */
   if (param_.use_pcl_to_est_vel) {
-    if (estimatePointVelocityFromPcl(
-          traj_yaw, nearest_collision_point, nearest_collision_point_time, &point_velocity)) {
-      success_estimate_vel = true;
-    }
+    point_velocity =
+      estimatePointVelocityFromPcl(traj_yaw, nearest_collision_point, nearest_collision_point_time);
   }
 
   if (param_.use_object_to_est_vel) {
-    if (estimatePointVelocityFromObject(
-          object_ptr, traj_yaw, nearest_collision_point, &point_velocity)) {
-      success_estimate_vel = true;
-    }
+    point_velocity = estimatePointVelocityFromObject(object_ptr, traj_yaw, nearest_collision_point);
   }
 
-  if (param_.use_rough_est_vel && !success_estimate_vel) {
+  if (param_.use_rough_est_vel && !point_velocity) {
     point_velocity = estimateRoughPointVelocity(current_velocity);
-    success_estimate_vel = true;
   }
 
-  if (!success_estimate_vel) {
+  if (!point_velocity) {
     // if failed to estimate velocity, need to stop
     RCLCPP_DEBUG_THROTTLE(
       node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(),
@@ -257,7 +250,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity(
 
   // calculate max(target) velocity of self
   const double upper_velocity =
-    calcUpperVelocity(col_point_distance, point_velocity, current_velocity);
+    calcUpperVelocity(col_point_distance, point_velocity.value(), current_velocity);
   pub_debug_->publish(debug_values_);
 
   if (upper_velocity <= param_.thresh_vel_to_stop) {
@@ -405,45 +398,47 @@ double AdaptiveCruiseController::calcTrajYaw(
   return tf2::getYaw(trajectory.at(collision_point_idx).pose.orientation);
 }
 
-bool AdaptiveCruiseController::estimatePointVelocityFromObject(
+std::optional<double> AdaptiveCruiseController::estimatePointVelocityFromObject(
   const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr,
-  const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, double * velocity)
+  const double traj_yaw, const pcl::PointXYZ & nearest_collision_point)
 {
   geometry_msgs::msg::Point nearest_collision_p_ros;
   nearest_collision_p_ros.x = nearest_collision_point.x;
   nearest_collision_p_ros.y = nearest_collision_point.y;
   nearest_collision_p_ros.z = nearest_collision_point.z;
 
   /* get object velocity, and current yaw */
-  bool get_obj = false;
-  double obj_vel_norm;
-  double obj_vel_yaw;
+  std::optional<std::pair<double, double>> obj_vel_norm_yaw = std::nullopt;
   const Point collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros);
-  for (const auto & obj : object_ptr->objects) {
-    const Polygon obj_poly = getPolygon(
-      obj.kinematics.initial_pose_with_covariance.pose, obj.shape.dimensions, 0.0,
-      param_.object_polygon_length_margin, param_.object_polygon_width_margin);
-    if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) {
-      obj_vel_norm = std::hypot(
-        obj.kinematics.initial_twist_with_covariance.twist.linear.x,
-        obj.kinematics.initial_twist_with_covariance.twist.linear.y);
-
-      const double obj_yaw =
-        tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation);
-      obj_vel_yaw = obj_yaw + std::atan2(
-                                obj.kinematics.initial_twist_with_covariance.twist.linear.y,
-                                obj.kinematics.initial_twist_with_covariance.twist.linear.x);
-      get_obj = true;
-      break;
+  if (object_ptr) {
+    for (const auto & obj : object_ptr->objects) {
+      const Polygon obj_poly = getPolygon(
+        obj.kinematics.initial_pose_with_covariance.pose, obj.shape.dimensions, 0.0,
+        param_.object_polygon_length_margin, param_.object_polygon_width_margin);
+      if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) {
+        const double obj_vel_norm = std::hypot(
+          obj.kinematics.initial_twist_with_covariance.twist.linear.x,
+          obj.kinematics.initial_twist_with_covariance.twist.linear.y);
+
+        const double obj_yaw =
+          tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation);
+        const double obj_vel_yaw =
+          obj_yaw + std::atan2(
+                      obj.kinematics.initial_twist_with_covariance.twist.linear.y,
+                      obj.kinematics.initial_twist_with_covariance.twist.linear.x);
+        obj_vel_norm_yaw = std::make_pair(obj_vel_norm, obj_vel_yaw);
+        break;
+      }
     }
   }
 
-  if (get_obj) {
-    *velocity = obj_vel_norm * std::cos(obj_vel_yaw - traj_yaw);
-    debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity;
-    return true;
+  if (obj_vel_norm_yaw) {
+    const auto [obj_vel_norm, obj_vel_yaw] = obj_vel_norm_yaw.value();
+    const double velocity = obj_vel_norm * std::cos(obj_vel_yaw - traj_yaw);
+    debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = velocity;
+    return velocity;
   } else {
-    return false;
+    return std::nullopt;
   }
 }
 
@@ -467,9 +462,9 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject(
   debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity;
 }
 
-bool AdaptiveCruiseController::estimatePointVelocityFromPcl(
+std::optional<double> AdaptiveCruiseController::estimatePointVelocityFromPcl(
   const double traj_yaw, const pcl::PointXYZ & nearest_collision_point,
-  const rclcpp::Time & nearest_collision_point_time, double * velocity)
+  const rclcpp::Time & nearest_collision_point_time)
 {
   geometry_msgs::msg::Point nearest_collision_p_ros;
   nearest_collision_p_ros.x = nearest_collision_point.x;
@@ -487,7 +482,7 @@ bool AdaptiveCruiseController::estimatePointVelocityFromPcl(
       prev_collision_point_time_ = nearest_collision_point_time;
       prev_collision_point_ = nearest_collision_point;
       prev_collision_point_valid_ = true;
-      return false;
+      return std::nullopt;
     }
     const double p_dx = nearest_collision_point.x - prev_collision_point_.x;
     const double p_dy = nearest_collision_point.y - prev_collision_point_.y;
@@ -501,22 +496,22 @@ bool AdaptiveCruiseController::estimatePointVelocityFromPcl(
       prev_collision_point_ = nearest_collision_point;
       prev_collision_point_valid_ = true;
       est_vel_que_.clear();
-      return false;
+      return std::nullopt;
     }
 
     // append new velocity and remove old velocity from que
     registerQueToVelocity(est_velocity, nearest_collision_point_time);
   }
 
   // calc average(median) velocity from que
-  *velocity = getMedianVel(est_vel_que_);
-  debug_values_.data.at(DBGVAL::ESTIMATED_VEL_PCL) = *velocity;
+  const double velocity = getMedianVel(est_vel_que_);
+  debug_values_.data.at(DBGVAL::ESTIMATED_VEL_PCL) = velocity;
 
   prev_collision_point_time_ = nearest_collision_point_time;
   prev_collision_point_ = nearest_collision_point;
-  prev_target_velocity_ = *velocity;
+  prev_target_velocity_ = velocity;
   prev_collision_point_valid_ = true;
-  return true;
+  return velocity;
 }
 
 double AdaptiveCruiseController::estimateRoughPointVelocity(double current_vel)