From bd58f21a1396cdc61cc983fed0d90ca832bbfd5b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 17 Aug 2023 02:38:28 +0900 Subject: [PATCH 1/3] feat(obstacle_cruise_planner): consider object velocity direction Signed-off-by: Takayuki Murooka --- planning/obstacle_cruise_planner/src/node.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index ab2092a3120ca..d0bb2d8461cda 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -100,12 +100,12 @@ std::pair projectObstacleVelocityToTrajectory( { const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); - const double object_yaw = tf2::getYaw(obstacle.pose.orientation); + const double object_vel = std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); + const double object_yaw = std::atan2(obstacle.twist.linear.y, obstacle.twist.linear.x); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); return std::make_pair( - obstacle.twist.linear.x * std::cos(object_yaw - traj_yaw), - obstacle.twist.linear.x * std::sin(object_yaw - traj_yaw)); + object_vel * std::cos(object_yaw - traj_yaw), object_vel * std::sin(object_yaw - traj_yaw)); } double calcObstacleMaxLength(const Shape & shape) @@ -818,7 +818,9 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( return std::nullopt; } - if (std::abs(obstacle.twist.linear.x) < p.outside_obstacle_min_velocity_threshold) { + if ( + std::abs(std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y)) < + p.outside_obstacle_min_velocity_threshold) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, "[Cruise] Ignore outside obstacle (%s) since the obstacle velocity is low.", @@ -920,7 +922,8 @@ ObstacleCruisePlannerNode::createCollisionPointForStopObstacle( // and the collision between ego and obstacles are within the margin threshold. const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); const double has_high_speed = - p.crossing_obstacle_velocity_threshold < std::abs(obstacle.twist.linear.x); + p.crossing_obstacle_velocity_threshold < + std::abs(std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y)); if (is_obstacle_crossing && has_high_speed) { // Get highest confidence predicted path const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( @@ -1141,8 +1144,10 @@ void ObstacleCruisePlannerNode::checkConsistency( } else { // prev obstacle is not in the target obstacles, but in the perception list const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds(); + const auto & object_twist = + predicted_object_itr->kinematics.initial_twist_with_covariance.twist; if ( - predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < + std::hypot(object_twist.linear.x, object_twist.linear.y) < behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise && elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) { stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_); From 14d24c80dabcc5cfd9f3c3762af72bd3e5331415 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 17 Aug 2023 03:10:38 +0900 Subject: [PATCH 2/3] update Signed-off-by: Takayuki Murooka --- planning/obstacle_cruise_planner/src/node.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index d0bb2d8461cda..e59904eab5766 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -100,12 +100,13 @@ std::pair projectObstacleVelocityToTrajectory( { const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); - const double object_vel = std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); - const double object_yaw = std::atan2(obstacle.twist.linear.y, obstacle.twist.linear.x); + const double object_vel_norm = std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); + const double object_vel_yaw = std::atan2(obstacle.twist.linear.y, obstacle.twist.linear.x); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); return std::make_pair( - object_vel * std::cos(object_yaw - traj_yaw), object_vel * std::sin(object_yaw - traj_yaw)); + object_vel_norm * std::cos(object_vel_yaw - traj_yaw), + object_vel_norm * std::sin(object_vel_yaw - traj_yaw)); } double calcObstacleMaxLength(const Shape & shape) @@ -819,7 +820,7 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( } if ( - std::abs(std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y)) < + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) < p.outside_obstacle_min_velocity_threshold) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -921,9 +922,8 @@ ObstacleCruisePlannerNode::createCollisionPointForStopObstacle( // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, // and the collision between ego and obstacles are within the margin threshold. const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); - const double has_high_speed = - p.crossing_obstacle_velocity_threshold < - std::abs(std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y)); + const double has_high_speed = p.crossing_obstacle_velocity_threshold < + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); if (is_obstacle_crossing && has_high_speed) { // Get highest confidence predicted path const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( @@ -1144,10 +1144,8 @@ void ObstacleCruisePlannerNode::checkConsistency( } else { // prev obstacle is not in the target obstacles, but in the perception list const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds(); - const auto & object_twist = - predicted_object_itr->kinematics.initial_twist_with_covariance.twist; if ( - std::hypot(object_twist.linear.x, object_twist.linear.y) < + predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise && elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) { stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_); From 327c487e1bc9cc43c4ff51df33bdd3be57a1986e Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Thu, 17 Aug 2023 12:05:13 +0900 Subject: [PATCH 3/3] chore: spell fix --- planning/obstacle_cruise_planner/src/node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index e59904eab5766..70c0c687b2514 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -998,7 +998,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } return true; } - // check if entrying slow down + // check if entering slow down if (is_lat_dist_low) { const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid); if (p.successive_num_to_entry_slow_down_condition <= count) { @@ -1019,7 +1019,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl const auto obstacle_poly = obstacle.toPolygon(); // calculate collision points with trajectory with lateral stop margin - // NOTE: For additional margin, hysteresis is not devided by two. + // NOTE: For additional margin, hysteresis is not divided by two. const auto traj_polys_with_lat_margin = polygon_utils::createOneStepPolygons( traj_points, vehicle_info_, p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down);