fix(obstacle_stop_planner): fix the issues when use_predicted_objects…

… true (autowarefoundation#3556)

Signed-off-by: Berkay Karaman <[email protected]>

planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp

@@ -16,6 +16,7 @@
 #define OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
@@ -32,6 +33,7 @@ namespace motion_planning
 {
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using autoware_auto_perception_msgs::msg::PredictedObject;
 class AdaptiveCruiseController
 {
 public:
@@ -47,6 +49,14 @@ class AdaptiveCruiseController
     const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop,
     TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header);
 
+  void insertAdaptiveCruiseVelocity(
+    const TrajectoryPoints & trajectory, const int nearest_collision_point_idx,
+    const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point,
+    const rclcpp::Time nearest_collision_point_time,
+    const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop,
+    TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header,
+    const PredictedObject & target_object);
+
 private:
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr pub_debug_;
 
@@ -187,6 +197,8 @@ class AdaptiveCruiseController
   bool estimatePointVelocityFromPcl(
     const double traj_yaw, const pcl::PointXYZ & nearest_collision_point,
     const rclcpp::Time & nearest_collision_point_time, double * velocity);
+  void calculateProjectedVelocityFromObject(
+    const PredictedObject & object, const double traj_yaw, double * velocity);
   double estimateRoughPointVelocity(double current_vel);
   bool isObstacleVelocityHigh(const double obj_vel);
   double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel);

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

@@ -88,7 +88,7 @@ using vehicle_info_util::VehicleInfo;
 
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
 using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
-
+using autoware_auto_perception_msgs::msg::PredictedObject;
 struct ObstacleWithDetectionTime
 {
   explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p)
@@ -102,13 +102,15 @@ struct ObstacleWithDetectionTime
 
 struct PredictedObjectWithDetectionTime
 {
-  explicit PredictedObjectWithDetectionTime(const rclcpp::Time & t, Pose & p)
-  : detection_time(t), point(p)
+  explicit PredictedObjectWithDetectionTime(
+    const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj)
+  : detection_time(t), point(p), object(std::move(obj))
   {
   }
 
   rclcpp::Time detection_time;
-  Pose point;
+  geometry_msgs::msg::Point point;
+  PredictedObject object;
 };
 
 class ObstacleStopPlannerNode : public rclcpp::Node
@@ -173,8 +175,9 @@ class ObstacleStopPlannerNode : public rclcpp::Node
     const StopParam & stop_param, const PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr);
 
   void searchPredictedObject(
-    const TrajectoryPoints & decimate_trajectory, PlannerData & planner_data,
-    const VehicleInfo & vehicle_info, const StopParam & stop_param);
+    const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output,
+    PlannerData & planner_data, const Header & trajectory_header, const VehicleInfo & vehicle_info,
+    const StopParam & stop_param);
 
   void insertVelocity(
     TrajectoryPoints & trajectory, PlannerData & planner_data, const Header & trajectory_header,

planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp

@@ -17,6 +17,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <geometry_msgs/msg/pose.hpp>
@@ -249,6 +250,8 @@ struct PlannerData
 
   pcl::PointXYZ lateral_nearest_slow_down_point;
 
+  autoware_auto_perception_msgs::msg::Shape slow_down_object_shape{};
+
   Pose nearest_collision_point_pose{};
 
   Pose nearest_slow_down_point_pose{};

planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp

@@ -21,13 +21,15 @@
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
 
 #include <boost/optional.hpp>
+#include <boost/variant.hpp>
 
 #include <map>
 #include <string>
@@ -54,6 +56,9 @@ using vehicle_info_util::VehicleInfo;
 
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
 using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using PointVariant = boost::variant<float, double>;
 
 boost::optional<std::pair<double, double>> calcFeasibleMarginAndVelocity(
   const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle,
@@ -101,12 +106,12 @@ void getLateralNearestPoint(
   const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point,
   double * deviation);
 
-void getNearestPointForPredictedObject(
-  const PoseArray & object, const Pose & base_pose, Pose * nearest_collision_point,
-  rclcpp::Time * nearest_collision_point_time);
+double getNearestPointAndDistanceForPredictedObject(
+  const geometry_msgs::msg::PoseArray & points, const Pose & base_pose,
+  geometry_msgs::msg::Point * nearest_collision_point);
 
 void getLateralNearestPointForPredictedObject(
-  const PoseArray & object, const Pose & base_pose, Pose * lateral_nearest_point,
+  const PoseArray & object, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point,
   double * deviation);
 
 Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info);
@@ -137,6 +142,13 @@ TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double e
 TrajectoryPoint getExtendTrajectoryPoint(
   double extend_distance, const TrajectoryPoint & goal_point);
 
+bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max);
+
+pcl::PointXYZ pointToPcl(const double x, const double y, const double z);
+
+boost::optional<PredictedObject> getObstacleFromUuid(
+  const PredictedObjects & obstacles, const unique_identifier_msgs::msg::UUID & target_object_id);
+
 rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr);
 
 }  // namespace motion_planning

planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp

@@ -273,6 +273,68 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity(
   *need_to_stop = false;
 }
 
+void AdaptiveCruiseController::insertAdaptiveCruiseVelocity(
+  const TrajectoryPoints & trajectory, const int nearest_collision_point_idx,
+  const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point,
+  const rclcpp::Time nearest_collision_point_time,
+  const nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr, bool * need_to_stop,
+  TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header,
+  const PredictedObject & target_object)
+{
+  debug_values_.data.clear();
+  debug_values_.data.resize(num_debug_values_, 0.0);
+
+  const double current_velocity = current_odometry_ptr->twist.twist.linear.x;
+  double col_point_distance;
+  double point_velocity;
+  /*
+   * calc distance to collision point
+   */
+  calcDistanceToNearestPointOnPath(
+    trajectory, nearest_collision_point_idx, self_pose, nearest_collision_point,
+    nearest_collision_point_time, &col_point_distance, trajectory_header);
+
+  /*
+   * calc yaw of trajectory at collision point
+   */
+  const double traj_yaw = calcTrajYaw(trajectory, nearest_collision_point_idx);
+
+  /*
+   * estimate velocity of collision point
+   */
+  calculateProjectedVelocityFromObject(target_object, traj_yaw, &point_velocity);
+
+  // calculate max(target) velocity of self
+  const double upper_velocity =
+    calcUpperVelocity(col_point_distance, point_velocity, current_velocity);
+  pub_debug_->publish(debug_values_);
+
+  if (target_object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
+    // if the target object is obstacle return stop true
+    RCLCPP_DEBUG_THROTTLE(
+      node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(),
+      "Target object is pedestrian. Insert stop line.");
+    *need_to_stop = true;
+    return;
+  }
+
+  if (upper_velocity <= param_.thresh_vel_to_stop) {
+    // if upper velocity is too low, need to stop
+    RCLCPP_DEBUG_THROTTLE(
+      node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(),
+      "Upper velocity is too low. Insert stop line.");
+    *need_to_stop = true;
+    return;
+  }
+
+  /*
+   * insert max velocity
+   */
+  insertMaxVelocityToPath(
+    self_pose, current_velocity, upper_velocity, col_point_distance, output_trajectory);
+  *need_to_stop = false;
+}
+
 void AdaptiveCruiseController::calcDistanceToNearestPointOnPath(
   const TrajectoryPoints & trajectory, const int nearest_point_idx,
   const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point,
@@ -376,6 +438,17 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject(
   }
 }
 
+void AdaptiveCruiseController::calculateProjectedVelocityFromObject(
+  const PredictedObject & object, const double traj_yaw, double * velocity)
+{
+  /* get object velocity, and current yaw */
+  double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x;
+  double obj_yaw = tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation);
+
+  *velocity = obj_vel * std::cos(tier4_autoware_utils::normalizeRadian(obj_yaw - traj_yaw));
+  debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity;
+}
+
 bool AdaptiveCruiseController::estimatePointVelocityFromPcl(
   const double traj_yaw, const pcl::PointXYZ & nearest_collision_point,
   const rclcpp::Time & nearest_collision_point_time, double * velocity)