From bd28ab1447b3300dbe4216ec6ef462ad7d641bc4 Mon Sep 17 00:00:00 2001 From: yutaka Date: Mon, 13 Jun 2022 13:06:32 +0900 Subject: [PATCH 1/6] update --- .../pid_based_planner/pid_based_planner.hpp | 1 - .../planner_interface.hpp | 15 +++++++++++ .../optimization_based_planner.cpp | 27 +++++++++---------- .../pid_based_planner/pid_based_planner.cpp | 15 ----------- 4 files changed, 27 insertions(+), 31 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index 765f120965092..60edc0c793471 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -78,7 +78,6 @@ class PIDBasedPlanner : public PlannerInterface boost::optional & cruise_obstacle_info); double calcDistanceToObstacle( const ObstacleCruisePlannerData & planner_data, const TargetObstacle & obstacle); - bool isStopRequired(const TargetObstacle & obstacle); void planCruise( const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 4c6bbc6845a2c..beee24c9106e4 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -167,6 +167,21 @@ class PlannerInterface return std::find(types.begin(), types.end(), label) != types.end(); } + // Note: If stop planning is not required, cruise planning will be done instead. + bool isStopRequired(const TargetObstacle & obstacle) + { + const bool is_cruise_obstacle = isCruiseObstacle(obstacle.classification.label); + const bool is_stop_obstacle = isStopObstacle(obstacle.classification.label); + + if (is_cruise_obstacle) { + return std::abs(obstacle.velocity) < obstacle_velocity_threshold_from_cruise_to_stop_; + } else if (is_stop_obstacle && !is_cruise_obstacle) { + return true; + } + + return false; + } + protected: // Parameters bool is_showing_debug_info_{false}; diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 171dfbc7061b8..8b26866997c30 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -475,12 +475,8 @@ double OptimizationBasedPlanner::getClosestStopDistance( for (const auto & obj : planner_data.target_obstacles) { const auto obj_base_time = obj.time_stamp; - // Step1. Ignore obstacles that are not vehicles - if ( - obj.classification.label != ObjectClassification::CAR && - obj.classification.label != ObjectClassification::TRUCK && - obj.classification.label != ObjectClassification::BUS && - obj.classification.label != ObjectClassification::MOTORCYCLE) { + // Step1. Ignore obstacles that are not required to stop + if (!isStopRequired(obj)) { continue; } @@ -508,20 +504,17 @@ double OptimizationBasedPlanner::getClosestStopDistance( // Calculate Safety Distance const auto & safe_distance_margin = longitudinal_info_.safe_distance_margin; - const double ego_vehicle_offset = vehicle_info_.wheel_base_m + vehicle_info_.front_overhang_m; + const auto & ego_vehicle_offset = vehicle_info_.max_longitudinal_offset_m; const double object_offset = obj_data.length / 2.0; const double safety_distance = ego_vehicle_offset + object_offset + safe_distance_margin; // If the object is on the current ego trajectory, // we assume the object travels along ego trajectory - const double obj_vel = std::abs(obj.velocity); - if (dist_to_collision_point && obj_vel < obstacle_velocity_threshold_from_cruise_to_stop_) { - const double current_stop_point = std::max(*dist_to_collision_point - safety_distance, 0.0); - closest_stop_dist = std::min(current_stop_point, closest_stop_dist); - } - - // Update Distance to the closest object on the ego trajectory if (dist_to_collision_point) { + const double stop_dist = std::max(*dist_to_collision_point - safety_distance, 0.0); + closest_stop_dist = std::min(stop_dist, closest_stop_dist); + + // Update Distance to the closest object on the ego trajectory const double current_obj_distance = std::max( *dist_to_collision_point - safety_distance + safe_distance_margin, -safety_distance); closest_obj_distance = std::min(closest_obj_distance, current_obj_distance); @@ -832,9 +825,13 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( double min_slow_down_point_length = std::numeric_limits::max(); boost::optional min_slow_down_idx = {}; for (size_t o_idx = 0; o_idx < planner_data.target_obstacles.size(); ++o_idx) { + const auto & obj = planner_data.target_obstacles.at(o_idx); const auto obj_base_time = planner_data.target_obstacles.at(o_idx).time_stamp; + // Only see cruise obstacles + if (!isCruiseObstacle(obj.classification.label)) { + continue; + } - const auto & obj = planner_data.target_obstacles.at(o_idx); // Step1 Get S boundary from the obstacle const auto obj_s_boundaries = getSBoundaries(planner_data.current_time, ego_traj_data, obj, obj_base_time, time_vec); diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 2a828e01af1e0..97072555397df 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -297,21 +297,6 @@ double PIDBasedPlanner::calcDistanceToObstacle( offset; } -// Note: If stop planning is not required, cruise planning will be done instead. -bool PIDBasedPlanner::isStopRequired(const TargetObstacle & obstacle) -{ - const bool is_cruise_obstacle = isCruiseObstacle(obstacle.classification.label); - const bool is_stop_obstacle = isStopObstacle(obstacle.classification.label); - - if (is_cruise_obstacle) { - return std::abs(obstacle.velocity) < obstacle_velocity_threshold_from_cruise_to_stop_; - } else if (is_stop_obstacle && !is_cruise_obstacle) { - return true; - } - - return false; -} - Trajectory PIDBasedPlanner::planStop( const ObstacleCruisePlannerData & planner_data, const boost::optional & stop_obstacle_info, DebugData & debug_data) From f90726ab2126ee3b6277c25cbf5d16f4dc366a42 Mon Sep 17 00:00:00 2001 From: yutaka Date: Mon, 13 Jun 2022 15:21:29 +0900 Subject: [PATCH 2/6] get current pose from predicted path --- .../optimization_based_planner.hpp | 3 +- .../include/obstacle_cruise_planner/utils.hpp | 17 ++++ planning/obstacle_cruise_planner/src/node.cpp | 15 +-- .../optimization_based_planner.cpp | 16 +--- .../obstacle_cruise_planner/src/utils.cpp | 92 ++++++++++++++++--- 5 files changed, 112 insertions(+), 31 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index d37b0e30f73e3..8e2bd3336a6d0 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -73,8 +73,7 @@ class OptimizationBasedPlanner : public PlannerInterface std::vector createTimeVector(); double getClosestStopDistance( - const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data, - const std::vector & resolutions); + const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data); std::tuple calcInitialMotion( const double current_vel, const Trajectory & input_traj, const size_t input_closest, diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index d1e6a0b86e715..b3a2b8f666602 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -18,6 +18,7 @@ #include #include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" #include "autoware_auto_perception_msgs/msg/predicted_path.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" @@ -26,6 +27,7 @@ #include #include +#include namespace obstacle_cruise_utils { @@ -41,9 +43,24 @@ boost::optional calcForwardPose( const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t nearest_idx, const double target_length); +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t); + +boost::optional lerpByTimeStamp( + const autoware_auto_perception_msgs::msg::PredictedPath & path, + const rclcpp::Duration & rel_time); + boost::optional getCurrentObjectPoseFromPredictedPath( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); + +boost::optional getCurrentObjectPoseFromPredictedPath( + const std::vector & predicted_paths, + const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); + +boost::optional getCurrentObjectPoseFromPredictedPath( + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, + const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); } // namespace obstacle_cruise_utils #endif // OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 0fe668a8f6e6f..ec34c923e51b4 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -488,6 +488,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( const PredictedObjects & predicted_objects, const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose, const double current_vel, DebugData & debug_data) { + const auto current_time = now(); const auto time_stamp = rclcpp::Time(predicted_objects.header.stamp); const size_t ego_idx = findExtendedNearestIndex( @@ -518,11 +519,13 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( continue; } - const auto & object_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + // const auto & object_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath( + predicted_object, time_stamp, current_time); const auto & object_velocity = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; - const bool is_front_obstacle = isFrontObstacle(traj, ego_idx, object_pose.position); + const bool is_front_obstacle = isFrontObstacle(traj, ego_idx, object_pose->position); if (!is_front_obstacle) { RCLCPP_INFO_EXPRESSION( get_logger(), is_showing_debug_info_, "Ignore obstacles since its not front obstacle."); @@ -531,7 +534,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( // rough detection area filtering without polygons const double dist_from_obstacle_to_traj = [&]() { - return tier4_autoware_utils::calcLateralOffset(decimated_traj.points, object_pose.position); + return tier4_autoware_utils::calcLateralOffset(decimated_traj.points, object_pose->position); }(); if (dist_from_obstacle_to_traj > obstacle_filtering_param_.rough_detection_area_expand_width) { RCLCPP_INFO_EXPRESSION( @@ -542,7 +545,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( // calculate collision points const auto obstacle_polygon = - polygon_utils::convertObstacleToPolygon(object_pose, predicted_object.shape); + polygon_utils::convertObstacleToPolygon(*object_pose, predicted_object.shape); std::vector collision_points; const auto first_within_idx = polygon_utils::getFirstCollisionIndex( decimated_traj_polygons, obstacle_polygon, collision_points); @@ -556,7 +559,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( debug_data.collision_points.push_back(nearest_collision_point); const bool is_angle_aligned = isAngleAlignedWithTrajectory( - decimated_traj, object_pose, + decimated_traj, *object_pose, obstacle_filtering_param_.crossing_obstacle_traj_angle_threshold); const double has_high_speed = std::abs(object_velocity) > obstacle_filtering_param_.crossing_obstacle_velocity_threshold; @@ -596,7 +599,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( obstacle_filtering_param_.max_prediction_time_for_collision_check); // TODO(murooka) think later - nearest_collision_point = object_pose.position; + nearest_collision_point = object_pose->position; if (!will_collide) { // Ignore condition 2 diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 8b26866997c30..66bbaf7dd1ede 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -212,7 +212,7 @@ Trajectory OptimizationBasedPlanner::generateTrajectory( } // Get Closest Stop Point for static obstacles - double closest_stop_dist = getClosestStopDistance(planner_data, base_traj_data, time_vec); + double closest_stop_dist = getClosestStopDistance(planner_data, base_traj_data); // Compute maximum velocity double v_max = 0.0; @@ -459,8 +459,7 @@ std::vector OptimizationBasedPlanner::createTimeVector() } double OptimizationBasedPlanner::getClosestStopDistance( - const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data, - const std::vector & resolutions) + const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data) { const auto & current_time = planner_data.current_time; double closest_stop_dist = ego_traj_data.s.back(); @@ -475,21 +474,14 @@ double OptimizationBasedPlanner::getClosestStopDistance( for (const auto & obj : planner_data.target_obstacles) { const auto obj_base_time = obj.time_stamp; - // Step1. Ignore obstacles that are not required to stop + // Ignore obstacles that are not required to stop if (!isStopRequired(obj)) { continue; } - // Get the predicted path, which has the most high confidence - const auto predicted_path = - resampledPredictedPath(obj, obj_base_time, current_time, resolutions, max_time_horizon_); - if (!predicted_path) { - continue; - } - // Get current pose from object's predicted path const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath( - *predicted_path, obj_base_time, current_time); + obj.predicted_paths, obj_base_time, current_time); if (!current_object_pose) { continue; } diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 41be60c4e8726..61479ea924e00 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -90,22 +90,92 @@ boost::optional calcForwardPose( return target_pose; } +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t) +{ + tf2::Transform tf_transform1, tf_transform2; + tf2::fromMsg(p1, tf_transform1); + tf2::fromMsg(p2, tf_transform2); + const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t); + const auto & tf_quaternion = + tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t); + + geometry_msgs::msg::Pose pose; + { + pose.position.x = tf_point.x(); + pose.position.y = tf_point.y(); + pose.position.z = tf_point.z(); + } + pose.orientation = tf2::toMsg(tf_quaternion); + return pose; +} + +boost::optional lerpByTimeStamp( + const autoware_auto_perception_msgs::msg::PredictedPath & path, const rclcpp::Duration & rel_time) +{ + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + if ( + path.path.empty() || rel_time < rclcpp::Duration::from_seconds(0.0) || + rel_time > rclcpp::Duration(path.time_step) * (static_cast(path.path.size()) - 1)) { + return boost::none; + } + + for (size_t i = 1; i < path.path.size(); ++i) { + const auto & pt = path.path.at(i); + const auto & prev_pt = path.path.at(i - 1); + if (rel_time <= rclcpp::Duration(path.time_step) * static_cast(i)) { + const auto offset = rel_time - rclcpp::Duration(path.time_step) * static_cast(i - 1); + const auto ratio = offset.seconds() / rclcpp::Duration(path.time_step).seconds(); + return lerpByPose(prev_pt, pt, ratio); + } + } + + return boost::none; +} + boost::optional getCurrentObjectPoseFromPredictedPath( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) { - for (size_t i = 0; i < predicted_path.path.size(); ++i) { - const auto & obj_p = predicted_path.path.at(i); - - const double object_time = - (obj_base_time + rclcpp::Duration(predicted_path.time_step) * static_cast(i) - - current_time) - .seconds(); - if (object_time >= 0) { - return obj_p; - } + const auto rel_time = current_time - obj_base_time; + if (rel_time.seconds() < 0.0) { + return boost::none; } - return boost::none; + return lerpByTimeStamp(predicted_path, rel_time); +} + +boost::optional getCurrentObjectPoseFromPredictedPath( + const std::vector & predicted_paths, + const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) +{ + if (predicted_paths.empty()) { + return boost::none; + } + // Get the most reliable path + const auto predicted_path = std::max_element( + predicted_paths.begin(), predicted_paths.end(), + []( + const autoware_auto_perception_msgs::msg::PredictedPath & a, + const autoware_auto_perception_msgs::msg::PredictedPath & b) { + return a.confidence < b.confidence; + }); + + return getCurrentObjectPoseFromPredictedPath(*predicted_path, obj_base_time, current_time); +} + +boost::optional getCurrentObjectPoseFromPredictedPath( + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, + const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) +{ + std::vector predicted_paths; + for (const auto & path : predicted_object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + const auto interpolated_pose = + getCurrentObjectPoseFromPredictedPath(predicted_paths, obj_base_time, current_time); + + return interpolated_pose ? interpolated_pose + : predicted_object.kinematics.initial_pose_with_covariance.pose; } } // namespace obstacle_cruise_utils From 8510dc8f31c66a6aaaa016235389183fc161099b Mon Sep 17 00:00:00 2001 From: yutaka Date: Mon, 13 Jun 2022 15:33:42 +0900 Subject: [PATCH 3/6] remove unnecessary code --- .../optimization_based_planner.hpp | 2 +- .../optimization_based_planner/resample.hpp | 7 -- planning/obstacle_cruise_planner/src/node.cpp | 1 - .../optimization_based_planner.cpp | 14 +-- .../optimization_based_planner/resample.cpp | 93 +------------------ 5 files changed, 7 insertions(+), 110 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index 8e2bd3336a6d0..1f8834a0e6a4e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -115,7 +115,7 @@ class OptimizationBasedPlanner : public PlannerInterface const TargetObstacle & object, const rclcpp::Time & obj_base_time, const PredictedPath & predicted_path); - boost::optional resampledPredictedPath( + boost::optional resamplePredictedPath( const TargetObstacle & object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time, const std::vector & resolutions, const double horizon); diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/resample.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/resample.hpp index dffe7f181fec5..2c5b3d3fe5920 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/resample.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/resample.hpp @@ -34,13 +34,6 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( const autoware_auto_perception_msgs::msg::PredictedPath & input_path, const std::vector & rel_time_vec); -geometry_msgs::msg::Pose lerpByPose( - const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t); - -boost::optional lerpByTimeStamp( - const autoware_auto_perception_msgs::msg::PredictedPath & path, - const rclcpp::Duration & rel_time); - autoware_auto_planning_msgs::msg::Trajectory applyLinearInterpolation( const std::vector & base_index, const autoware_auto_planning_msgs::msg::Trajectory & base_trajectory, diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index ec34c923e51b4..72a50234754de 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -519,7 +519,6 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( continue; } - // const auto & object_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; const auto object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath( predicted_object, time_stamp, current_time); const auto & object_velocity = diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 66bbaf7dd1ede..854f057078bf9 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -841,11 +841,8 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( // Step3 search nearest obstacle to follow for rviz marker const double object_offset = obj.shape.dimensions.x / 2.0; - const auto predicted_path = - resampledPredictedPath(obj, obj_base_time, current_time, time_vec, max_time_horizon_); - const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath( - *predicted_path, obj_base_time, current_time); + obj.predicted_paths, obj_base_time, current_time); const double obj_vel = std::abs(obj.velocity); const double rss_dist = calcRSSDistance(planner_data.current_vel, obj_vel); @@ -867,11 +864,8 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( if (min_slow_down_idx) { const auto & obj = planner_data.target_obstacles.at(min_slow_down_idx.get()); - const auto predicted_path = - resampledPredictedPath(obj, obj.time_stamp, current_time, time_vec, max_time_horizon_); - const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath( - *predicted_path, obj.time_stamp, current_time); + obj.predicted_paths, obj.time_stamp, current_time); const auto marker_pose = calcForwardPose( ego_traj_data.traj, planner_data.current_pose.position, min_slow_down_point_length); @@ -906,7 +900,7 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( // Get the predicted path, which has the most high confidence const double max_horizon = time_vec.back(); const auto predicted_path = - resampledPredictedPath(object, obj_base_time, current_time, time_vec, max_horizon); + resamplePredictedPath(object, obj_base_time, current_time, time_vec, max_horizon); if (!predicted_path) { RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), @@ -1105,7 +1099,7 @@ boost::optional OptimizationBasedPlanner::getCollisionIdx( return boost::none; } -boost::optional OptimizationBasedPlanner::resampledPredictedPath( +boost::optional OptimizationBasedPlanner::resamplePredictedPath( const TargetObstacle & object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time, const std::vector & resolutions, const double horizon) { diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/resample.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/resample.cpp index a629abca7f0dd..a71fbf4231b12 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/resample.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/resample.cpp @@ -16,37 +16,10 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" +#include "obstacle_cruise_planner/utils.hpp" #include -namespace -{ -[[maybe_unused]] rclcpp::Duration safeSubtraction(const rclcpp::Time & t1, const rclcpp::Time & t2) -{ - rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.0); - try { - duration = t1 - t2; - } catch (std::runtime_error & err) { - if (t1 > t2) { - duration = rclcpp::Duration::max() * -1.0; - } else { - duration = rclcpp::Duration::max(); - } - } - return duration; -} - -// tf2::toMsg does not have this type of function -geometry_msgs::msg::Point toMsg(tf2::Vector3 vec) -{ - geometry_msgs::msg::Point point; - point.x = vec.x(); - point.y = vec.y(); - point.z = vec.z(); - return point; -} -} // namespace - namespace resampling { std::vector resampledValidRelativeTimeVector( @@ -86,7 +59,7 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( resampled_path.time_step = input_path.time_step; for (const auto & rel_time : rel_time_vec) { - const auto opt_pose = lerpByTimeStamp(input_path, rel_time); + const auto opt_pose = obstacle_cruise_utils::lerpByTimeStamp(input_path, rel_time); if (!opt_pose) { continue; } @@ -97,68 +70,6 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( return resampled_path; } -geometry_msgs::msg::Pose lerpByPose( - const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t) -{ - tf2::Transform tf_transform1, tf_transform2; - tf2::fromMsg(p1, tf_transform1); - tf2::fromMsg(p2, tf_transform2); - const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t); - const auto & tf_quaternion = - tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t); - - geometry_msgs::msg::Pose pose; - pose.position = ::toMsg(tf_point); - pose.orientation = tf2::toMsg(tf_quaternion); - return pose; -} - -boost::optional lerpByTimeStamp( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const rclcpp::Duration & rel_time) -{ - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; - if (path.path.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - rclcpp::get_logger("DynamicAvoidance.resample"), clock, 1000, - "Empty path. Failed to interpolate path by time!"); - return {}; - } - if (rel_time < rclcpp::Duration::from_seconds(0.0)) { - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("DynamicAvoidance.resample"), "failed to interpolate path by time!" - << std::endl - << "query time: " << rel_time.seconds()); - - return {}; - } - - if (rel_time > rclcpp::Duration(path.time_step) * (static_cast(path.path.size()) - 1)) { - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("DynamicAvoidance.resample"), - "failed to interpolate path by time!" - << std::endl - << "path max duration: " << path.path.size() * rclcpp::Duration(path.time_step).seconds() - << std::endl - << "query time : " << rel_time.seconds()); - - return {}; - } - - for (size_t i = 1; i < path.path.size(); ++i) { - const auto & pt = path.path.at(i); - const auto & prev_pt = path.path.at(i - 1); - if (rel_time <= rclcpp::Duration(path.time_step) * static_cast(i)) { - const auto offset = rel_time - rclcpp::Duration(path.time_step) * static_cast(i - 1); - const auto ratio = offset.seconds() / rclcpp::Duration(path.time_step).seconds(); - return lerpByPose(prev_pt, pt, ratio); - } - } - - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("DynamicAvoidance.resample"), "Something failed in function: " << __func__); - return {}; -} - inline void convertEulerAngleToMonotonic(std::vector & a) { for (unsigned int i = 1; i < a.size(); ++i) { From 79fefe0e8a48a7994a27079441e831c0dd60bbcf Mon Sep 17 00:00:00 2001 From: yutaka Date: Mon, 13 Jun 2022 17:42:48 +0900 Subject: [PATCH 4/6] modify return format --- .../include/obstacle_cruise_planner/utils.hpp | 2 +- planning/obstacle_cruise_planner/src/node.cpp | 10 +++++----- planning/obstacle_cruise_planner/src/utils.cpp | 4 ++-- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index b3a2b8f666602..23601aa35b9c6 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -58,7 +58,7 @@ boost::optional getCurrentObjectPoseFromPredictedPath( const std::vector & predicted_paths, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); -boost::optional getCurrentObjectPoseFromPredictedPath( +geometry_msgs::msg::Pose getCurrentObjectPoseFromPredictedPath( const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); } // namespace obstacle_cruise_utils diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 72a50234754de..1f2b5b95f21ea 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -524,7 +524,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( const auto & object_velocity = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; - const bool is_front_obstacle = isFrontObstacle(traj, ego_idx, object_pose->position); + const bool is_front_obstacle = isFrontObstacle(traj, ego_idx, object_pose.position); if (!is_front_obstacle) { RCLCPP_INFO_EXPRESSION( get_logger(), is_showing_debug_info_, "Ignore obstacles since its not front obstacle."); @@ -533,7 +533,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( // rough detection area filtering without polygons const double dist_from_obstacle_to_traj = [&]() { - return tier4_autoware_utils::calcLateralOffset(decimated_traj.points, object_pose->position); + return tier4_autoware_utils::calcLateralOffset(decimated_traj.points, object_pose.position); }(); if (dist_from_obstacle_to_traj > obstacle_filtering_param_.rough_detection_area_expand_width) { RCLCPP_INFO_EXPRESSION( @@ -544,7 +544,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( // calculate collision points const auto obstacle_polygon = - polygon_utils::convertObstacleToPolygon(*object_pose, predicted_object.shape); + polygon_utils::convertObstacleToPolygon(object_pose, predicted_object.shape); std::vector collision_points; const auto first_within_idx = polygon_utils::getFirstCollisionIndex( decimated_traj_polygons, obstacle_polygon, collision_points); @@ -558,7 +558,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( debug_data.collision_points.push_back(nearest_collision_point); const bool is_angle_aligned = isAngleAlignedWithTrajectory( - decimated_traj, *object_pose, + decimated_traj, object_pose, obstacle_filtering_param_.crossing_obstacle_traj_angle_threshold); const double has_high_speed = std::abs(object_velocity) > obstacle_filtering_param_.crossing_obstacle_velocity_threshold; @@ -598,7 +598,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( obstacle_filtering_param_.max_prediction_time_for_collision_check); // TODO(murooka) think later - nearest_collision_point = object_pose->position; + nearest_collision_point = object_pose.position; if (!will_collide) { // Ignore condition 2 diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 61479ea924e00..502b92fd5fd5b 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -164,7 +164,7 @@ boost::optional getCurrentObjectPoseFromPredictedPath( return getCurrentObjectPoseFromPredictedPath(*predicted_path, obj_base_time, current_time); } -boost::optional getCurrentObjectPoseFromPredictedPath( +geometry_msgs::msg::Pose getCurrentObjectPoseFromPredictedPath( const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) { @@ -175,7 +175,7 @@ boost::optional getCurrentObjectPoseFromPredictedPath( const auto interpolated_pose = getCurrentObjectPoseFromPredictedPath(predicted_paths, obj_base_time, current_time); - return interpolated_pose ? interpolated_pose + return interpolated_pose ? interpolated_pose.value() : predicted_object.kinematics.initial_pose_with_covariance.pose; } } // namespace obstacle_cruise_utils From 686c1cd9509ad26e2b75b1a7839d14df6b10b58b Mon Sep 17 00:00:00 2001 From: yutaka Date: Mon, 13 Jun 2022 17:51:13 +0900 Subject: [PATCH 5/6] add logger --- planning/obstacle_cruise_planner/src/utils.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 502b92fd5fd5b..6a354c631e88c 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -175,7 +175,12 @@ geometry_msgs::msg::Pose getCurrentObjectPoseFromPredictedPath( const auto interpolated_pose = getCurrentObjectPoseFromPredictedPath(predicted_paths, obj_base_time, current_time); - return interpolated_pose ? interpolated_pose.value() - : predicted_object.kinematics.initial_pose_with_covariance.pose; + if (!interpolated_pose) { + RCLCPP_WARN( + rclcpp::get_logger("ObstacleCruisePlanner"), "Failed to find the interpolated obstacle pose"); + return predicted_object.kinematics.initial_pose_with_covariance.pose; + } + + return interpolated_pose.value(); } } // namespace obstacle_cruise_utils From 228057898b1611970a1ce05d4c00b67ad2aca479 Mon Sep 17 00:00:00 2001 From: yutaka Date: Wed, 15 Jun 2022 16:36:28 +0900 Subject: [PATCH 6/6] change get value Signed-off-by: yutaka --- planning/obstacle_cruise_planner/src/utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 6a354c631e88c..5198d02b9074e 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -181,6 +181,6 @@ geometry_msgs::msg::Pose getCurrentObjectPoseFromPredictedPath( return predicted_object.kinematics.initial_pose_with_covariance.pose; } - return interpolated_pose.value(); + return interpolated_pose.get(); } } // namespace obstacle_cruise_utils