diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index f29a955ae38e1..da7f284085ef6 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -167,6 +167,8 @@ struct DebugData std::vector mpt_traj; std::vector extended_fixed_traj; std::vector extended_non_fixed_traj; + + SequentialBoundsCandidates sequential_bounds_candidates; }; struct Trajectories diff --git a/planning/obstacle_avoidance_planner/src/debug_visualization.cpp b/planning/obstacle_avoidance_planner/src/debug_visualization.cpp index 678ba112cbd27..1ddcb9978b8d8 100644 --- a/planning/obstacle_avoidance_planner/src/debug_visualization.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_visualization.cpp @@ -436,51 +436,49 @@ visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray( return msg; } -/* visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray( const std::vector & ref_points, - const SequentialBoundsCandidates & sequential_bounds_candidates, const double r, const double g, - const double b, const double vehicle_circle_radius, const size_t sampling_num) + std::vector> & bounds_candidates, const double r, const double g, + const double b, [[maybe_unused]] const double vehicle_width, const size_t sampling_num) { const auto current_time = rclcpp::Clock().now(); visualization_msgs::msg::MarkerArray msg; + const std::string ns = "bounds_candidates"; - int unique_id = 0; + if (ref_points.empty()) return msg; auto marker = createDefaultMarker( - "map", rclcpp::Clock().now(), "bounds_candidates", 0, - visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.05, 0, 0), - createMarkerColor(r + 0.5, g, b, 0.3)); - marker.lifetime = rclcpp::Duration::from_seconds(1.0); + "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r + 0.5, g, b, 0.3)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); - for (size_t p_idx = 0; p_idx < sequential_bounds_candidates.size(); ++p_idx) { - if (p_idx % sampling_num != 0) { + for (size_t i = 0; i < ref_points.size(); i++) { + if (i % sampling_num != 0) { continue; } - const auto & bounds_candidates = sequential_bounds_candidates.at(p_idx); - for (size_t b_idx = 0; b_idx < bounds_candidates.size(); ++b_idx) { - const auto & bounds_candidate = bounds_candidates.at(b_idx); - - marker.id = unique_id++; + const auto & bound_candidates = bounds_candidates.at(i); + for (size_t j = 0; j < bound_candidates.size(); ++j) { geometry_msgs::msg::Pose pose; - pose.position = ref_points.at(p_idx).p; - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(p_idx).yaw); + pose.position = ref_points.at(i).p; + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(i).yaw); - const double lb_y = bounds_candidate.lower_bound - vehicle_circle_radius; + // lower bound + const double lb_y = bound_candidates.at(j).lower_bound; const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; marker.points.push_back(lb); - const double ub_y = bounds_candidate.upper_bound + vehicle_circle_radius; + // upper bound + const double ub_y = bound_candidates.at(j).upper_bound; const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; marker.points.push_back(ub); + + msg.markers.push_back(marker); } } - msg.markers.push_back(marker); return msg; } -*/ visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray( const std::vector & ref_points, const double r, const double g, const double b, @@ -762,14 +760,12 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( debug_data_ptr->mpt_visualize_sampling_num), &vis_marker_array); - /* // bounds candidates appendMarkerArray( getBoundsCandidatesLineMarkerArray( debug_data_ptr->ref_points, debug_data_ptr->sequential_bounds_candidates, 0.2, 0.99, 0.99, - debug_data_ptr->vehicle_circle_radius, debug_data_ptr->mpt_visualize_sampling_num), + vehicle_param.width, debug_data_ptr->mpt_visualize_sampling_num), &vis_marker_array); - */ // vehicle circle line appendMarkerArray( diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index bf3c02949ff9b..8d68e5f19f7b6 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -1372,6 +1372,7 @@ void MPTOptimizer::calcBounds( enable_avoidance, convertRefPointsToPose(ref_point), maps, debug_data_ptr); sequential_bounds_candidates.push_back(bounds_candidates); } + debug_data_ptr->sequential_bounds_candidates = sequential_bounds_candidates; // search continuous and widest bounds only for front point for (size_t i = 0; i < sequential_bounds_candidates.size(); ++i) { @@ -1408,6 +1409,11 @@ void MPTOptimizer::calcBounds( RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "non-overlapped length bounds is ignored."); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, + "In detail, prev: lower=%f, upper=%f, candidate: lower=%f, upper=%f", + prev_continuous_bounds.lower_bound, prev_continuous_bounds.upper_bound, + bounds_candidate.lower_bound, bounds_candidate.upper_bound); continue; } @@ -1429,8 +1435,8 @@ void MPTOptimizer::calcBounds( const auto nearest_bounds = std::min_element( filtered_bounds_candidates.begin(), filtered_bounds_candidates.end(), [](const auto & a, const auto & b) { - return std::abs(a.lower_bound + a.upper_bound) < - std::abs(b.lower_bound + b.upper_bound); + return std::min(std::abs(a.lower_bound), std::abs(a.upper_bound)) < + std::min(std::abs(b.lower_bound), std::abs(b.upper_bound)); }); if ( filtered_bounds_candidates.begin() <= nearest_bounds && @@ -1442,7 +1448,7 @@ void MPTOptimizer::calcBounds( // invalid bounds RCLCPP_WARN_EXPRESSION( - rclcpp::get_logger("getBounds: front"), is_showing_debug_info_, "invalid bounds"); + rclcpp::get_logger("getBounds: not front"), is_showing_debug_info_, "invalid bounds"); const auto invalid_bounds = Bounds{-5.0, 5.0, CollisionType::OUT_OF_ROAD, CollisionType::OUT_OF_ROAD}; ref_points.at(i).bounds = invalid_bounds;