From 7b685e168d8883fc9c6f8cd2fa1f3b9efc40bb26 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 19 Sep 2023 18:11:16 +0900 Subject: [PATCH 1/2] fix(obstacle_cruise_planner): fix backward virtual wall Signed-off-by: kosuke55 --- .../motion_utils/marker/marker_helper.hpp | 9 +++++--- .../marker/virtual_wall_marker_creator.hpp | 3 ++- .../motion_utils/src/marker/marker_helper.cpp | 21 +++++++++++-------- .../marker/virtual_wall_marker_creator.cpp | 2 +- .../src/planner_interface.cpp | 8 +++---- 5 files changed, 25 insertions(+), 18 deletions(-) diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index 6433f1b9ae2f9..99f7dd4333bae 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp @@ -28,15 +28,18 @@ using geometry_msgs::msg::Pose; visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); diff --git a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp index 331b3dd5275fd..3f92f41b8738d 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -38,6 +38,7 @@ struct VirtualWall std::string ns{}; VirtualWallType style = stop; double longitudinal_offset{}; + bool is_driving_forward{true}; }; typedef std::vector VirtualWalls; @@ -54,7 +55,7 @@ class VirtualWallMarkerCreator using create_wall_function = std::function; + const std::string & ns_prefix, const bool is_driving_forward)>; VirtualWalls virtual_walls; std::unordered_map marker_count_per_namespace; diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 25d8616949b02..7998dbeabe7f9 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -86,10 +86,11 @@ namespace motion_utils { visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "stop_", now, id, createMarkerColor(1.0, 0.0, 0.0, 0.5)); @@ -97,10 +98,11 @@ visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, createMarkerColor(1.0, 1.0, 0.0, 0.5)); @@ -108,10 +110,11 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5)); diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp index 63c456ab24fb0..87736f8a2982b 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -64,7 +64,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( } auto markers = create_fn( virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, - virtual_wall.ns); + virtual_wall.ns, virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { marker.id = marker_count_per_namespace[marker.ns].current++; marker_array.markers.push_back(marker); diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 1aaa897986bcd..8bb897dc8ae89 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -323,7 +323,7 @@ std::vector PlannerInterface::generateStopTrajectory( // virtual wall marker for stop obstacle const auto markers = motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, - abs_ego_offset); + abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle); @@ -479,7 +479,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", - planner_data.current_time, i, abs_ego_offset); + planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); } @@ -487,14 +487,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( if (slow_down_start_idx) { const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", - planner_data.current_time, i * 2, abs_ego_offset); + planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", - planner_data.current_time, i * 2 + 1, abs_ego_offset); + planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } From cca56a37f2c2730860d28f895ffb181a7afed81a Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 19 Sep 2023 18:18:59 +0900 Subject: [PATCH 2/2] fix(obstacle_cruise): fix stop margin after backward terminal Signed-off-by: kosuke55 --- planning/obstacle_cruise_planner/src/node.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index ab2092a3120ca..f6fe3896088cf 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -129,11 +129,11 @@ double calcObstacleMaxLength(const Shape & shape) } TrajectoryPoint getExtendTrajectoryPoint( - const double extend_distance, const TrajectoryPoint & goal_point) + const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) { TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = - tier4_autoware_utils::calcOffsetPose(goal_point.pose, extend_distance, 0.0, 0.0); + extend_trajectory_point.pose = tier4_autoware_utils::calcOffsetPose( + goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; @@ -145,6 +145,8 @@ std::vector extendTrajectoryPoints( const double step_length) { auto output_points = input_points; + const auto is_driving_forward_opt = motion_utils::isDrivingForwardWithTwist(input_points); + const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (extend_distance < std::numeric_limits::epsilon()) { return output_points; @@ -154,11 +156,13 @@ std::vector extendTrajectoryPoints( double extend_sum = 0.0; while (extend_sum <= (extend_distance - step_length)) { - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_sum, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); extend_sum += step_length; } - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_distance, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); return output_points;