diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml index 78abda6861593..64f6affbb9b45 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml @@ -77,8 +77,8 @@ # replanning & trimming trajectory param outside algorithm replan: max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m] - max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m] - max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second] + max_ego_moving_dist: 3.0 # threshold of ego's moving distance for replan [m] + max_delta_time_sec: 1.0 # threshold of delta time for replan [second] # advanced parameters to improve performance as much as possible advanced: diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt index a6dfc110cd157..dd5b84bfcdfae 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/obstacle_avoidance_planner/CMakeLists.txt @@ -10,12 +10,13 @@ find_package(OpenCV REQUIRED) ament_auto_add_library(obstacle_avoidance_planner SHARED src/vehicle_model/vehicle_model_interface.cpp src/vehicle_model/vehicle_model_bicycle_kinematics.cpp - src/utils.cpp src/costmap_generator.cpp - src/debug_visualization.cpp src/eb_path_optimizer.cpp src/mpt_optimizer.cpp - src/cv_utils.cpp + src/replan_checker.cpp + src/utils/utils.cpp + src/utils/debug_utils.cpp + src/utils/cv_utils.cpp src/node.cpp ) diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index 78abda6861593..64f6affbb9b45 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -77,8 +77,8 @@ # replanning & trimming trajectory param outside algorithm replan: max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m] - max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m] - max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second] + max_ego_moving_dist: 3.0 # threshold of ego's moving distance for replan [m] + max_delta_time_sec: 1.0 # threshold of delta time for replan [second] # advanced parameters to improve performance as much as possible advanced: 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 667c0916e2aba..097891f4b30d6 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 @@ -20,6 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "nav_msgs/msg/map_meta_data.hpp" @@ -38,6 +39,11 @@ using SequentialBounds = std::vector; using BoundsCandidates = std::vector; using SequentialBoundsCandidates = std::vector; +// message type +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + struct CVMaps { cv::Mat drivable_area; @@ -148,8 +154,8 @@ struct DebugData std::vector fixed_points; std::vector non_fixed_points; std::vector constrain_rectangles; - std::vector avoiding_traj_points; - std::vector avoiding_objects; + std::vector avoiding_traj_points; + std::vector avoiding_objects; cv::Mat clearance_map; cv::Mat only_object_clearance_map; @@ -161,21 +167,21 @@ struct DebugData std::vector mpt_ref_poses; std::vector lateral_errors; - std::vector eb_traj; - std::vector mpt_fixed_traj; - std::vector mpt_ref_traj; - std::vector mpt_traj; - std::vector extended_fixed_traj; - std::vector extended_non_fixed_traj; + std::vector eb_traj; + std::vector mpt_fixed_traj; + std::vector mpt_ref_traj; + std::vector mpt_traj; + std::vector extended_fixed_traj; + std::vector extended_non_fixed_traj; SequentialBoundsCandidates sequential_bounds_candidates; }; struct Trajectories { - std::vector smoothed_trajectory; + std::vector smoothed_trajectory; std::vector mpt_ref_points; - std::vector model_predictive_trajectory; + std::vector model_predictive_trajectory; }; struct TrajectoryParam @@ -260,4 +266,12 @@ struct MPTParam double max_plan_from_ego_length; }; +struct PlannerData +{ + Path path; + geometry_msgs::msg::Pose ego_pose; + double ego_vel; + std::vector objects; +}; + #endif // OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp index 8165b523ca0d2..cdff21bd71132 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp @@ -36,7 +36,7 @@ class CostmapGenerator CVMaps getMaps( const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, const std::vector & objects, - const TrajectoryParam & traj_param, std::shared_ptr debug_data_ptr); + const TrajectoryParam & traj_param, DebugData & debug_data); private: mutable tier4_autoware_utils::StopWatch< @@ -44,11 +44,9 @@ class CostmapGenerator stop_watch_; cv::Mat getAreaWithObjects( - const cv::Mat & drivable_area, const cv::Mat & objects_image, - std::shared_ptr debug_data_ptr) const; + const cv::Mat & drivable_area, const cv::Mat & objects_image, DebugData & debug_data) const; - cv::Mat getClearanceMap( - const cv::Mat & drivable_area, std::shared_ptr debug_data_ptr) const; + cv::Mat getClearanceMap(const cv::Mat & drivable_area, DebugData & debug_data) const; cv::Mat drawObstaclesOnImage( const bool enable_avoidance, @@ -57,10 +55,9 @@ class CostmapGenerator const nav_msgs::msg::MapMetaData & map_info, [[maybe_unused]] const cv::Mat & drivable_area, const cv::Mat & clearance_map, const TrajectoryParam & traj_param, std::vector * debug_avoiding_objects, - std::shared_ptr debug_data_ptr) const; + DebugData & debug_data) const; cv::Mat getDrivableAreaInCV( - const nav_msgs::msg::OccupancyGrid & occupancy_grid, - std::shared_ptr debug_data_ptr) const; + const nav_msgs::msg::OccupancyGrid & occupancy_grid, DebugData & debug_data) const; }; #endif // OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp index 41f296eb053df..ea6dc2a5127d0 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp @@ -33,6 +33,11 @@ #include #include +using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + class EBPathOptimizer { private: @@ -142,7 +147,7 @@ class EBPathOptimizer boost::optional> getOptimizedTrajectory( const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points, - std::shared_ptr debug_data_ptr); + DebugData & debug_data); void updateConstrain( const std::vector & interpolated_points, @@ -155,12 +160,12 @@ class EBPathOptimizer std::vector getFixedPoints( const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, - const std::unique_ptr & prev_optimized_points); + const std::shared_ptr> prev_traj); CandidatePoints getCandidatePoints( const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, - const std::unique_ptr & prev_trajs, std::shared_ptr debug_data_ptr); + const std::shared_ptr> prev_traj, DebugData & debug_data); CandidatePoints getDefaultCandidatePoints( const std::vector & path_points); @@ -171,7 +176,7 @@ class EBPathOptimizer calculateTrajectory( const std::vector & padded_interpolated_points, const std::vector & constrain_rectangles, const int farthest_idx, - std::shared_ptr debug_data_ptr); + DebugData & debug_data); public: EBPathOptimizer( @@ -180,8 +185,8 @@ class EBPathOptimizer boost::optional> getEBTrajectory( const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path, - const std::unique_ptr & prev_trajs, const double current_ego_vel, - std::shared_ptr debug_data_ptr); + const std::shared_ptr> prev_traj, const double current_ego_vel, + DebugData & debug_data); }; #endif // OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/marker_helper.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/marker_helper.hpp deleted file mode 100644 index d52d25499dbf1..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/marker_helper.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__MARKER_HELPER_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__MARKER_HELPER_HPP_ - -#include - -inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z) -{ - geometry_msgs::msg::Vector3 scale; - - scale.x = x; - scale.y = y; - scale.z = z; - - return scale; -} - -inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a) -{ - std_msgs::msg::ColorRGBA color; - - color.r = r; - color.g = g; - color.b = b; - color.a = a; - - return color; -} - -inline void appendMarkerArray( - const visualization_msgs::msg::MarkerArray & additional_marker_array, - visualization_msgs::msg::MarkerArray * marker_array) -{ - for (const auto & marker : additional_marker_array.markers) { - marker_array->markers.push_back(marker); - } -} -#endif // OBSTACLE_AVOIDANCE_PLANNER__MARKER_HELPER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index 1c12ece517e89..d6f2ebb5065bf 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -58,6 +58,11 @@ #include #include +using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + enum class CollisionType { NO_COLLISION = 0, OUT_OF_SIGHT = 1, OUT_OF_ROAD = 2, OBJECT = 3 }; struct Bounds @@ -139,6 +144,12 @@ struct ReferencePoint std::vector vehicle_bounds_poses; // for debug visualization }; +struct MPTTrajs +{ + std::vector ref_points; + std::vector mpt; +}; + class MPTOptimizer { private: @@ -174,12 +185,6 @@ class MPTOptimizer Eigen::VectorXd upper_bound; }; - struct MPTTrajs - { - std::vector mpt; - std::vector ref_points; - }; - const double osqp_epsilon_ = 1.0e-3; bool is_showing_debug_info_; @@ -201,8 +206,8 @@ class MPTOptimizer std::vector getReferencePoints( const std::vector & points, - const std::unique_ptr & prev_mpt_points, const bool enable_avoidance, - const CVMaps & maps, std::shared_ptr debug_data_ptr) const; + const std::shared_ptr prev_trajs, const bool enable_avoidance, const CVMaps & maps, + DebugData & debug_data) const; void calcPlanningFromEgo(std::vector & ref_points) const; @@ -210,25 +215,24 @@ class MPTOptimizer std::vector convertToReferencePoints( const std::vector & points, const std::unique_ptr & prev_mpt_points, const bool enable_avoidance, - const CVMaps & maps, std::shared_ptr debug_data_ptr) const; + const CVMaps & maps, DebugData & debug_data) const; */ std::vector getFixedReferencePoints( const std::vector & points, - const std::unique_ptr & prev_trajs) const; + const std::shared_ptr prev_trajs) const; void calcBounds( std::vector & ref_points, const bool enable_avoidance, const CVMaps & maps, - const std::unique_ptr & prev_trajs, - std::shared_ptr debug_data_ptr) const; + const std::shared_ptr prev_trajs, DebugData & debug_data) const; void calcVehicleBounds( - std::vector & ref_points, const CVMaps & maps, - std::shared_ptr debug_data_ptr, const bool enable_avoidance) const; + std::vector & ref_points, const CVMaps & maps, DebugData & debug_data, + const bool enable_avoidance) const; // void calcFixState( // std::vector & ref_points, - // const std::unique_ptr & prev_trajs) const; + // const std::shared_ptr prev_trajs) const; void calcOrientation(std::vector & ref_points) const; @@ -237,8 +241,7 @@ class MPTOptimizer void calcArcLength(std::vector & ref_points) const; void calcExtraPoints( - std::vector & ref_points, - const std::unique_ptr & prev_trajs) const; + std::vector & ref_points, const std::shared_ptr prev_trajs) const; /* * predict equation: Xec = Aex * x0 + Bex * Uex + Wex @@ -246,34 +249,33 @@ class MPTOptimizer * Uex Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) */ MPTMatrix generateMPTMatrix( - const std::vector & reference_points, - std::shared_ptr debug_data_ptr) const; + const std::vector & reference_points, DebugData & debug_data) const; ValueMatrix generateValueMatrix( const std::vector & reference_points, const std::vector & path_points, - std::shared_ptr debug_data_ptr) const; + DebugData & debug_data) const; void addSteerWeightR( std::vector> & Rex_triplet_vec, const std::vector & ref_points) const; boost::optional executeOptimization( - const std::unique_ptr & prev_trajs, const bool enable_avoidance, + const std::shared_ptr prev_trajs, const bool enable_avoidance, const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, - const std::vector & ref_points, std::shared_ptr debug_data_ptr); + const std::vector & ref_points, DebugData & debug_data); std::vector getMPTPoints( std::vector & fixed_ref_points, std::vector & non_fixed_ref_points, const Eigen::VectorXd & Uex, - const MPTMatrix & mpt_matrix, std::shared_ptr debug_data_ptr); + const MPTMatrix & mpt_matrix, DebugData & debug_data); std::vector getMPTFixedPoints( const std::vector & ref_points) const; BoundsCandidates getBoundsCandidates( const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, - const CVMaps & maps, std::shared_ptr debug_data_ptr) const; + const CVMaps & maps, DebugData & debug_data) const; CollisionType getCollisionType( const CVMaps & maps, const bool enable_avoidance, @@ -286,13 +288,11 @@ class MPTOptimizer ObjectiveMatrix getObjectiveMatrix( const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, - [[maybe_unused]] const std::vector & ref_points, - std::shared_ptr debug_data_ptr) const; + [[maybe_unused]] const std::vector & ref_points, DebugData & debug_data) const; ConstraintMatrix getConstraintMatrix( const bool enable_avoidance, const MPTMatrix & mpt_mat, - const std::vector & ref_points, - std::shared_ptr debug_data_ptr) const; + const std::vector & ref_points, DebugData & debug_data) const; public: MPTOptimizer( @@ -303,9 +303,9 @@ class MPTOptimizer const bool enable_avoidance, const std::vector & smoothed_points, const std::vector & path_points, - const std::unique_ptr & prev_trajs, const CVMaps & maps, + const std::shared_ptr prev_trajs, const CVMaps & maps, const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, - std::shared_ptr debug_data_ptr); + DebugData & debug_data); }; #endif // OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index d1a83ea4d1728..8c4bcd88dde42 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -19,6 +19,7 @@ #include "obstacle_avoidance_planner/costmap_generator.hpp" #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "obstacle_avoidance_planner/replan_checker.hpp" #include "opencv2/core.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" @@ -48,6 +49,15 @@ #include #include +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using nav_msgs::msg::OccupancyGrid; +using nav_msgs::msg::Odometry; + namespace { template @@ -143,8 +153,10 @@ double lerpPoseZ( class ObstacleAvoidancePlanner : public rclcpp::Node { +public: + explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); + private: - OnSetParametersCallbackHandle::SharedPtr set_param_res_; rclcpp::Clock logger_ros_clock_; int eb_solved_count_; bool is_driving_forward_{true}; @@ -166,15 +178,11 @@ class ObstacleAvoidancePlanner : public rclcpp::Node int vehicle_circle_num_for_calculation_; std::vector vehicle_circle_radius_ratios_; - // params for replan - double max_path_shape_change_dist_for_replan_; - double max_ego_moving_dist_for_replan_; - double max_delta_time_sec_for_replan_; - - // logic + // core algorithm std::unique_ptr costmap_generator_ptr_; std::unique_ptr eb_path_optimizer_ptr_; std::unique_ptr mpt_optimizer_ptr_; + std::shared_ptr replan_checker_; // params TrajectoryParam traj_param_; @@ -183,113 +191,105 @@ class ObstacleAvoidancePlanner : public rclcpp::Node MPTParam mpt_param_; int mpt_visualize_sampling_num_; - // debug - mutable std::shared_ptr debug_data_ptr_; + // variables for debug + mutable DebugData debug_data_; mutable tier4_autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; - geometry_msgs::msg::Pose current_ego_pose_; + // variables for subscribers std::unique_ptr current_twist_ptr_; - std::unique_ptr prev_ego_pose_ptr_; - std::unique_ptr prev_optimal_trajs_ptr_; - std::unique_ptr> prev_path_points_ptr_; - std::unique_ptr objects_ptr_; + std::unique_ptr objects_ptr_; + + // variables for previous information + std::shared_ptr prev_mpt_trajs_ptr_; + std::shared_ptr> prev_eb_traj_ptr_; - std::unique_ptr latest_replanned_time_ptr_; tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; // ROS - rclcpp::Publisher::SharedPtr traj_pub_; - rclcpp::Publisher::SharedPtr - debug_extended_fixed_traj_pub_; - rclcpp::Publisher::SharedPtr - debug_extended_non_fixed_traj_pub_; - rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; - rclcpp::Publisher::SharedPtr - debug_mpt_fixed_traj_pub_; - rclcpp::Publisher::SharedPtr - debug_mpt_ref_traj_pub_; - rclcpp::Publisher::SharedPtr debug_mpt_traj_pub_; + rclcpp::Publisher::SharedPtr traj_pub_; + rclcpp::Publisher::SharedPtr debug_extended_fixed_traj_pub_; + rclcpp::Publisher::SharedPtr debug_extended_non_fixed_traj_pub_; + rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; + rclcpp::Publisher::SharedPtr debug_mpt_fixed_traj_pub_; + rclcpp::Publisher::SharedPtr debug_mpt_ref_traj_pub_; + rclcpp::Publisher::SharedPtr debug_mpt_traj_pub_; rclcpp::Publisher::SharedPtr debug_markers_pub_; rclcpp::Publisher::SharedPtr debug_wall_markers_pub_; - rclcpp::Publisher::SharedPtr debug_clearance_map_pub_; - rclcpp::Publisher::SharedPtr debug_object_clearance_map_pub_; - rclcpp::Publisher::SharedPtr debug_area_with_objects_pub_; + rclcpp::Publisher::SharedPtr debug_clearance_map_pub_; + rclcpp::Publisher::SharedPtr debug_object_clearance_map_pub_; + rclcpp::Publisher::SharedPtr debug_area_with_objects_pub_; rclcpp::Publisher::SharedPtr debug_msg_pub_; - rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr - objects_sub_; + rclcpp::Subscription::SharedPtr path_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr is_avoidance_sub_; - // param callback function - rcl_interfaces::msg::SetParametersResult paramCallback( + // callback function for dynamic parameters + rcl_interfaces::msg::SetParametersResult onParam( const std::vector & parameters); + OnSetParametersCallbackHandle::SharedPtr set_param_res_; // subscriber callback functions - void odomCallback(const nav_msgs::msg::Odometry::SharedPtr); - void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr); - void enableAvoidanceCallback(const tier4_planning_msgs::msg::EnableAvoidance::SharedPtr); - void pathCallback(const autoware_auto_planning_msgs::msg::Path::SharedPtr); + void onOdometry(const Odometry::SharedPtr); + void onObjects(const PredictedObjects::SharedPtr); + void onEnableAvoidance(const tier4_planning_msgs::msg::EnableAvoidance::SharedPtr); + void onPath(const Path::SharedPtr); // functions void resetPlanning(); void resetPrevOptimization(); - std::vector generateOptimizedTrajectory( - const autoware_auto_planning_msgs::msg::Path & input_path); + std::vector generateOptimizedTrajectory(const PlannerData & planner_data); - bool checkReplan(const std::vector & path_points); + Trajectory generateTrajectory(const PlannerData & planner_data); - autoware_auto_planning_msgs::msg::Trajectory generateTrajectory( - const autoware_auto_planning_msgs::msg::Path & path); + std::vector optimizeTrajectory( + const PlannerData & planner_data, const CVMaps & cv_maps); - Trajectories optimizeTrajectory( - const autoware_auto_planning_msgs::msg::Path & path, const CVMaps & cv_maps); - - Trajectories getPrevTrajs( - const std::vector & path_points) const; + std::vector getPrevOptimizedTrajectory( + const std::vector & path_points) const; void calcVelocity( - const std::vector & path_points, - std::vector & traj_points) const; + const std::vector & path_points, std::vector & traj_points) const; void insertZeroVelocityOutsideDrivableArea( - std::vector & traj_points, + const PlannerData & planner_data, std::vector & traj_points, const CVMaps & cv_maps); void publishDebugDataInOptimization( - const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & traj_points); - - Trajectories makePrevTrajectories( - const std::vector & path_points, - const Trajectories & trajs); - - std::vector generatePostProcessedTrajectory( - const std::vector & path_points, - const std::vector & merged_optimized_points); - - std::vector getExtendedTrajectory( - const std::vector & path_points, - const std::vector & optimized_points); - - std::vector generateFineTrajectoryPoints( - const std::vector & path_points, - const std::vector & traj_points) const; - - std::vector alignVelocity( - const std::vector & fine_traj_points, - const std::vector & path_points, - const std::vector & traj_points) const; - - void publishDebugDataInMain(const autoware_auto_planning_msgs::msg::Path & path) const; - -public: - explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); + const PlannerData & planner_data, const std::vector & traj_points); + + std::vector generatePostProcessedTrajectory( + const std::vector & path_points, + const std::vector & merged_optimized_points, const PlannerData & planner_data); + + std::vector getExtendedTrajectory( + const std::vector & path_points, + const std::vector & optimized_points); + + std::vector generateFineTrajectoryPoints( + const std::vector & path_points, + const std::vector & traj_points) const; + + std::vector alignVelocity( + const std::vector & fine_traj_points, + const std::vector & path_points, + const std::vector & traj_points) const; + + void publishDebugDataInMain(const Path & path) const; + + template + size_t findEgoNearestIndex( + const std::vector & points, const geometry_msgs::msg::Pose & ego_pose) + { + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, ego_pose, traj_param_.ego_nearest_dist_threshold, + traj_param_.ego_nearest_yaw_threshold); + } }; #endif // OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/process_cv.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/process_cv.hpp deleted file mode 100644 index 8556b90ada41d..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/process_cv.hpp +++ /dev/null @@ -1,144 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__PROCESS_CV_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__PROCESS_CV_HPP_ - -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" - -#include - -#include -#include -#include - -#include - -#include - -namespace util -{ -struct Footprint; -} - -struct CVMaps -{ - cv::Mat drivable_area; - cv::Mat clearance_map; - cv::Mat only_objects_clearance_map; - cv::Mat area_with_objects_map; - nav_msgs::msg::MapMetaData map_info; -}; - -struct Edges -{ - int front_idx; - int back_idx; - geometry_msgs::msg::Point extended_front; - geometry_msgs::msg::Point extended_back; - geometry_msgs::msg::Point origin; -}; - -struct PolygonPoints -{ - std::vector points_in_image; - std::vector points_in_map; -}; - -namespace process_cv -{ -void getOccupancyGridValue( - const nav_msgs::msg::OccupancyGrid & og, const int i, const int j, unsigned char & value); - -void putOccupancyGridValue( - nav_msgs::msg::OccupancyGrid & og, const int i, const int j, const unsigned char value); - -cv::Mat drawObstaclesOnImage( - const bool enable_avoidance, - const std::vector & objects, - const nav_msgs::msg::MapMetaData & map_info, const cv::Mat & clearance_map, - const TrajectoryParam & traj_param, - std::vector * debug_avoiding_objects); - -PolygonPoints getPolygonPoints( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); - -PolygonPoints getPolygonPointsFromBB( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); -PolygonPoints getPolygonPointsFromCircle( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); -PolygonPoints getPolygonPointsFromPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); - -bool isAvoidingObject( - const PolygonPoints & polygon_points, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info, - const std::vector & path_points, - const TrajectoryParam & traj_param); - -bool isAvoidingObjectType( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const TrajectoryParam & traj_param); - -std::vector getCVPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const PolygonPoints & polygon_points, - const std::vector & path_points, - const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info); - -std::vector getDefaultCVPolygon( - const std::vector & points_in_image); - -std::vector getExtendedCVPolygon( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info); - -cv::Mat getDrivableAreaInCV(const nav_msgs::msg::OccupancyGrid & occupancy_grid); - -cv::Mat getClearanceMap(const cv::Mat & drivable_area); - -cv::Mat getAreaWithObjects(const cv::Mat & drivable_area, const cv::Mat & objects_image); - -boost::optional getEdges( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info); - -bool arePointsInsideDriveableArea( - const std::vector & image_points, const cv::Mat & clearance_map); - -boost::optional getDistance( - const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & map_info); - -boost::optional getStopIdx( - const std::vector & vehicle_footprints, - const geometry_msgs::msg::Pose & ego_pose, const cv::Mat & road_clearance_map, - const nav_msgs::msg::MapMetaData & map_info); - -CVMaps getMaps( - const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & objects, - const TrajectoryParam & traj_param, DebugData * debug_data); -} // namespace process_cv -#endif // OBSTACLE_AVOIDANCE_PLANNER__PROCESS_CV_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp new file mode 100644 index 0000000000000..f2539567b5b39 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp @@ -0,0 +1,75 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "obstacle_avoidance_planner/utils/utils.hpp" + +#include + +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include +#include + +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +class ReplanChecker +{ +public: + explicit ReplanChecker( + rclcpp::Node & node, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold); + void onParam(const std::vector & parameters); + bool isReplanRequired( + const PlannerData & planner_data, const rclcpp::Time & current_time, + const std::shared_ptr prev_mpt_trajs_ptr); + bool isResetOptimizationRequired(); + +private: + bool check(const PlannerData & planner_data, const rclcpp::Time & current_time); + bool isPathShapeChanged(const PlannerData & planner_data); + bool isPathGoalChanged(const PlannerData & planner_data); + + template + size_t findEgoNearestIndex( + const std::vector & points, const geometry_msgs::msg::Pose & ego_pose) + { + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, ego_pose, ego_nearest_dist_threshold_, ego_nearest_yaw_threshold_); + } + + // previous variables + std::shared_ptr> prev_path_points_ptr_{nullptr}; + std::shared_ptr prev_replanned_time_ptr_{nullptr}; + std::shared_ptr prev_ego_pose_ptr_{nullptr}; + + bool reset_optimization_{false}; + + // algorithm parameters + double max_path_shape_change_dist_; + double max_ego_moving_dist_; + double max_delta_time_sec_; + + double ego_nearest_dist_threshold_; + double ego_nearest_yaw_threshold_; +}; + +#endif // OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/cv_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp similarity index 96% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/cv_utils.hpp rename to planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp index b05a6edeb94d7..da5edace43536 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/cv_utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__CV_UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__CV_UTILS_HPP_ +#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_ #include "obstacle_avoidance_planner/common_structs.hpp" #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" @@ -118,4 +118,4 @@ bool isOutsideDrivableAreaFromCirclesFootprint( const double vehicle_circle_radius); } // namespace cv_drivable_area_utils -#endif // OBSTACLE_AVOIDANCE_PLANNER__CV_UTILS_HPP_ +#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_visualization.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp similarity index 80% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_visualization.hpp rename to planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp index ce7bda4bd1c7f..083b5dd00efec 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_visualization.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__DEBUG_VISUALIZATION_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__DEBUG_VISUALIZATION_HPP_ +#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_ #include "obstacle_avoidance_planner/common_structs.hpp" #include "opencv2/core.hpp" @@ -33,18 +33,18 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; -namespace debug_visualization +namespace debug_utils { visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( - const std::shared_ptr debug_data_ptr, + DebugData & debug_data, const std::vector & optimized_points, const VehicleParam & vehicle_param, const bool is_showing_debug_detail); visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( - const std::shared_ptr debug_data_ptr, const VehicleParam & vehicle_param); + DebugData & debug_data, const VehicleParam & vehicle_param); nav_msgs::msg::OccupancyGrid getDebugCostmap( const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid); -} // namespace debug_visualization +} // namespace debug_utils -#endif // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_VISUALIZATION_HPP_ +#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp similarity index 98% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp rename to planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp index b21254ef20297..0a0c0f10f4c68 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTILS_HPP_ +#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" @@ -344,4 +344,4 @@ namespace utils void logOSQPSolutionStatus(const int solution_status, const std::string & msg); } // namespace utils -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS_HPP_ +#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp index 078a453cb461a..0936ccc9854c3 100644 --- a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp +++ b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp @@ -14,8 +14,8 @@ #include "obstacle_avoidance_planner/costmap_generator.hpp" -#include "obstacle_avoidance_planner/cv_utils.hpp" -#include "obstacle_avoidance_planner/utils.hpp" +#include "obstacle_avoidance_planner/utils/cv_utils.hpp" +#include "obstacle_avoidance_planner/utils/utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" namespace @@ -146,39 +146,37 @@ geometry_msgs::msg::Point getPoint(const cv::Point & p) CVMaps CostmapGenerator::getMaps( const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, const std::vector & objects, - const TrajectoryParam & traj_param, std::shared_ptr debug_data_ptr) + const TrajectoryParam & traj_param, DebugData & debug_data) { stop_watch_.tic(__func__); // make cv_maps CVMaps cv_maps; - cv_maps.drivable_area = getDrivableAreaInCV(path.drivable_area, debug_data_ptr); - cv_maps.clearance_map = getClearanceMap(cv_maps.drivable_area, debug_data_ptr); + cv_maps.drivable_area = getDrivableAreaInCV(path.drivable_area, debug_data); + cv_maps.clearance_map = getClearanceMap(cv_maps.drivable_area, debug_data); std::vector debug_avoiding_objects; cv::Mat objects_image = drawObstaclesOnImage( enable_avoidance, objects, path.points, path.drivable_area.info, cv_maps.drivable_area, - cv_maps.clearance_map, traj_param, &debug_avoiding_objects, debug_data_ptr); + cv_maps.clearance_map, traj_param, &debug_avoiding_objects, debug_data); cv_maps.area_with_objects_map = - getAreaWithObjects(cv_maps.drivable_area, objects_image, debug_data_ptr); - cv_maps.only_objects_clearance_map = getClearanceMap(objects_image, debug_data_ptr); + getAreaWithObjects(cv_maps.drivable_area, objects_image, debug_data); + cv_maps.only_objects_clearance_map = getClearanceMap(objects_image, debug_data); cv_maps.map_info = path.drivable_area.info; // debug data - debug_data_ptr->clearance_map = cv_maps.clearance_map; - debug_data_ptr->only_object_clearance_map = cv_maps.only_objects_clearance_map; - debug_data_ptr->area_with_objects_map = cv_maps.area_with_objects_map; - debug_data_ptr->avoiding_objects = debug_avoiding_objects; - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.clearance_map = cv_maps.clearance_map; + debug_data.only_object_clearance_map = cv_maps.only_objects_clearance_map; + debug_data.area_with_objects_map = cv_maps.area_with_objects_map; + debug_data.avoiding_objects = debug_avoiding_objects; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return cv_maps; } cv::Mat CostmapGenerator::getDrivableAreaInCV( - const nav_msgs::msg::OccupancyGrid & occupancy_grid, - std::shared_ptr debug_data_ptr) const + const nav_msgs::msg::OccupancyGrid & occupancy_grid, DebugData & debug_data) const { stop_watch_.tic(__func__); @@ -188,21 +186,19 @@ cv::Mat CostmapGenerator::getDrivableAreaInCV( cv_utils::getOccupancyGridValue(occupancy_grid, position[0], position[1], value); }); - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return drivable_area; } cv::Mat CostmapGenerator::getClearanceMap( - const cv::Mat & drivable_area, std::shared_ptr debug_data_ptr) const + const cv::Mat & drivable_area, DebugData & debug_data) const { stop_watch_.tic(__func__); cv::Mat clearance_map; cv::distanceTransform(drivable_area, clearance_map, cv::DIST_L2, 5); - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return clearance_map; } @@ -213,7 +209,7 @@ cv::Mat CostmapGenerator::drawObstaclesOnImage( const nav_msgs::msg::MapMetaData & map_info, [[maybe_unused]] const cv::Mat & drivable_area, const cv::Mat & clearance_map, const TrajectoryParam & traj_param, std::vector * debug_avoiding_objects, - std::shared_ptr debug_data_ptr) const + DebugData & debug_data) const { stop_watch_.tic(__func__); @@ -324,21 +320,18 @@ cv::Mat CostmapGenerator::drawObstaclesOnImage( } */ - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return objects_image; } cv::Mat CostmapGenerator::getAreaWithObjects( - const cv::Mat & drivable_area, const cv::Mat & objects_image, - std::shared_ptr debug_data_ptr) const + const cv::Mat & drivable_area, const cv::Mat & objects_image, DebugData & debug_data) const { stop_watch_.tic(__func__); cv::Mat area_with_objects = cv::min(objects_image, drivable_area); - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return area_with_objects; } diff --git a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp index c50f8d724aaf4..998a7e157f46d 100644 --- a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp @@ -14,7 +14,7 @@ #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/utils.hpp" +#include "obstacle_avoidance_planner/utils/utils.hpp" #include "geometry_msgs/msg/vector3.hpp" @@ -90,8 +90,8 @@ Eigen::MatrixXd EBPathOptimizer::makeAMatrix() boost::optional> EBPathOptimizer::getEBTrajectory( const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path, - const std::unique_ptr & prev_trajs, const double current_ego_vel, - std::shared_ptr debug_data_ptr) + const std::shared_ptr> prev_traj, const double current_ego_vel, + DebugData & debug_data) { stop_watch_.tic(__func__); @@ -100,7 +100,7 @@ EBPathOptimizer::getEBTrajectory( // get candidate points for optimization // decide fix or non fix, might not required only for smoothing purpose const CandidatePoints candidate_points = - getCandidatePoints(ego_pose, path.points, prev_trajs, debug_data_ptr); + getCandidatePoints(ego_pose, path.points, prev_traj, debug_data); if (candidate_points.fixed_points.empty() && candidate_points.non_fixed_points.empty()) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, @@ -109,7 +109,7 @@ EBPathOptimizer::getEBTrajectory( } // get optimized smooth points with elastic band - const auto eb_traj_points = getOptimizedTrajectory(path, candidate_points, debug_data_ptr); + const auto eb_traj_points = getOptimizedTrajectory(path, candidate_points, debug_data); if (!eb_traj_points) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, @@ -117,15 +117,14 @@ EBPathOptimizer::getEBTrajectory( return boost::none; } - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return eb_traj_points; } boost::optional> EBPathOptimizer::getOptimizedTrajectory( const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points, - std::shared_ptr debug_data_ptr) + DebugData & debug_data) { stop_watch_.tic(__func__); @@ -142,7 +141,7 @@ EBPathOptimizer::getOptimizedTrajectory( return boost::none; } - debug_data_ptr->interpolated_points = interpolated_points; + debug_data.interpolated_points = interpolated_points; // number of optimizing points const int farthest_idx = std::min( (eb_param_.num_sampling_points_for_eb - 1), static_cast(interpolated_points.size() - 1)); @@ -155,7 +154,7 @@ EBPathOptimizer::getOptimizedTrajectory( // consider straight after `straight_line_idx` and then tighten space for optimization after // `straight_line_idx` const int straight_line_idx = - getStraightLineIdx(interpolated_points, farthest_idx, debug_data_ptr->straight_points); + getStraightLineIdx(interpolated_points, farthest_idx, debug_data.straight_points); // if `farthest_idx` is lower than `number_of_sampling_points`, duplicate the point at the end of // `interpolated_points` @@ -170,13 +169,13 @@ EBPathOptimizer::getOptimizedTrajectory( } const auto traj_points = - calculateTrajectory(padded_interpolated_points, rectangles.get(), farthest_idx, debug_data_ptr); + calculateTrajectory(padded_interpolated_points, rectangles.get(), farthest_idx, debug_data); if (!traj_points) { return boost::none; } - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return *traj_points; } @@ -184,7 +183,7 @@ boost::optional> EBPathOptimizer::calculateTrajectory( const std::vector & padded_interpolated_points, const std::vector & constrain_rectangles, const int farthest_idx, - std::shared_ptr debug_data_ptr) + DebugData & debug_data) { stop_watch_.tic(__func__); @@ -203,10 +202,8 @@ EBPathOptimizer::calculateTrajectory( const auto traj_points = convertOptimizedPointsToTrajectory(optimized_points, constrain_rectangles, farthest_idx); - if (debug_data_ptr) { - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - } + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return traj_points; } @@ -227,17 +224,17 @@ std::pair, int64_t> EBPathOptimizer::solveQP() std::vector EBPathOptimizer::getFixedPoints( const geometry_msgs::msg::Pose & ego_pose, [[maybe_unused]] const std::vector & path_points, - const std::unique_ptr & prev_trajs) + const std::shared_ptr> prev_traj) { /* use of prev_traj_points(fine resolution) instead of prev_opt_traj(coarse resolution) stabilize trajectory's yaw*/ - if (prev_trajs) { - if (prev_trajs->smoothed_trajectory.empty()) { + if (prev_traj) { + if (prev_traj->empty()) { std::vector empty_points; return empty_points; } const size_t begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - prev_trajs->smoothed_trajectory, ego_pose, traj_param_.ego_nearest_dist_threshold, + *prev_traj, ego_pose, traj_param_.ego_nearest_dist_threshold, traj_param_.ego_nearest_yaw_threshold); const int backward_fixing_idx = std::max( static_cast( @@ -254,10 +251,10 @@ std::vector EBPathOptimizer::getFixedPoints( const int forward_fixing_idx = std::min( static_cast( begin_idx + forward_fixed_length / traj_param_.delta_arc_length_for_trajectory), - static_cast(prev_trajs->smoothed_trajectory.size() - 1)); + static_cast(prev_traj->size() - 1)); std::vector fixed_points; for (int i = backward_fixing_idx; i <= forward_fixing_idx; i++) { - fixed_points.push_back(prev_trajs->smoothed_trajectory.at(i).pose); + fixed_points.push_back(prev_traj->at(i).pose); } return fixed_points; } else { @@ -269,10 +266,10 @@ std::vector EBPathOptimizer::getFixedPoints( EBPathOptimizer::CandidatePoints EBPathOptimizer::getCandidatePoints( const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, - const std::unique_ptr & prev_trajs, std::shared_ptr debug_data_ptr) + const std::shared_ptr> prev_traj, DebugData & debug_data) { const std::vector fixed_points = - getFixedPoints(ego_pose, path_points, prev_trajs); + getFixedPoints(ego_pose, path_points, prev_traj); if (fixed_points.empty()) { CandidatePoints candidate_points = getDefaultCandidatePoints(path_points); return candidate_points; @@ -304,8 +301,8 @@ EBPathOptimizer::CandidatePoints EBPathOptimizer::getCandidatePoints( candidate_points.begin_path_idx = begin_idx; candidate_points.end_path_idx = path_points.size() - 1; - debug_data_ptr->fixed_points = candidate_points.fixed_points; - debug_data_ptr->non_fixed_points = candidate_points.non_fixed_points; + debug_data.fixed_points = candidate_points.fixed_points; + debug_data.non_fixed_points = candidate_points.non_fixed_points; return candidate_points; } diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 1dea0206252eb..b5b21b7c91826 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -15,7 +15,7 @@ #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "obstacle_avoidance_planner/utils.hpp" +#include "obstacle_avoidance_planner/utils/utils.hpp" #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "tf2/utils.h" @@ -217,13 +217,13 @@ MPTOptimizer::MPTOptimizer( { } -boost::optional MPTOptimizer::getModelPredictiveTrajectory( +boost::optional MPTOptimizer::getModelPredictiveTrajectory( const bool enable_avoidance, const std::vector & smoothed_points, const std::vector & path_points, - const std::unique_ptr & prev_trajs, const CVMaps & maps, + const std::shared_ptr prev_trajs, const CVMaps & maps, const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, - std::shared_ptr debug_data_ptr) + DebugData & debug_data) { stop_watch_.tic(__func__); @@ -238,7 +238,7 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto } std::vector full_ref_points = - getReferencePoints(smoothed_points, prev_trajs, enable_avoidance, maps, debug_data_ptr); + getReferencePoints(smoothed_points, prev_trajs, enable_avoidance, maps, debug_data); if (full_ref_points.empty()) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, @@ -251,7 +251,7 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto return boost::none; } - debug_data_ptr->mpt_fixed_traj = getMPTFixedPoints(full_ref_points); + debug_data.mpt_fixed_traj = getMPTFixedPoints(full_ref_points); std::vector fixed_ref_points; std::vector non_fixed_ref_points; @@ -278,13 +278,13 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto } // calculate B and W matrices - const auto mpt_matrix = generateMPTMatrix(non_fixed_ref_points, debug_data_ptr); + const auto mpt_matrix = generateMPTMatrix(non_fixed_ref_points, debug_data); // calculate Q and R matrices - const auto val_matrix = generateValueMatrix(non_fixed_ref_points, path_points, debug_data_ptr); + const auto val_matrix = generateValueMatrix(non_fixed_ref_points, path_points, debug_data); const auto optimized_control_variables = executeOptimization( - prev_trajs, enable_avoidance, mpt_matrix, val_matrix, non_fixed_ref_points, debug_data_ptr); + prev_trajs, enable_avoidance, mpt_matrix, val_matrix, non_fixed_ref_points, debug_data); if (!optimized_control_variables) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, @@ -294,14 +294,13 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto const auto mpt_points = getMPTPoints( fixed_ref_points, non_fixed_ref_points, optimized_control_variables.get(), mpt_matrix, - debug_data_ptr); + debug_data); auto full_optimized_ref_points = fixed_ref_points; full_optimized_ref_points.insert( full_optimized_ref_points.end(), non_fixed_ref_points.begin(), non_fixed_ref_points.end()); - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; MPTTrajs mpt_trajs; mpt_trajs.mpt = mpt_points; @@ -311,8 +310,8 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto std::vector MPTOptimizer::getReferencePoints( const std::vector & smoothed_points, - const std::unique_ptr & prev_trajs, const bool enable_avoidance, - const CVMaps & maps, std::shared_ptr debug_data_ptr) const + const std::shared_ptr prev_trajs, const bool enable_avoidance, const CVMaps & maps, + DebugData & debug_data) const { stop_watch_.tic(__func__); @@ -394,8 +393,8 @@ std::vector MPTOptimizer::getReferencePoints( } // set bounds information - calcBounds(ref_points, enable_avoidance, maps, prev_trajs, debug_data_ptr); - calcVehicleBounds(ref_points, maps, debug_data_ptr, enable_avoidance); + calcBounds(ref_points, enable_avoidance, maps, prev_trajs, debug_data); + calcVehicleBounds(ref_points, maps, debug_data, enable_avoidance); // set extra information (alpha and has_object_collision) // NOTE: This must be after bounds calculation. @@ -406,7 +405,7 @@ std::vector MPTOptimizer::getReferencePoints( ref_points = points_utils::clipForwardPoints(ref_points, 0, ref_length); // bounds information is assigned to debug data after truncating reference points - debug_data_ptr->ref_points = ref_points; + debug_data.ref_points = ref_points; return ref_points; }(); @@ -414,8 +413,8 @@ std::vector MPTOptimizer::getReferencePoints( return std::vector{}; } - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return ref_points; } @@ -471,12 +470,11 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) std::vector MPTOptimizer::getFixedReferencePoints( const std::vector & points, - const std::unique_ptr & prev_trajs) const + const std::shared_ptr prev_trajs) const { if ( - !prev_trajs || prev_trajs->model_predictive_trajectory.empty() || - prev_trajs->mpt_ref_points.empty() || - prev_trajs->model_predictive_trajectory.size() != prev_trajs->mpt_ref_points.size()) { + !prev_trajs || prev_trajs->mpt.empty() || prev_trajs->ref_points.empty() || + prev_trajs->mpt.size() != prev_trajs->ref_points.size()) { return std::vector(); } @@ -484,7 +482,7 @@ std::vector MPTOptimizer::getFixedReferencePoints( return std::vector(); } - const auto & prev_ref_points = prev_trajs->mpt_ref_points; + const auto & prev_ref_points = prev_trajs->ref_points; const int nearest_prev_ref_idx = static_cast(motion_utils::findFirstNearestIndexWithSoftConstraints( points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(prev_ref_points)), @@ -556,7 +554,7 @@ std::vector MPTOptimizer::get // predict equation: x = Bex u + Wex (u includes x_0) // cost function: J = x' Qex x + u' Rex u MPTOptimizer::MPTMatrix MPTOptimizer::generateMPTMatrix( - const std::vector & ref_points, std::shared_ptr debug_data_ptr) const + const std::vector & ref_points, DebugData & debug_data) const { if (ref_points.empty()) { return MPTMatrix{}; @@ -619,15 +617,15 @@ MPTOptimizer::MPTMatrix MPTOptimizer::generateMPTMatrix( m.Bex = Bex; m.Wex = Wex; - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return m; } MPTOptimizer::ValueMatrix MPTOptimizer::generateValueMatrix( const std::vector & ref_points, const std::vector & path_points, - std::shared_ptr debug_data_ptr) const + DebugData & debug_data) const { if (ref_points.empty()) { return ValueMatrix{}; @@ -706,15 +704,15 @@ MPTOptimizer::ValueMatrix MPTOptimizer::generateValueMatrix( m.Qex = Qex_sparse_mat; m.Rex = Rex_sparse_mat; - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return m; } boost::optional MPTOptimizer::executeOptimization( - const std::unique_ptr & prev_trajs, const bool enable_avoidance, + const std::shared_ptr prev_trajs, const bool enable_avoidance, const MPTMatrix & mpt_mat, const ValueMatrix & val_mat, - const std::vector & ref_points, std::shared_ptr debug_data_ptr) + const std::vector & ref_points, DebugData & debug_data) { if (ref_points.empty()) { return Eigen::VectorXd{}; @@ -725,9 +723,8 @@ boost::optional MPTOptimizer::executeOptimization( const size_t N_ref = ref_points.size(); // get matrix - ObjectiveMatrix obj_m = getObjectiveMatrix(mpt_mat, val_mat, ref_points, debug_data_ptr); - ConstraintMatrix const_m = - getConstraintMatrix(enable_avoidance, mpt_mat, ref_points, debug_data_ptr); + ObjectiveMatrix obj_m = getObjectiveMatrix(mpt_mat, val_mat, ref_points, debug_data); + ConstraintMatrix const_m = getConstraintMatrix(enable_avoidance, mpt_mat, ref_points, debug_data); // manual warm start Eigen::VectorXd u0 = Eigen::VectorXd::Zero(obj_m.gradient.size()); @@ -735,31 +732,31 @@ boost::optional MPTOptimizer::executeOptimization( if (mpt_param_.enable_manual_warm_start) { const size_t D_x = vehicle_model_ptr_->getDimX(); - if (prev_trajs && prev_trajs->mpt_ref_points.size() > 1) { + if (prev_trajs && prev_trajs->ref_points.size() > 1) { geometry_msgs::msg::Pose ref_front_point; ref_front_point.position = ref_points.front().p; ref_front_point.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.front().yaw); const size_t seg_idx = findNearestIndexWithSoftYawConstraints( - points_utils::convertToPoints(prev_trajs->mpt_ref_points), ref_front_point, + points_utils::convertToPoints(prev_trajs->ref_points), ref_front_point, traj_param_.ego_nearest_dist_threshold, traj_param_.ego_nearest_yaw_threshold); double offset = motion_utils::calcLongitudinalOffsetToSegment( - prev_trajs->mpt_ref_points, seg_idx, ref_points.front().p); + prev_trajs->ref_points, seg_idx, ref_points.front().p); - u0(0) = prev_trajs->mpt_ref_points.at(seg_idx).optimized_kinematic_state(0); - u0(1) = prev_trajs->mpt_ref_points.at(seg_idx).optimized_kinematic_state(1); + u0(0) = prev_trajs->ref_points.at(seg_idx).optimized_kinematic_state(0); + u0(1) = prev_trajs->ref_points.at(seg_idx).optimized_kinematic_state(1); for (size_t i = 0; i + 1 < N_ref; ++i) { size_t prev_idx = seg_idx + i; - const size_t prev_N_ref = prev_trajs->mpt_ref_points.size(); + const size_t prev_N_ref = prev_trajs->ref_points.size(); if (prev_idx + 2 > prev_N_ref) { prev_idx = static_cast(prev_N_ref) - 2; offset = 0.5; } - const double prev_val = prev_trajs->mpt_ref_points.at(prev_idx).optimized_input; - const double next_val = prev_trajs->mpt_ref_points.at(prev_idx + 1).optimized_input; + const double prev_val = prev_trajs->ref_points.at(prev_idx).optimized_input; + const double next_val = prev_trajs->ref_points.at(prev_idx + 1).optimized_input; u0(D_x + i) = interpolation::lerp(prev_val, next_val, offset); } } @@ -806,16 +803,16 @@ boost::optional MPTOptimizer::executeOptimization( prev_mat_n = H.rows(); prev_mat_m = A.rows(); - debug_data_ptr->msg_stream << " " - << "initOsqp" - << ":= " << stop_watch_.toc("initOsqp") << " [ms]\n"; + debug_data.msg_stream << " " + << "initOsqp" + << ":= " << stop_watch_.toc("initOsqp") << " [ms]\n"; // solve stop_watch_.tic("solveOsqp"); const auto result = osqp_solver_ptr_->optimize(); - debug_data_ptr->msg_stream << " " - << "solveOsqp" - << ":= " << stop_watch_.toc("solveOsqp") << " [ms]\n"; + debug_data.msg_stream << " " + << "solveOsqp" + << ":= " << stop_watch_.toc("solveOsqp") << " [ms]\n"; // check solution status const int solution_status = std::get<3>(result); @@ -837,8 +834,8 @@ boost::optional MPTOptimizer::executeOptimization( const Eigen::VectorXd optimized_control_variables = Eigen::Map(&result_vec[0], DIM_X + (N_ref - 1) * DIM_U); - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; const Eigen::VectorXd optimized_control_variables_with_offset = mpt_param_.enable_manual_warm_start @@ -849,8 +846,7 @@ boost::optional MPTOptimizer::executeOptimization( MPTOptimizer::ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( const MPTMatrix & mpt_mat, const ValueMatrix & val_mat, - [[maybe_unused]] const std::vector & ref_points, - std::shared_ptr debug_data_ptr) const + [[maybe_unused]] const std::vector & ref_points, DebugData & debug_data) const { stop_watch_.tic(__func__); @@ -933,8 +929,8 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( obj_matrix.hessian = full_H; obj_matrix.gradient = full_f; - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return obj_matrix; } @@ -945,8 +941,7 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( // \in \mathbb{R}^{N * (N_vehicle_circle + 1)} MPTOptimizer::ConstraintMatrix MPTOptimizer::getConstraintMatrix( [[maybe_unused]] const bool enable_avoidance, const MPTMatrix & mpt_mat, - const std::vector & ref_points, - [[maybe_unused]] std::shared_ptr debug_data_ptr) const + const std::vector & ref_points, [[maybe_unused]] DebugData & debug_data) const { stop_watch_.tic(__func__); @@ -1135,15 +1130,15 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::getConstraintMatrix( constraint_matrix.lower_bound = lb; constraint_matrix.upper_bound = ub; - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return constraint_matrix; } std::vector MPTOptimizer::getMPTPoints( std::vector & fixed_ref_points, std::vector & non_fixed_ref_points, const Eigen::VectorXd & Uex, - const MPTMatrix & mpt_mat, std::shared_ptr debug_data_ptr) + const MPTMatrix & mpt_mat, DebugData & debug_data) { const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); @@ -1170,7 +1165,7 @@ std::vector MPTOptimizer::get // calculate trajectory from optimization result std::vector traj_points; - debug_data_ptr->vehicle_circles_pose.resize(lat_error_vec.size()); + debug_data.vehicle_circles_pose.resize(lat_error_vec.size()); for (size_t i = 0; i < lat_error_vec.size(); ++i) { auto & ref_point = (i < fixed_ref_points.size()) ? fixed_ref_points.at(i) @@ -1181,8 +1176,8 @@ std::vector MPTOptimizer::get geometry_msgs::msg::Pose ref_pose; ref_pose.position = ref_point.p; ref_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); - debug_data_ptr->mpt_ref_poses.push_back(ref_pose); - debug_data_ptr->lateral_errors.push_back(lat_error); + debug_data.mpt_ref_poses.push_back(ref_pose); + debug_data.lateral_errors.push_back(lat_error); ref_point.optimized_kinematic_state << lat_error_vec.at(i), yaw_error_vec.at(i); if (i >= fixed_ref_points.size()) { @@ -1213,7 +1208,7 @@ std::vector MPTOptimizer::get vehicle_circle_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw + ref_point.alpha); - debug_data_ptr->vehicle_circles_pose.at(i).push_back(vehicle_circle_pose); + debug_data.vehicle_circles_pose.at(i).push_back(vehicle_circle_pose); } } } @@ -1236,8 +1231,8 @@ std::vector MPTOptimizer::get // } // } - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return traj_points; } @@ -1294,7 +1289,7 @@ void MPTOptimizer::calcArcLength(std::vector & ref_points) const } void MPTOptimizer::calcExtraPoints( - std::vector & ref_points, const std::unique_ptr & prev_trajs) const + std::vector & ref_points, const std::shared_ptr prev_trajs) const { for (size_t i = 0; i < ref_points.size(); ++i) { // alpha @@ -1332,8 +1327,8 @@ void MPTOptimizer::calcExtraPoints( // The point are considered to be near the object if nearest previous ref point is near the // object. - if (prev_trajs && !prev_trajs->mpt_ref_points.empty()) { - const auto & prev_ref_points = prev_trajs->mpt_ref_points; + if (prev_trajs && !prev_trajs->ref_points.empty()) { + const auto & prev_ref_points = prev_trajs->ref_points; const auto ref_points_with_yaw = points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)); @@ -1371,18 +1366,18 @@ void MPTOptimizer::addSteerWeightR( void MPTOptimizer::calcBounds( std::vector & ref_points, const bool enable_avoidance, const CVMaps & maps, - const std::unique_ptr & prev_trajs, std::shared_ptr debug_data_ptr) const + const std::shared_ptr prev_trajs, DebugData & debug_data) const { stop_watch_.tic(__func__); // search bounds candidate for each ref points SequentialBoundsCandidates sequential_bounds_candidates; for (const auto & ref_point : ref_points) { - const auto bounds_candidates = getBoundsCandidates( - enable_avoidance, convertRefPointsToPose(ref_point), maps, debug_data_ptr); + const auto bounds_candidates = + getBoundsCandidates(enable_avoidance, convertRefPointsToPose(ref_point), maps, debug_data); sequential_bounds_candidates.push_back(bounds_candidates); } - debug_data_ptr->sequential_bounds_candidates = sequential_bounds_candidates; + debug_data.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) { @@ -1392,8 +1387,8 @@ void MPTOptimizer::calcBounds( // extract only continuous bounds; if (i == 0) { // TODO(murooka) use previous bounds, not widest bounds const auto target_pos = [&]() { - if (prev_trajs && !prev_trajs->mpt_ref_points.empty()) { - return prev_trajs->mpt_ref_points.front().p; + if (prev_trajs && !prev_trajs->ref_points.empty()) { + return prev_trajs->ref_points.front().p; } return current_ego_pose_.position; }(); @@ -1465,14 +1460,14 @@ void MPTOptimizer::calcBounds( } } - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return; } void MPTOptimizer::calcVehicleBounds( std::vector & ref_points, [[maybe_unused]] const CVMaps & maps, - std::shared_ptr debug_data_ptr, [[maybe_unused]] const bool enable_avoidance) const + DebugData & debug_data, [[maybe_unused]] const bool enable_avoidance) const { stop_watch_.tic(__func__); @@ -1549,13 +1544,13 @@ void MPTOptimizer::calcVehicleBounds( } } - debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; } BoundsCandidates MPTOptimizer::getBoundsCandidates( const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, const CVMaps & maps, - [[maybe_unused]] std::shared_ptr debug_data_ptr) const + [[maybe_unused]] DebugData & debug_data) const { BoundsCandidates bounds_candidate; diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 0bd3bf97dd541..9799b99187d2c 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -15,10 +15,10 @@ #include "obstacle_avoidance_planner/node.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" -#include "obstacle_avoidance_planner/cv_utils.hpp" -#include "obstacle_avoidance_planner/debug_visualization.hpp" -#include "obstacle_avoidance_planner/utils.hpp" +#include "motion_utils/motion_utils.hpp" +#include "obstacle_avoidance_planner/utils/cv_utils.hpp" +#include "obstacle_avoidance_planner/utils/debug_utils.hpp" +#include "obstacle_avoidance_planner/utils/utils.hpp" #include "rclcpp/time.hpp" #include "tf2/utils.h" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -36,96 +36,10 @@ #include #include -namespace -{ -bool isPathShapeChanged( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::unique_ptr> & - prev_path_points, - const double max_mpt_length, const double max_path_shape_change_dist, const double dist_threshold, - const double yaw_threshold) -{ - if (!prev_path_points) { - return false; - } - - // truncate prev points from ego pose to fixed end points - const auto prev_begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - *prev_path_points, ego_pose, dist_threshold, yaw_threshold); - const auto truncated_prev_points = - points_utils::clipForwardPoints(*prev_path_points, prev_begin_idx, max_mpt_length); - - // truncate points from ego pose to fixed end points - const auto begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - path_points, ego_pose, dist_threshold, yaw_threshold); - const auto truncated_points = - points_utils::clipForwardPoints(path_points, begin_idx, max_mpt_length); - - // guard for lateral offset - if (truncated_prev_points.size() < 2 || truncated_points.size() < 2) { - return false; - } - - // calculate lateral deviations between truncated path_points and prev_path_points - for (const auto & prev_point : truncated_prev_points) { - const double dist = - std::abs(motion_utils::calcLateralOffset(truncated_points, prev_point.pose.position)); - if (dist > max_path_shape_change_dist) { - return true; - } - } - - return false; -} - -bool isPathGoalChanged( - const double current_vel, - const std::vector & path_points, - const std::unique_ptr> & - prev_path_points) -{ - if (!prev_path_points) { - return false; - } - - constexpr double min_vel = 1e-3; - if (std::abs(current_vel) > min_vel) { - return false; - } - - // NOTE: Path may be cropped and does not contain the goal. - // Therefore we set a large value to distance threshold. - constexpr double max_goal_moving_dist = 1.0; - const double goal_moving_dist = - tier4_autoware_utils::calcDistance2d(path_points.back(), prev_path_points->back()); - if (goal_moving_dist < max_goal_moving_dist) { - return false; - } - - return true; -} +// TODO(murooka) check if velocity is updated while optimization is skipped. -bool hasValidNearestPointFromEgo( - const geometry_msgs::msg::Pose & ego_pose, const Trajectories & trajs, - const TrajectoryParam & traj_param) +namespace { - const auto traj = trajs.model_predictive_trajectory; - const auto interpolated_points = - interpolation_utils::getInterpolatedPoints(traj, traj_param.delta_arc_length_for_trajectory); - - const auto interpolated_poses_with_yaw = - points_utils::convertToPosesWithYawEstimation(interpolated_points); - const auto opt_nearest_idx = motion_utils::findNearestIndex( - interpolated_poses_with_yaw, ego_pose, traj_param.delta_dist_threshold_for_closest_point, - traj_param.delta_yaw_threshold_for_closest_point); - - if (!opt_nearest_idx) { - return false; - } - return true; -} - std::tuple, std::vector> calcVehicleCirclesInfo( const VehicleParam & vehicle_param, const size_t circle_num, const double rear_radius_ratio, const double front_radius_ratio) @@ -174,8 +88,7 @@ std::tuple, std::vector> calcVehicleCirclesInfo( return {radiuses, longitudinal_offsets}; } -[[maybe_unused]] void fillYawInTrajectoryPoint( - std::vector & traj_points) +[[maybe_unused]] void fillYawInTrajectoryPoint(std::vector & traj_points) { std::vector points; for (const auto & traj_point : traj_points) { @@ -244,6 +157,15 @@ size_t searchExtendedZeroVelocityIndex( fine_points, vel_points.at(zero_vel_idx).pose, std::numeric_limits::max(), yaw_threshold); } + +Trajectory createTrajectory( + const std::vector & traj_points, const std_msgs::msg::Header & header) +{ + auto traj = motion_utils::convertToTrajectory(traj_points); + traj.header = header; + + return traj; +} } // namespace ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options) @@ -258,48 +180,41 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n durable_qos.transient_local(); // publisher to other nodes - traj_pub_ = create_publisher("~/output/path", 1); + traj_pub_ = create_publisher("~/output/path", 1); // debug publisher - debug_eb_traj_pub_ = create_publisher( - "~/debug/eb_trajectory", durable_qos); - debug_extended_fixed_traj_pub_ = create_publisher( - "~/debug/extended_fixed_traj", 1); + debug_eb_traj_pub_ = create_publisher("~/debug/eb_trajectory", durable_qos); + debug_extended_fixed_traj_pub_ = create_publisher("~/debug/extended_fixed_traj", 1); debug_extended_non_fixed_traj_pub_ = - create_publisher( - "~/debug/extended_non_fixed_traj", 1); - debug_mpt_fixed_traj_pub_ = - create_publisher("~/debug/mpt_fixed_traj", 1); - debug_mpt_ref_traj_pub_ = - create_publisher("~/debug/mpt_ref_traj", 1); - debug_mpt_traj_pub_ = - create_publisher("~/debug/mpt_traj", 1); + create_publisher("~/debug/extended_non_fixed_traj", 1); + debug_mpt_fixed_traj_pub_ = create_publisher("~/debug/mpt_fixed_traj", 1); + debug_mpt_ref_traj_pub_ = create_publisher("~/debug/mpt_ref_traj", 1); + debug_mpt_traj_pub_ = create_publisher("~/debug/mpt_traj", 1); debug_markers_pub_ = create_publisher("~/debug/marker", durable_qos); debug_wall_markers_pub_ = create_publisher("~/debug/wall_marker", durable_qos); - debug_clearance_map_pub_ = - create_publisher("~/debug/clearance_map", durable_qos); + debug_clearance_map_pub_ = create_publisher("~/debug/clearance_map", durable_qos); debug_object_clearance_map_pub_ = - create_publisher("~/debug/object_clearance_map", durable_qos); + create_publisher("~/debug/object_clearance_map", durable_qos); debug_area_with_objects_pub_ = - create_publisher("~/debug/area_with_objects", durable_qos); + create_publisher("~/debug/area_with_objects", durable_qos); debug_msg_pub_ = create_publisher("~/debug/calculation_time", 1); // subscriber - path_sub_ = create_subscription( + path_sub_ = create_subscription( "~/input/path", rclcpp::QoS{1}, - std::bind(&ObstacleAvoidancePlanner::pathCallback, this, std::placeholders::_1)); - odom_sub_ = create_subscription( + std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); + odom_sub_ = create_subscription( "/localization/kinematic_state", rclcpp::QoS{1}, - std::bind(&ObstacleAvoidancePlanner::odomCallback, this, std::placeholders::_1)); - objects_sub_ = create_subscription( + std::bind(&ObstacleAvoidancePlanner::onOdometry, this, std::placeholders::_1)); + objects_sub_ = create_subscription( "~/input/objects", rclcpp::QoS{10}, - std::bind(&ObstacleAvoidancePlanner::objectsCallback, this, std::placeholders::_1)); + std::bind(&ObstacleAvoidancePlanner::onObjects, this, std::placeholders::_1)); is_avoidance_sub_ = create_subscription( "/planning/scenario_planning/lane_driving/obstacle_avoidance_approval", rclcpp::QoS{10}, - std::bind(&ObstacleAvoidancePlanner::enableAvoidanceCallback, this, std::placeholders::_1)); + std::bind(&ObstacleAvoidancePlanner::onEnableAvoidance, this, std::placeholders::_1)); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); { // vehicle param @@ -545,28 +460,21 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n declare_parameter("advanced.mpt.weight.terminal_path_yaw_error_weight"); } - { // replan - max_path_shape_change_dist_for_replan_ = - declare_parameter("replan.max_path_shape_change_dist"); - max_ego_moving_dist_for_replan_ = - declare_parameter("replan.max_ego_moving_dist_for_replan"); - max_delta_time_sec_for_replan_ = - declare_parameter("replan.max_delta_time_sec_for_replan"); - } - // TODO(murooka) tune this param when avoiding with obstacle_avoidance_planner traj_param_.center_line_width = vehicle_param_.width; // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ObstacleAvoidancePlanner::paramCallback, this, std::placeholders::_1)); + std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); resetPlanning(); + replan_checker_ = std::make_shared( + *this, traj_param_.ego_nearest_dist_threshold, traj_param_.ego_nearest_yaw_threshold); self_pose_listener_.waitForFirstPose(); } -rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::paramCallback( +rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( const std::vector & parameters) { using tier4_autoware_utils::updateParam; @@ -824,37 +732,30 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::paramCallback mpt_param_.terminal_path_yaw_error_weight); } - { // replan - updateParam( - parameters, "replan.max_path_shape_change_dist", max_path_shape_change_dist_for_replan_); - updateParam( - parameters, "replan.max_ego_moving_dist_for_replan", max_ego_moving_dist_for_replan_); - updateParam( - parameters, "replan.max_delta_time_sec_for_replan", max_delta_time_sec_for_replan_); - } - resetPlanning(); + // update parameters for replan checker + replan_checker_->onParam(parameters); + rcl_interfaces::msg::SetParametersResult result; result.successful = true; result.reason = "success"; return result; } -void ObstacleAvoidancePlanner::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) +void ObstacleAvoidancePlanner::onOdometry(const Odometry::SharedPtr msg) { current_twist_ptr_ = std::make_unique(); current_twist_ptr_->header = msg->header; current_twist_ptr_->twist = msg->twist.twist; } -void ObstacleAvoidancePlanner::objectsCallback( - const autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr msg) +void ObstacleAvoidancePlanner::onObjects(const PredictedObjects::SharedPtr msg) { - objects_ptr_ = std::make_unique(*msg); + objects_ptr_ = std::make_unique(*msg); } -void ObstacleAvoidancePlanner::enableAvoidanceCallback( +void ObstacleAvoidancePlanner::onEnableAvoidance( const tier4_planning_msgs::msg::EnableAvoidance::SharedPtr msg) { enable_avoidance_ = msg->enable_avoidance; @@ -872,18 +773,17 @@ void ObstacleAvoidancePlanner::resetPlanning() mpt_optimizer_ptr_ = std::make_unique(is_showing_debug_info_, traj_param_, vehicle_param_, mpt_param_); - prev_path_points_ptr_ = nullptr; resetPrevOptimization(); } void ObstacleAvoidancePlanner::resetPrevOptimization() { - prev_optimal_trajs_ptr_ = nullptr; + prev_eb_traj_ptr_ = nullptr; + prev_mpt_trajs_ptr_ = nullptr; eb_solved_count_ = 0; } -void ObstacleAvoidancePlanner::pathCallback( - const autoware_auto_planning_msgs::msg::Path::SharedPtr path_ptr) +void ObstacleAvoidancePlanner::onPath(const Path::SharedPtr path_ptr) { stop_watch_.tic(__func__); @@ -893,203 +793,127 @@ void ObstacleAvoidancePlanner::pathCallback( return; } - current_ego_pose_ = self_pose_listener_.getCurrentPose()->pose; - debug_data_ptr_ = std::make_shared(); - debug_data_ptr_->init( - is_showing_calculation_time_, mpt_visualize_sampling_num_, current_ego_pose_, + // create planner data + PlannerData planner_data; + planner_data.path = *path_ptr; + planner_data.ego_pose = self_pose_listener_.getCurrentPose()->pose; + planner_data.ego_vel = current_twist_ptr_->twist.linear.x; + planner_data.objects = objects_ptr_->objects; + + // create debug data + debug_data_ = DebugData(); + debug_data_.init( + is_showing_calculation_time_, mpt_visualize_sampling_num_, planner_data.ego_pose, mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets); - autoware_auto_planning_msgs::msg::Trajectory output_traj_msg = generateTrajectory(*path_ptr); + // generate trajectory + const auto output_traj_msg = generateTrajectory(planner_data); // publish debug data publishDebugDataInMain(*path_ptr); { // print and publish debug msg - debug_data_ptr_->msg_stream << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n" - << "========================================"; + debug_data_.msg_stream << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n" + << "========================================"; tier4_debug_msgs::msg::StringStamped debug_msg_msg; debug_msg_msg.stamp = get_clock()->now(); - debug_msg_msg.data = debug_data_ptr_->msg_stream.getString(); + debug_msg_msg.data = debug_data_.msg_stream.getString(); debug_msg_pub_->publish(debug_msg_msg); } - // make previous variables - prev_path_points_ptr_ = - std::make_unique>(path_ptr->points); - prev_ego_pose_ptr_ = std::make_unique(current_ego_pose_); - traj_pub_->publish(output_traj_msg); } -autoware_auto_planning_msgs::msg::Trajectory ObstacleAvoidancePlanner::generateTrajectory( - const autoware_auto_planning_msgs::msg::Path & path) +Trajectory ObstacleAvoidancePlanner::generateTrajectory(const PlannerData & planner_data) { - autoware_auto_planning_msgs::msg::Trajectory output_traj_msg; + const auto & p = planner_data; // TODO(someone): support backward path - const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + const auto is_driving_forward = motion_utils::isDrivingForward(p.path.points); is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; if (!is_driving_forward_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), 3000, "[ObstacleAvoidancePlanner] Backward path is NOT supported. Just converting path to " "trajectory"); - const auto traj_points = points_utils::convertToTrajectoryPoints(path.points); - output_traj_msg = motion_utils::convertToTrajectory(traj_points); - output_traj_msg.header = path.header; - return output_traj_msg; + const auto traj_points = points_utils::convertToTrajectoryPoints(p.path.points); + return createTrajectory(traj_points, p.path.header); } // generate optimized trajectory - const auto optimized_traj_points = generateOptimizedTrajectory(path); + const auto optimized_traj_points = generateOptimizedTrajectory(planner_data); // generate post processed trajectory const auto post_processed_traj_points = - generatePostProcessedTrajectory(path.points, optimized_traj_points); + generatePostProcessedTrajectory(p.path.points, optimized_traj_points, planner_data); // convert to output msg type - output_traj_msg = motion_utils::convertToTrajectory(post_processed_traj_points); - - output_traj_msg.header = path.header; - return output_traj_msg; + return createTrajectory(post_processed_traj_points, p.path.header); } -std::vector -ObstacleAvoidancePlanner::generateOptimizedTrajectory( - const autoware_auto_planning_msgs::msg::Path & path) +std::vector ObstacleAvoidancePlanner::generateOptimizedTrajectory( + const PlannerData & planner_data) { stop_watch_.tic(__func__); - if (reset_prev_optimization_) { - resetPrevOptimization(); - } + const auto & path = planner_data.path; - // return prev trajectory if replan is not required - if (!checkReplan(path.points)) { - if (prev_optimal_trajs_ptr_) { - return prev_optimal_trajs_ptr_->model_predictive_trajectory; - } + // check if optimization is required or not. + // NOTE: previous trajectories information will be reset in some cases. + const bool is_replan_required = + replan_checker_->isReplanRequired(planner_data, now(), prev_mpt_trajs_ptr_); + if (!is_replan_required) { + return getPrevOptimizedTrajectory(path.points); + } - return points_utils::convertToTrajectoryPoints(path.points); + const bool reset_prev_optimization_required = replan_checker_->isResetOptimizationRequired(); + if (reset_prev_optimization_ || reset_prev_optimization_required) { + resetPrevOptimization(); } - latest_replanned_time_ptr_ = std::make_unique(this->now()); // create clearance maps const CVMaps cv_maps = costmap_generator_ptr_->getMaps( - enable_avoidance_, path, objects_ptr_->objects, traj_param_, debug_data_ptr_); + enable_avoidance_, path, planner_data.objects, traj_param_, debug_data_); // calculate trajectory with EB and MPT - auto optimal_trajs = optimizeTrajectory(path, cv_maps); + auto mpt_traj = optimizeTrajectory(planner_data, cv_maps); // calculate velocity // NOTE: Velocity is not considered in optimization. - calcVelocity(path.points, optimal_trajs.model_predictive_trajectory); + calcVelocity(path.points, mpt_traj); // insert 0 velocity when trajectory is over drivable area if (is_stopping_if_outside_drivable_area_) { - insertZeroVelocityOutsideDrivableArea(optimal_trajs.model_predictive_trajectory, cv_maps); - } - - publishDebugDataInOptimization(path, optimal_trajs.model_predictive_trajectory); - - // make previous trajectories - prev_optimal_trajs_ptr_ = - std::make_unique(makePrevTrajectories(path.points, optimal_trajs)); - - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return optimal_trajs.model_predictive_trajectory; -} - -// check if optimization is required or not. -// NOTE: previous trajectories information will be reset as well in some cases. -bool ObstacleAvoidancePlanner::checkReplan( - const std::vector & path_points) -{ - if ( - !prev_ego_pose_ptr_ || !latest_replanned_time_ptr_ || !prev_path_points_ptr_ || - !prev_optimal_trajs_ptr_) { - return true; - } - - if (prev_optimal_trajs_ptr_->model_predictive_trajectory.empty()) { - RCLCPP_INFO( - get_logger(), - "Replan with resetting optimization since previous optimized trajectory is empty."); - resetPrevOptimization(); - return true; - } - - const double max_mpt_length = - traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points; - if (isPathShapeChanged( - current_ego_pose_, path_points, prev_path_points_ptr_, max_mpt_length, - max_path_shape_change_dist_for_replan_, traj_param_.ego_nearest_dist_threshold, - traj_param_.ego_nearest_yaw_threshold)) { - RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path shape was changed."); - resetPrevOptimization(); - return true; - } - - if (isPathGoalChanged(current_twist_ptr_->twist.linear.x, path_points, prev_path_points_ptr_)) { - RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path goal was changed."); - resetPrevOptimization(); - return true; + insertZeroVelocityOutsideDrivableArea(planner_data, mpt_traj, cv_maps); } - // For when ego pose is lost or new ego pose is designated in simulation - const double delta_dist = - tier4_autoware_utils::calcDistance2d(current_ego_pose_.position, prev_ego_pose_ptr_->position); - if (delta_dist > max_ego_moving_dist_for_replan_) { - RCLCPP_INFO( - get_logger(), - "Replan with resetting optimization since current ego pose is far from previous ego pose."); - resetPrevOptimization(); - return true; - } - - // For when ego pose moves far from trajectory - if (!hasValidNearestPointFromEgo(current_ego_pose_, *prev_optimal_trajs_ptr_, traj_param_)) { - RCLCPP_INFO( - get_logger(), - "Replan with resetting optimization since valid nearest trajectory point from ego was not " - "found."); - resetPrevOptimization(); - return true; - } + publishDebugDataInOptimization(planner_data, mpt_traj); - const double delta_time_sec = (this->now() - *latest_replanned_time_ptr_).seconds(); - if (delta_time_sec > max_delta_time_sec_for_replan_) { - return true; - } - return false; + debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; + return mpt_traj; } -Trajectories ObstacleAvoidancePlanner::optimizeTrajectory( - const autoware_auto_planning_msgs::msg::Path & path, const CVMaps & cv_maps) +std::vector ObstacleAvoidancePlanner::optimizeTrajectory( + const PlannerData & planner_data, const CVMaps & cv_maps) { stop_watch_.tic(__func__); + const auto & p = planner_data; + if (skip_optimization_) { - const auto traj = points_utils::convertToTrajectoryPoints(path.points); - Trajectories trajs; - trajs.smoothed_trajectory = traj; - trajs.model_predictive_trajectory = traj; - return trajs; + return points_utils::convertToTrajectoryPoints(p.path.points); } // EB: smooth trajectory if enable_pre_smoothing is true - const auto eb_traj = - [&]() -> boost::optional> { + const auto eb_traj = [&]() -> boost::optional> { if (enable_pre_smoothing_) { return eb_path_optimizer_ptr_->getEBTrajectory( - current_ego_pose_, path, prev_optimal_trajs_ptr_, current_twist_ptr_->twist.linear.x, - debug_data_ptr_); + p.ego_pose, p.path, prev_eb_traj_ptr_, p.ego_vel, debug_data_); } - return points_utils::convertToTrajectoryPoints(path.points); + return points_utils::convertToTrajectoryPoints(p.path.points); }(); if (!eb_traj) { - return getPrevTrajs(path.points); + return getPrevOptimizedTrajectory(p.path.points); } // EB has to be solved twice before solving MPT with fixed points @@ -1098,54 +922,45 @@ Trajectories ObstacleAvoidancePlanner::optimizeTrajectory( if (eb_solved_count_ < 2) { eb_solved_count_++; - if (prev_optimal_trajs_ptr_) { - prev_optimal_trajs_ptr_->model_predictive_trajectory.clear(); - prev_optimal_trajs_ptr_->mpt_ref_points.clear(); + if (prev_mpt_trajs_ptr_) { + prev_mpt_trajs_ptr_->mpt.clear(); + prev_mpt_trajs_ptr_->ref_points.clear(); } } // MPT: optimize trajectory to be kinematically feasible and collision free const auto mpt_trajs = mpt_optimizer_ptr_->getModelPredictiveTrajectory( - enable_avoidance_, eb_traj.get(), path.points, prev_optimal_trajs_ptr_, cv_maps, - current_ego_pose_, current_twist_ptr_->twist.linear.x, debug_data_ptr_); + enable_avoidance_, eb_traj.get(), p.path.points, prev_mpt_trajs_ptr_, cv_maps, p.ego_pose, + p.ego_vel, debug_data_); if (!mpt_trajs) { - return getPrevTrajs(path.points); + return getPrevOptimizedTrajectory(p.path.points); } - // make trajectories, which has all optimized trajectories information - Trajectories trajs; - trajs.smoothed_trajectory = eb_traj.get(); - trajs.mpt_ref_points = mpt_trajs.get().ref_points; - trajs.model_predictive_trajectory = mpt_trajs.get().mpt; + // make prev trajectories + prev_eb_traj_ptr_ = std::make_shared>(eb_traj.get()); + prev_mpt_trajs_ptr_ = std::make_shared(mpt_trajs.get()); // debug data - debug_data_ptr_->mpt_traj = mpt_trajs.get().mpt; - debug_data_ptr_->mpt_ref_traj = - points_utils::convertToTrajectoryPoints(mpt_trajs.get().ref_points); - debug_data_ptr_->eb_traj = eb_traj.get(); - - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - return trajs; + debug_data_.mpt_traj = mpt_trajs.get().mpt; + debug_data_.mpt_ref_traj = points_utils::convertToTrajectoryPoints(mpt_trajs.get().ref_points); + debug_data_.eb_traj = eb_traj.get(); + + debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; + + return mpt_trajs->mpt; } -Trajectories ObstacleAvoidancePlanner::getPrevTrajs( - const std::vector & path_points) const +std::vector ObstacleAvoidancePlanner::getPrevOptimizedTrajectory( + const std::vector & path_points) const { - if (prev_optimal_trajs_ptr_) { - return *prev_optimal_trajs_ptr_; + if (prev_mpt_trajs_ptr_) { + return prev_mpt_trajs_ptr_->mpt; } - - const auto traj = points_utils::convertToTrajectoryPoints(path_points); - Trajectories trajs; - trajs.smoothed_trajectory = traj; - trajs.model_predictive_trajectory = traj; - return trajs; + return points_utils::convertToTrajectoryPoints(path_points); } void ObstacleAvoidancePlanner::calcVelocity( - const std::vector & path_points, - std::vector & traj_points) const + const std::vector & path_points, std::vector & traj_points) const { for (size_t i = 0; i < traj_points.size(); i++) { const size_t nearest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints( @@ -1164,7 +979,7 @@ void ObstacleAvoidancePlanner::calcVelocity( } void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( - std::vector & traj_points, + const PlannerData & planner_data, std::vector & traj_points, const CVMaps & cv_maps) { if (traj_points.empty()) { @@ -1176,9 +991,7 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( const auto & map_info = cv_maps.map_info; const auto & road_clearance_map = cv_maps.clearance_map; - const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - traj_points, current_ego_pose_, traj_param_.ego_nearest_dist_threshold, - traj_param_.ego_nearest_yaw_threshold); + const size_t nearest_idx = findEgoNearestIndex(traj_points, planner_data.ego_pose); // NOTE: Some end trajectory points will be ignored to check if outside the drivable area // since these points might be outside drivable area if only end reference points have high @@ -1203,104 +1016,74 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( // only insert zero velocity to the first point outside drivable area if (is_outside) { traj_points[i].longitudinal_velocity_mps = 0.0; - debug_data_ptr_->stop_pose_by_drivable_area = traj_points[i].pose; + debug_data_.stop_pose_by_drivable_area = traj_points[i].pose; break; } } - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; } void ObstacleAvoidancePlanner::publishDebugDataInOptimization( - const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & traj_points) + const PlannerData & planner_data, const std::vector & traj_points) { stop_watch_.tic(__func__); + const auto & p = planner_data; + { // publish trajectories - auto debug_eb_traj = motion_utils::convertToTrajectory(debug_data_ptr_->eb_traj); - debug_eb_traj.header = path.header; + const auto debug_eb_traj = createTrajectory(debug_data_.eb_traj, p.path.header); debug_eb_traj_pub_->publish(debug_eb_traj); - auto debug_mpt_fixed_traj = motion_utils::convertToTrajectory(debug_data_ptr_->mpt_fixed_traj); - debug_mpt_fixed_traj.header = path.header; + const auto debug_mpt_fixed_traj = createTrajectory(debug_data_.mpt_fixed_traj, p.path.header); debug_mpt_fixed_traj_pub_->publish(debug_mpt_fixed_traj); - auto debug_mpt_ref_traj = motion_utils::convertToTrajectory(debug_data_ptr_->mpt_ref_traj); - debug_mpt_ref_traj.header = path.header; + const auto debug_mpt_ref_traj = createTrajectory(debug_data_.mpt_ref_traj, p.path.header); debug_mpt_ref_traj_pub_->publish(debug_mpt_ref_traj); - auto debug_mpt_traj = motion_utils::convertToTrajectory(debug_data_ptr_->mpt_traj); - debug_mpt_traj.header = path.header; + const auto debug_mpt_traj = createTrajectory(debug_data_.mpt_traj, p.path.header); debug_mpt_traj_pub_->publish(debug_mpt_traj); } { // publish markers if (is_publishing_debug_visualization_marker_) { stop_watch_.tic("getDebugVisualizationMarker"); - const auto & debug_marker = debug_visualization::getDebugVisualizationMarker( - debug_data_ptr_, traj_points, vehicle_param_, false); - debug_data_ptr_->msg_stream << " getDebugVisualizationMarker:= " - << stop_watch_.toc("getDebugVisualizationMarker") << " [ms]\n"; + const auto & debug_marker = + debug_utils::getDebugVisualizationMarker(debug_data_, traj_points, vehicle_param_, false); + debug_data_.msg_stream << " getDebugVisualizationMarker:= " + << stop_watch_.toc("getDebugVisualizationMarker") << " [ms]\n"; stop_watch_.tic("publishDebugVisualizationMarker"); debug_markers_pub_->publish(debug_marker); - debug_data_ptr_->msg_stream << " publishDebugVisualizationMarker:= " - << stop_watch_.toc("publishDebugVisualizationMarker") - << " [ms]\n"; + debug_data_.msg_stream << " publishDebugVisualizationMarker:= " + << stop_watch_.toc("publishDebugVisualizationMarker") << " [ms]\n"; stop_watch_.tic("getDebugVisualizationWallMarker"); const auto & debug_wall_marker = - debug_visualization::getDebugVisualizationWallMarker(debug_data_ptr_, vehicle_param_); - debug_data_ptr_->msg_stream << " getDebugVisualizationWallMarker:= " - << stop_watch_.toc("getDebugVisualizationWallMarker") - << " [ms]\n"; + debug_utils::getDebugVisualizationWallMarker(debug_data_, vehicle_param_); + debug_data_.msg_stream << " getDebugVisualizationWallMarker:= " + << stop_watch_.toc("getDebugVisualizationWallMarker") << " [ms]\n"; stop_watch_.tic("publishDebugVisualizationWallMarker"); debug_wall_markers_pub_->publish(debug_wall_marker); - debug_data_ptr_->msg_stream << " publishDebugVisualizationWallMarker:= " - << stop_watch_.toc("publishDebugVisualizationWallMarker") - << " [ms]\n"; + debug_data_.msg_stream << " publishDebugVisualizationWallMarker:= " + << stop_watch_.toc("publishDebugVisualizationWallMarker") << " [ms]\n"; } } - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; -} - -Trajectories ObstacleAvoidancePlanner::makePrevTrajectories( - const std::vector & path_points, - const Trajectories & trajs) -{ - stop_watch_.tic(__func__); - - const auto post_processed_smoothed_traj = - generatePostProcessedTrajectory(path_points, trajs.smoothed_trajectory); - - // TODO(murooka) generatePoseProcessedTrajectory may be too large - Trajectories trajectories; - trajectories.smoothed_trajectory = post_processed_smoothed_traj; - trajectories.mpt_ref_points = trajs.mpt_ref_points; - trajectories.model_predictive_trajectory = trajs.model_predictive_trajectory; - - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; - - return trajectories; + debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; } -std::vector -ObstacleAvoidancePlanner::generatePostProcessedTrajectory( - const std::vector & path_points, - const std::vector & optimized_traj_points) +std::vector ObstacleAvoidancePlanner::generatePostProcessedTrajectory( + const std::vector & path_points, + const std::vector & optimized_traj_points, const PlannerData & planner_data) { stop_watch_.tic(__func__); - std::vector trajectory_points; + std::vector trajectory_points; if (path_points.empty()) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_point; - tmp_point.pose = current_ego_pose_; + TrajectoryPoint tmp_point; + tmp_point.pose = planner_data.ego_pose; tmp_point.longitudinal_velocity_mps = 0.0; trajectory_points.push_back(tmp_point); return trajectory_points; @@ -1323,15 +1106,12 @@ ObstacleAvoidancePlanner::generatePostProcessedTrajectory( const auto fine_traj_points_with_vel = alignVelocity(fine_traj_points, path_points, full_traj_points); - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return fine_traj_points_with_vel; } -std::vector -ObstacleAvoidancePlanner::getExtendedTrajectory( - const std::vector & path_points, - const std::vector & optimized_points) +std::vector ObstacleAvoidancePlanner::getExtendedTrajectory( + const std::vector & path_points, const std::vector & optimized_points) { stop_watch_.tic(__func__); @@ -1342,7 +1122,7 @@ ObstacleAvoidancePlanner::getExtendedTrajectory( RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), "[Avoidance] Not extend trajectory"); - return std::vector{}; + return std::vector{}; } // calculate end idx of optimized points on path points @@ -1353,11 +1133,10 @@ ObstacleAvoidancePlanner::getExtendedTrajectory( RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), "[Avoidance] Not extend trajectory since could not find nearest idx from last opt point"); - return std::vector{}; + return std::vector{}; } - auto extended_traj_points = - [&]() -> std::vector { + auto extended_traj_points = [&]() -> std::vector { constexpr double non_fixed_traj_length = 5.0; // TODO(murooka) may be better to tune const size_t non_fixed_begin_path_idx = opt_end_path_idx.get(); const size_t non_fixed_end_path_idx = @@ -1369,23 +1148,22 @@ ObstacleAvoidancePlanner::getExtendedTrajectory( if (points_utils::isNearLastPathPoint( optimized_points.back(), path_points, traj_param_.max_dist_for_extending_end_point, traj_param_.delta_yaw_threshold_for_closest_point)) { - return std::vector{}; + return std::vector{}; } const auto last_traj_point = points_utils::convertToTrajectoryPoint(path_points.back()); - return std::vector{last_traj_point}; + return std::vector{last_traj_point}; } else if (non_fixed_end_path_idx == path_points.size() - 1) { // no need to connect smoothly since extended trajectory length is short enough const auto last_traj_point = points_utils::convertToTrajectoryPoint(path_points.back()); - return std::vector{last_traj_point}; + return std::vector{last_traj_point}; } // define non_fixed/fixed_traj_points const auto begin_point = optimized_points.back(); const auto end_point = points_utils::convertToTrajectoryPoint(path_points.at(non_fixed_end_path_idx)); - const std::vector non_fixed_traj_points{ - begin_point, end_point}; - const std::vector fixed_path_points{ + const std::vector non_fixed_traj_points{begin_point, end_point}; + const std::vector fixed_path_points{ path_points.begin() + non_fixed_end_path_idx + 1, path_points.end()}; const auto fixed_traj_points = points_utils::convertToTrajectoryPoints(fixed_path_points); @@ -1401,8 +1179,8 @@ ObstacleAvoidancePlanner::getExtendedTrajectory( extended_points.insert( extended_points.end(), fixed_traj_points.begin(), fixed_traj_points.end()); - debug_data_ptr_->extended_non_fixed_traj = interpolated_non_fixed_traj_points; - debug_data_ptr_->extended_fixed_traj = fixed_traj_points; + debug_data_.extended_non_fixed_traj = interpolated_non_fixed_traj_points; + debug_data_.extended_fixed_traj = fixed_traj_points; return extended_points; }(); @@ -1415,15 +1193,13 @@ ObstacleAvoidancePlanner::getExtendedTrajectory( point.longitudinal_velocity_mps = large_velocity; } - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return extended_traj_points; } -std::vector -ObstacleAvoidancePlanner::generateFineTrajectoryPoints( - const std::vector & path_points, - const std::vector & traj_points) const +std::vector ObstacleAvoidancePlanner::generateFineTrajectoryPoints( + const std::vector & path_points, + const std::vector & traj_points) const { stop_watch_.tic(__func__); @@ -1441,24 +1217,20 @@ ObstacleAvoidancePlanner::generateFineTrajectoryPoints( path_points.back(), interpolated_traj_points, traj_param_.max_dist_for_extending_end_point, traj_param_.delta_yaw_threshold_for_closest_point); - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return interpolated_traj_points; } -std::vector -ObstacleAvoidancePlanner::alignVelocity( - const std::vector & fine_traj_points, - const std::vector & path_points, - const std::vector & traj_points) const +std::vector ObstacleAvoidancePlanner::alignVelocity( + const std::vector & fine_traj_points, const std::vector & path_points, + const std::vector & traj_points) const { stop_watch_.tic(__func__); // insert zero velocity path index, and get optional zero_vel_path_idx - const auto path_zero_vel_info = [&]() - -> std::pair< - std::vector, boost::optional> { + const auto path_zero_vel_info = + [&]() -> std::pair, boost::optional> { const auto opt_path_zero_vel_idx = motion_utils::searchZeroVelocityIndex(path_points); if (opt_path_zero_vel_idx) { const auto & zero_vel_path_point = path_points.at(opt_path_zero_vel_idx.get()); @@ -1469,7 +1241,7 @@ ObstacleAvoidancePlanner::alignVelocity( const auto interpolated_pose = lerpPose(fine_traj_points, zero_vel_path_point.pose.position, opt_traj_seg_idx.get()); if (interpolated_pose) { - autoware_auto_planning_msgs::msg::TrajectoryPoint zero_vel_traj_point; + TrajectoryPoint zero_vel_traj_point; zero_vel_traj_point.pose = interpolated_pose.get(); zero_vel_traj_point.longitudinal_velocity_mps = zero_vel_path_point.longitudinal_velocity_mps; @@ -1520,7 +1292,7 @@ ObstacleAvoidancePlanner::alignVelocity( auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0); if (truncated_points.size() < 2) { // NOTE: At least, two points must be contained in truncated_points - truncated_points = std::vector( + truncated_points = std::vector( path_points.begin() + prev_begin_idx, path_points.begin() + std::min(path_points.size(), prev_begin_idx + 2)); } @@ -1547,25 +1319,21 @@ ObstacleAvoidancePlanner::alignVelocity( prev_begin_idx += closest_seg_idx; } - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return fine_traj_points_with_vel; } -void ObstacleAvoidancePlanner::publishDebugDataInMain( - const autoware_auto_planning_msgs::msg::Path & path) const +void ObstacleAvoidancePlanner::publishDebugDataInMain(const Path & path) const { stop_watch_.tic(__func__); { // publish trajectories - auto debug_extended_fixed_traj = - motion_utils::convertToTrajectory(debug_data_ptr_->extended_fixed_traj); - debug_extended_fixed_traj.header = path.header; + const auto debug_extended_fixed_traj = + createTrajectory(debug_data_.extended_fixed_traj, path.header); debug_extended_fixed_traj_pub_->publish(debug_extended_fixed_traj); - auto debug_extended_non_fixed_traj = - motion_utils::convertToTrajectory(debug_data_ptr_->extended_non_fixed_traj); - debug_extended_non_fixed_traj.header = path.header; + const auto debug_extended_non_fixed_traj = + createTrajectory(debug_data_.extended_non_fixed_traj, path.header); debug_extended_non_fixed_traj_pub_->publish(debug_extended_non_fixed_traj); } @@ -1573,23 +1341,22 @@ void ObstacleAvoidancePlanner::publishDebugDataInMain( stop_watch_.tic("publishClearanceMap"); if (is_publishing_area_with_objects_) { // false - debug_area_with_objects_pub_->publish(debug_visualization::getDebugCostmap( - debug_data_ptr_->area_with_objects_map, path.drivable_area)); + debug_area_with_objects_pub_->publish( + debug_utils::getDebugCostmap(debug_data_.area_with_objects_map, path.drivable_area)); } if (is_publishing_object_clearance_map_) { // false - debug_object_clearance_map_pub_->publish(debug_visualization::getDebugCostmap( - debug_data_ptr_->only_object_clearance_map, path.drivable_area)); + debug_object_clearance_map_pub_->publish( + debug_utils::getDebugCostmap(debug_data_.only_object_clearance_map, path.drivable_area)); } if (is_publishing_clearance_map_) { // true debug_clearance_map_pub_->publish( - debug_visualization::getDebugCostmap(debug_data_ptr_->clearance_map, path.drivable_area)); + debug_utils::getDebugCostmap(debug_data_.clearance_map, path.drivable_area)); } - debug_data_ptr_->msg_stream << " getDebugCostMap * 3:= " - << stop_watch_.toc("publishClearanceMap") << " [ms]\n"; + debug_data_.msg_stream << " getDebugCostMap * 3:= " << stop_watch_.toc("publishClearanceMap") + << " [ms]\n"; } - debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) - << " [ms]\n"; + debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; } #include "rclcpp_components/register_node_macro.hpp" diff --git a/planning/obstacle_avoidance_planner/src/process_cv.cpp b/planning/obstacle_avoidance_planner/src/process_cv.cpp deleted file mode 100644 index f35a9ada665f0..0000000000000 --- a/planning/obstacle_avoidance_planner/src/process_cv.cpp +++ /dev/null @@ -1,575 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#include "obstacle_avoidance_planner/process_cv.hpp" - -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" - -#include -#include - -#include -#include - -#include - -#include -#include - -#include -#include -#include - -namespace process_cv -{ -void getOccupancyGridValue( - const nav_msgs::msg::OccupancyGrid & og, const int i, const int j, unsigned char & value) -{ - int i_flip = og.info.width - i - 1; - int j_flip = og.info.height - j - 1; - if (og.data[i_flip + j_flip * og.info.width] > 0) { - value = 0; - } else { - value = 255; - } -} - -void putOccupancyGridValue( - nav_msgs::msg::OccupancyGrid & og, const int i, const int j, const unsigned char value) -{ - int i_flip = og.info.width - i - 1; - int j_flip = og.info.height - j - 1; - og.data[i_flip + j_flip * og.info.width] = value; -} - -cv::Mat drawObstaclesOnImage( - const bool enable_avoidance, - const std::vector & objects, - const std::vector & path_points, - const nav_msgs::msg::MapMetaData & map_info, const cv::Mat & clearance_map, - const TrajectoryParam & traj_param, - std::vector * debug_avoiding_objects) -{ - std::vector path_points_inside_area; - for (const auto & point : path_points) { - std::vector points; - geometry_msgs::msg::Point image_point; - if (!util::transformMapToImage(point.pose.position, map_info, image_point)) { - continue; - } - const float clearance = - clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)]; - if (clearance < 1e-5) { - continue; - } - path_points_inside_area.push_back(point); - } - cv::Mat objects_image = cv::Mat::ones(clearance_map.rows, clearance_map.cols, CV_8UC1) * 255; - if (!enable_avoidance) { - return objects_image; - } - - std::vector> cv_polygons; - for (const auto & object : objects) { - const PolygonPoints polygon_points = getPolygonPoints(object, map_info); - if (isAvoidingObject( - polygon_points, object, clearance_map, map_info, path_points_inside_area, traj_param)) { - cv_polygons.push_back( - getCVPolygon(object, polygon_points, path_points_inside_area, clearance_map, map_info)); - debug_avoiding_objects->push_back(object); - } - } - for (const auto & polygon : cv_polygons) { - cv::fillConvexPoly(objects_image, polygon, cv::Scalar(0)); - } - return objects_image; -} - -bool isAvoidingObject( - const PolygonPoints & polygon_points, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info, - const std::vector & path_points, - const TrajectoryParam & traj_param) -{ - if (path_points.empty()) { - return false; - } - if (!isAvoidingObjectType(object, traj_param)) { - return false; - } - const auto image_point = util::transformMapToOptionalImage( - object.kinematics.initial_pose_with_covariance.pose.position, map_info); - if (!image_point) { - return false; - } - - const int nearest_idx = - util::getNearestIdx(path_points, object.kinematics.initial_pose_with_covariance.pose.position); - const auto nearest_path_point = path_points[nearest_idx]; - const auto rel_p = util::transformToRelativeCoordinate2D( - object.kinematics.initial_pose_with_covariance.pose.position, nearest_path_point.pose); - // skip object located back the beginning of path points - if (nearest_idx == 0 && rel_p.x < 0) { - return false; - } - - const float object_clearance_from_road = - clearance_map.ptr( - static_cast(image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - const geometry_msgs::msg::Vector3 twist = - object.kinematics.initial_twist_with_covariance.twist.linear; - const double vel = std::sqrt(twist.x * twist.x + twist.y * twist.y + twist.z * twist.z); - const auto nearest_path_point_image = - util::transformMapToOptionalImage(nearest_path_point.pose.position, map_info); - if (!nearest_path_point_image) { - return false; - } - const float nearest_path_point_clearance = - clearance_map.ptr(static_cast( - nearest_path_point_image.get().y))[static_cast(nearest_path_point_image.get().x)] * - map_info.resolution; - if ( - nearest_path_point_clearance - traj_param.center_line_width * 0.5 < - object_clearance_from_road || - vel > traj_param.max_avoiding_objects_velocity_ms || - !arePointsInsideDriveableArea(polygon_points.points_in_image, clearance_map)) { - return false; - } - return true; -} - -bool isAvoidingObjectType( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const TrajectoryParam & traj_param) -{ - if ( - (object.classification.at(0).label == object.classification.at(0).UNKNOWN && - traj_param.is_avoiding_unknown) || - (object.classification.at(0).label == object.classification.at(0).CAR && - traj_param.is_avoiding_car) || - (object.classification.at(0).label == object.classification.at(0).TRUCK && - traj_param.is_avoiding_truck) || - (object.classification.at(0).label == object.classification.at(0).BUS && - traj_param.is_avoiding_bus) || - (object.classification.at(0).label == object.classification.at(0).BICYCLE && - traj_param.is_avoiding_bicycle) || - (object.classification.at(0).label == object.classification.at(0).MOTORCYCLE && - traj_param.is_avoiding_motorbike) || - (object.classification.at(0).label == object.classification.at(0).PEDESTRIAN && - traj_param.is_avoiding_pedestrian)) { - return true; - } - return false; -} - -PolygonPoints getPolygonPoints( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - PolygonPoints polygon_points; - if (object.shape.type == object.shape.BOUNDING_BOX) { - polygon_points = getPolygonPointsFromBB(object, map_info); - } else if (object.shape.type == object.shape.CYLINDER) { - polygon_points = getPolygonPointsFromCircle(object, map_info); - } else if (object.shape.type == object.shape.POLYGON) { - polygon_points = getPolygonPointsFromPolygon(object, map_info); - } - return polygon_points; -} - -PolygonPoints getPolygonPointsFromBB( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - const double dim_x = object.shape.dimensions.x; - const double dim_y = object.shape.dimensions.y; - const std::vector rel_x = {0.5 * dim_x, 0.5 * dim_x, -0.5 * dim_x, -0.5 * dim_x}; - const std::vector rel_y = {0.5 * dim_y, -0.5 * dim_y, -0.5 * dim_y, 0.5 * dim_y}; - const geometry_msgs::msg::Pose object_pose = object.kinematics.initial_pose_with_covariance.pose; - for (std::size_t i = 0; i < rel_x.size(); i++) { - geometry_msgs::msg::Point rel_point; - rel_point.x = rel_x[i]; - rel_point.y = rel_y[i]; - auto abs_point = util::transformToAbsoluteCoordinate2D(rel_point, object_pose); - geometry_msgs::msg::Point image_point; - if (util::transformMapToImage(abs_point, map_info, image_point)) { - points_in_image.push_back(image_point); - points_in_map.push_back(abs_point); - } - } - PolygonPoints polygon_points; - polygon_points.points_in_image = points_in_image; - polygon_points.points_in_map = points_in_map; - return polygon_points; -} - -PolygonPoints getPolygonPointsFromCircle( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - const double radius = object.shape.dimensions.x; - const geometry_msgs::msg::Point center = - object.kinematics.initial_pose_with_covariance.pose.position; - constexpr int num_sampling_points = 5; - for (int i = 0; i < num_sampling_points; ++i) { - std::vector deltas = {0, 1.0}; - for (const auto & delta : deltas) { - geometry_msgs::msg::Point point; - point.x = std::cos( - ((i + delta) / static_cast(num_sampling_points)) * 2.0 * M_PI + - M_PI / static_cast(num_sampling_points)) * - (radius / 2.0) + - center.x; - point.y = std::sin( - ((i + delta) / static_cast(num_sampling_points)) * 2.0 * M_PI + - M_PI / static_cast(num_sampling_points)) * - (radius / 2.0) + - center.y; - point.z = center.z; - geometry_msgs::msg::Point image_point; - if (util::transformMapToImage(point, map_info, image_point)) { - points_in_image.push_back(image_point); - points_in_map.push_back(point); - } - } - } - PolygonPoints polygon_points; - polygon_points.points_in_image = points_in_image; - polygon_points.points_in_map = points_in_map; - return polygon_points; -} - -PolygonPoints getPolygonPointsFromPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - for (const auto & polygon_p : object.shape.footprint.points) { - geometry_msgs::msg::Point rel_point; - rel_point.x = polygon_p.x; - rel_point.y = polygon_p.y; - geometry_msgs::msg::Point point = util::transformToAbsoluteCoordinate2D( - rel_point, object.kinematics.initial_pose_with_covariance.pose); - const auto image_point = util::transformMapToOptionalImage(point, map_info); - if (image_point) { - points_in_image.push_back(image_point.get()); - points_in_map.push_back(point); - } - } - PolygonPoints polygon_points; - polygon_points.points_in_image = points_in_image; - polygon_points.points_in_map = points_in_map; - return polygon_points; -} - -std::vector getCVPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const PolygonPoints & polygon_points, - const std::vector & path_points, - const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info) -{ - const int nearest_idx = - util::getNearestIdx(path_points, object.kinematics.initial_pose_with_covariance.pose.position); - const auto nearest_path_point = path_points[nearest_idx]; - if (path_points.empty()) { - return getDefaultCVPolygon(polygon_points.points_in_image); - } - return getExtendedCVPolygon( - polygon_points.points_in_image, polygon_points.points_in_map, nearest_path_point.pose, object, - clearance_map, map_info); -} - -std::vector getDefaultCVPolygon( - const std::vector & points_in_image) -{ - std::vector cv_polygon; - for (const auto & point : points_in_image) { - cv::Point image_point = cv::Point(point.x, point.y); - cv_polygon.push_back(image_point); - } - return cv_polygon; -} - -std::vector getExtendedCVPolygon( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) -{ - const boost::optional optional_edges = getEdges( - points_in_image, points_in_map, nearest_path_point_pose, object, clearance_map, map_info); - if (!optional_edges) { - return getDefaultCVPolygon(points_in_image); - } - const Edges edges = optional_edges.get(); - - const int nearest_polygon_idx = util::getNearestIdx(points_in_image, edges.origin); - std::vector cv_polygon; - if (edges.back_idx == nearest_polygon_idx || edges.front_idx == nearest_polygon_idx) { - // make polygon only with edges and extended edges - } else if (edges.back_idx < nearest_polygon_idx) { - // back_idx -> nearest_idx -> frond_idx - if (edges.back_idx < edges.front_idx && nearest_polygon_idx < edges.front_idx) { - for (int i = edges.back_idx + 1; i < edges.front_idx; i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - // back_idx -> vector_front -> vector_back -> nearest_idx -> frond_idx - } else if (edges.back_idx < edges.front_idx && nearest_polygon_idx > edges.front_idx) { - for (int i = edges.back_idx - 1; i >= 0; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = points_in_image.size() - 1; i > edges.front_idx; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - // back_idx -> vector_back -> vector_front -> nearest_idx -> front_idx - } else { - for (std::size_t i = edges.back_idx + 1; i < points_in_image.size(); i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = 0; i < edges.front_idx; i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - } - } else { - // back_idx -> nearest_idx -> front_idx - if (edges.back_idx >= edges.front_idx && nearest_polygon_idx > edges.front_idx) { - for (int i = edges.back_idx - 1; i > edges.front_idx; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - // back_idx -> vector_back -> vector_front -> nearest_idx -> front_idx - } else { - if (edges.back_idx >= edges.front_idx && nearest_polygon_idx < edges.front_idx) { - for (std::size_t i = edges.back_idx + 1; i < points_in_image.size(); i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = 0; i < edges.front_idx; i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - } else { // back_idx -> vector_front -> vector_back -> nearest_idx -> front_idx - for (int i = edges.back_idx - 1; i >= 0; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = points_in_image.size() - 1; i > edges.front_idx; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - } - } - } - cv_polygon.push_back( - cv::Point(points_in_image[edges.front_idx].x, points_in_image[edges.front_idx].y)); - cv_polygon.push_back(cv::Point(edges.extended_front.x, edges.extended_front.y)); - cv_polygon.push_back(cv::Point(edges.extended_back.x, edges.extended_back.y)); - cv_polygon.push_back( - cv::Point(points_in_image[edges.back_idx].x, points_in_image[edges.back_idx].y)); - return cv_polygon; -} - -boost::optional getEdges( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) -{ - // calculate perpendicular point to object along with path point orientation - const double yaw = tf2::getYaw(nearest_path_point_pose.orientation); - const Eigen::Vector2d rel_path_vec(std::cos(yaw), std::sin(yaw)); - const Eigen::Vector2d obj_vec( - object.kinematics.initial_pose_with_covariance.pose.position.x, - object.kinematics.initial_pose_with_covariance.pose.position.y); - const double inner_product = rel_path_vec[0] * (obj_vec[0] - nearest_path_point_pose.position.x) + - rel_path_vec[1] * (obj_vec[1] - nearest_path_point_pose.position.y); - geometry_msgs::msg::Point origin; - origin.x = nearest_path_point_pose.position.x + rel_path_vec[0] * inner_product; - origin.y = nearest_path_point_pose.position.y + rel_path_vec[1] * inner_product; - const Eigen::Vector2d obj2origin(origin.x - obj_vec[0], origin.y - obj_vec[1]); - - // calculate origin for casting ray to edges - const auto path_point_image = - util::transformMapToImage(nearest_path_point_pose.position, map_info); - constexpr double ray_origin_dist_scale = 1.0; - const float clearance = clearance_map.ptr(static_cast( - path_point_image.y))[static_cast(path_point_image.x)] * - map_info.resolution * ray_origin_dist_scale; - const Eigen::Vector2d obj2ray_origin = obj2origin.normalized() * (obj2origin.norm() + clearance); - geometry_msgs::msg::Point ray_origin; - ray_origin.x = obj_vec[0] + obj2ray_origin[0]; - ray_origin.y = obj_vec[1] + obj2ray_origin[1]; - geometry_msgs::msg::Point ray_origin_image; - ray_origin_image = util::transformMapToImage(ray_origin, map_info); - - double min_cos = std::numeric_limits::max(); - double max_cos = std::numeric_limits::lowest(); - const double path_yaw = tf2::getYaw(nearest_path_point_pose.orientation); - const double dx1 = std::cos(path_yaw); - const double dy1 = std::sin(path_yaw); - const Eigen::Vector2d path_point_vec(dx1, dy1); - const double path_point_vec_norm = path_point_vec.norm(); - Edges edges; - for (std::size_t i = 0; i < points_in_image.size(); i++) { - const double dx2 = points_in_map[i].x - ray_origin.x; - const double dy2 = points_in_map[i].y - ray_origin.y; - const Eigen::Vector2d path_point2point(dx2, dy2); - const double inner_product = path_point_vec.dot(path_point2point); - const double cos = inner_product / (path_point_vec_norm * path_point2point.norm()); - if (cos > max_cos) { - max_cos = cos; - edges.front_idx = i; - } - if (cos < min_cos) { - min_cos = cos; - edges.back_idx = i; - } - } - - const double max_sin = std::sqrt(1 - max_cos * max_cos); - const double min_sin = std::sqrt(1 - min_cos * min_cos); - const Eigen::Vector2d point2front_edge( - points_in_image[edges.front_idx].x - ray_origin_image.x, - points_in_image[edges.front_idx].y - ray_origin_image.y); - const Eigen::Vector2d point2back_edge( - points_in_image[edges.back_idx].x - ray_origin_image.x, - points_in_image[edges.back_idx].y - ray_origin_image.y); - const Eigen::Vector2d point2extended_front_edge = - point2front_edge.normalized() * (clearance * 2 / max_sin) * (1 / map_info.resolution); - const Eigen::Vector2d point2extended_back_edge = - point2back_edge.normalized() * (clearance * 2 / min_sin) * (1 / map_info.resolution); - - const double dist2extended_front_edge = point2extended_front_edge.norm() * map_info.resolution; - const double dist2front_edge = point2front_edge.norm() * map_info.resolution; - const double dist2extended_back_edge = point2extended_back_edge.norm() * map_info.resolution; - const double dist2back_edge = point2back_edge.norm() * map_info.resolution; - if ( - dist2extended_front_edge < clearance * 2 || dist2extended_back_edge < clearance * 2 || - dist2front_edge > dist2extended_front_edge || dist2back_edge > dist2extended_back_edge) { - return boost::none; - } - geometry_msgs::msg::Point extended_front; - geometry_msgs::msg::Point extended_back; - extended_front.x = point2extended_front_edge(0) + ray_origin_image.x; - extended_front.y = point2extended_front_edge(1) + ray_origin_image.y; - extended_back.x = point2extended_back_edge(0) + ray_origin_image.x; - extended_back.y = point2extended_back_edge(1) + ray_origin_image.y; - edges.extended_front = extended_front; - edges.extended_back = extended_back; - edges.origin = ray_origin_image; - return edges; -} - -bool arePointsInsideDriveableArea( - const std::vector & image_points, const cv::Mat & clearance_map) -{ - bool points_inside_area = false; - for (const auto & image_point : image_points) { - const float clearance = - clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)]; - if (clearance > 0) { - points_inside_area = true; - } - } - return points_inside_area; -} - -cv::Mat getDrivableAreaInCV(const nav_msgs::msg::OccupancyGrid & occupancy_grid) -{ - cv::Mat drivable_area = cv::Mat(occupancy_grid.info.width, occupancy_grid.info.height, CV_8UC1); - - drivable_area.forEach([&](unsigned char & value, const int * position) -> void { - getOccupancyGridValue(occupancy_grid, position[0], position[1], value); - }); - return drivable_area; -} - -cv::Mat getClearanceMap(const cv::Mat & drivable_area) -{ - cv::Mat clearance_map; - cv::distanceTransform(drivable_area, clearance_map, cv::DIST_L2, 5); - return clearance_map; -} - -cv::Mat getAreaWithObjects(const cv::Mat & drivable_area, const cv::Mat & objects_image) -{ - cv::Mat area_with_objects = cv::min(objects_image, drivable_area); - return area_with_objects; -} - -boost::optional getStopIdx( - const std::vector & footprints, const geometry_msgs::msg::Pose & ego_pose, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info) -{ - const int nearest_idx = util::getNearestPointIdx(footprints, ego_pose.position); - for (std::size_t i = nearest_idx; i < footprints.size(); i++) { - const auto top_left = getDistance(road_clearance_map, footprints[i].top_left, map_info); - const auto top_right = getDistance(road_clearance_map, footprints[i].top_right, map_info); - const auto bottom_right = getDistance(road_clearance_map, footprints[i].bottom_right, map_info); - const auto bottom_left = getDistance(road_clearance_map, footprints[i].bottom_left, map_info); - const double epsilon = 1e-8; - if (!top_left || !top_right || !bottom_left || !bottom_right) { - continue; - } else if ( // NOLINT - top_left.get() < epsilon || top_right.get() < epsilon || bottom_left.get() < epsilon || - bottom_right.get() < epsilon) { - return std::max(static_cast(i - 1), 0); - } - } - return boost::none; -} - -boost::optional getDistance( - const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & map_info) -{ - const auto image_point = util::transformMapToOptionalImage(map_point, map_info); - if (!image_point) { - return boost::none; - } - const float clearance = clearance_map.ptr(static_cast( - image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - return clearance; -} - -CVMaps getMaps( - const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & objects, - const TrajectoryParam & traj_param, DebugData * debug_data) -{ - CVMaps cv_maps; - cv_maps.drivable_area = getDrivableAreaInCV(path.drivable_area); - cv_maps.clearance_map = getClearanceMap(cv_maps.drivable_area); - - std::vector debug_avoiding_objects; - cv::Mat objects_image = drawObstaclesOnImage( - enable_avoidance, objects, path.points, path.drivable_area.info, cv_maps.clearance_map, - traj_param, &debug_avoiding_objects); - cv_maps.area_with_objects_map = getAreaWithObjects(cv_maps.drivable_area, objects_image); - cv_maps.only_objects_clearance_map = getClearanceMap(objects_image); - cv_maps.map_info = path.drivable_area.info; - - debug_data->clearance_map = cv_maps.clearance_map; - debug_data->only_object_clearance_map = cv_maps.only_objects_clearance_map; - debug_data->area_with_objects_map = cv_maps.area_with_objects_map; - debug_data->avoiding_objects = debug_avoiding_objects; - return cv_maps; -} -} // namespace process_cv diff --git a/planning/obstacle_avoidance_planner/src/replan_checker.cpp b/planning/obstacle_avoidance_planner/src/replan_checker.cpp new file mode 100644 index 0000000000000..3d3c3970027ec --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/replan_checker.cpp @@ -0,0 +1,179 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_avoidance_planner/replan_checker.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +namespace +{ +[[maybe_unused]] std::vector resampleTrajectoryPoints( + const std::vector & traj_points, const double interval) +{ + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); + + // convert Trajectory to std::vector + std::vector resampled_traj_points; + for (const auto & point : resampled_traj.points) { + resampled_traj_points.push_back(point); + } + + return resampled_traj_points; +} +} // namespace + +ReplanChecker::ReplanChecker( + rclcpp::Node & node, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold) +{ + ego_nearest_dist_threshold_ = ego_nearest_dist_threshold; + ego_nearest_yaw_threshold_ = ego_nearest_yaw_threshold; + + max_path_shape_change_dist_ = node.declare_parameter("replan.max_path_shape_change_dist"); + max_ego_moving_dist_ = node.declare_parameter("replan.max_ego_moving_dist_for_replan"); + max_delta_time_sec_ = node.declare_parameter("replan.max_delta_time_sec_for_replan"); +} + +void ReplanChecker::onParam(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + updateParam(parameters, "replan.max_path_shape_change_dist", max_path_shape_change_dist_); + updateParam(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); + updateParam(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); +} + +bool ReplanChecker::isReplanRequired( + const PlannerData & planner_data, const rclcpp::Time & current_time, + const std::shared_ptr prev_mpt_trajs_ptr) +{ + reset_optimization_ = false; + const auto & p = planner_data; + + if ( + !prev_ego_pose_ptr_ || !prev_replanned_time_ptr_ || !prev_path_points_ptr_ || + !prev_mpt_trajs_ptr) { + return true; + } + + if (prev_mpt_trajs_ptr->mpt.empty()) { + RCLCPP_INFO( + rclcpp::get_logger("ReplanChecker"), + "Replan with resetting optimization since previous optimized trajectory is empty."); + reset_optimization_ = true; + return true; + } + + if (isPathShapeChanged(p)) { + RCLCPP_INFO( + rclcpp::get_logger("ReplanChecker"), + "Replan with resetting optimization since path shape was changed."); + reset_optimization_ = true; + return true; + } + + if (isPathGoalChanged(planner_data)) { + RCLCPP_INFO( + rclcpp::get_logger("ReplanChecker"), + "Replan with resetting optimization since path goal was changed."); + reset_optimization_ = true; + return true; + } + + // For when ego pose is lost or new ego pose is designated in simulation + const double delta_dist = + tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + if (delta_dist > max_ego_moving_dist_) { + RCLCPP_INFO( + rclcpp::get_logger("ReplanChecker"), + "Replan with resetting optimization since current ego pose is far from previous ego pose."); + reset_optimization_ = true; + return true; + } + + const double delta_time_sec = (current_time - *prev_replanned_time_ptr_).seconds(); + if (delta_time_sec > max_delta_time_sec_) { + return true; + } + + return false; +} + +bool ReplanChecker::isResetOptimizationRequired() { return reset_optimization_; } + +bool ReplanChecker::isPathShapeChanged(const PlannerData & planner_data) +{ + if (!prev_path_points_ptr_) { + return true; + } + + const auto & p = planner_data; + + const double max_path_length = 50.0; + + // truncate prev points from ego pose to fixed end points + const auto prev_begin_idx = findEgoNearestIndex(*prev_path_points_ptr_, p.ego_pose); + const auto truncated_prev_points = + points_utils::clipForwardPoints(*prev_path_points_ptr_, prev_begin_idx, max_path_length); + + // truncate points from ego pose to fixed end points + const auto begin_idx = findEgoNearestIndex(p.path.points, p.ego_pose); + const auto truncated_points = + points_utils::clipForwardPoints(p.path.points, begin_idx, max_path_length); + + // guard for lateral offset + if (truncated_prev_points.size() < 2 || truncated_points.size() < 2) { + return false; + } + + // calculate lateral deviations between truncated path_points and prev_path_points + for (const auto & prev_point : truncated_prev_points) { + const double dist = + std::abs(motion_utils::calcLateralOffset(truncated_points, prev_point.pose.position)); + if (dist > max_path_shape_change_dist_) { + return true; + } + } + + return false; +} + +bool ReplanChecker::isPathGoalChanged(const PlannerData & planner_data) +{ + const auto & p = planner_data; + + if (prev_path_points_ptr_) { + return true; + } + + constexpr double min_vel = 1e-3; + if (std::abs(p.ego_vel) > min_vel) { + return false; + } + + // NOTE: Path may be cropped and does not contain the goal. + // Therefore we set a large value to distance threshold. + constexpr double max_goal_moving_dist = 1.0; + const double goal_moving_dist = + tier4_autoware_utils::calcDistance2d(p.path.points.back(), prev_path_points_ptr_->back()); + if (goal_moving_dist < max_goal_moving_dist) { + return false; + } + + return true; +} diff --git a/planning/obstacle_avoidance_planner/src/cv_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/cv_utils.cpp similarity index 99% rename from planning/obstacle_avoidance_planner/src/cv_utils.cpp rename to planning/obstacle_avoidance_planner/src/utils/cv_utils.cpp index ab77f05dd9f7b..207d48cfd3105 100644 --- a/planning/obstacle_avoidance_planner/src/cv_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/cv_utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/cv_utils.hpp" +#include "obstacle_avoidance_planner/utils/cv_utils.hpp" -#include "obstacle_avoidance_planner/utils.hpp" +#include "obstacle_avoidance_planner/utils/utils.hpp" #include "tf2/utils.h" #include "tier4_autoware_utils/system/stop_watch.hpp" diff --git a/planning/obstacle_avoidance_planner/src/debug_visualization.cpp b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp similarity index 93% rename from planning/obstacle_avoidance_planner/src/debug_visualization.cpp rename to planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp index 1ddcb9978b8d8..9ebe66184c4ce 100644 --- a/planning/obstacle_avoidance_planner/src/debug_visualization.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/debug_visualization.hpp" +#include "obstacle_avoidance_planner/utils/debug_utils.hpp" -#include "obstacle_avoidance_planner/cv_utils.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/utils.hpp" +#include "obstacle_avoidance_planner/utils/cv_utils.hpp" +#include "obstacle_avoidance_planner/utils/utils.hpp" #include "tf2/utils.h" #include "visualization_msgs/msg/marker.hpp" @@ -713,10 +713,10 @@ visualization_msgs::msg::MarkerArray getVirtualWallTextMarkerArray( } } // namespace -namespace debug_visualization +namespace debug_utils { visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( - const std::shared_ptr debug_data_ptr, + DebugData & debug_data, const std::vector & optimized_points, const VehicleParam & vehicle_param, const bool is_showing_debug_detail) { @@ -724,88 +724,88 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( if (is_showing_debug_detail) { const auto points_marker_array = getDebugPointsMarkers( - debug_data_ptr->interpolated_points, optimized_points, debug_data_ptr->straight_points, - debug_data_ptr->fixed_points, debug_data_ptr->non_fixed_points); + debug_data.interpolated_points, optimized_points, debug_data.straight_points, + debug_data.fixed_points, debug_data.non_fixed_points); const auto constrain_marker_array = - getDebugConstrainMarkers(debug_data_ptr->constrain_rectangles, "constrain_rect"); + getDebugConstrainMarkers(debug_data.constrain_rectangles, "constrain_rect"); appendMarkerArray(points_marker_array, &vis_marker_array); appendMarkerArray(constrain_marker_array, &vis_marker_array); appendMarkerArray( getRectanglesNumMarkerArray( - debug_data_ptr->mpt_traj, vehicle_param, "num_vehicle_footprint", 0.99, 0.99, 0.2), + debug_data.mpt_traj, vehicle_param, "num_vehicle_footprint", 0.99, 0.99, 0.2), &vis_marker_array); appendMarkerArray( - getPointsTextMarkerArray(debug_data_ptr->eb_traj, "eb_traj_text", 0.99, 0.99, 0.2), + getPointsTextMarkerArray(debug_data.eb_traj, "eb_traj_text", 0.99, 0.99, 0.2), &vis_marker_array); } // avoiding objects appendMarkerArray( - getObjectsMarkerArray(debug_data_ptr->avoiding_objects, "avoiding_objects", 0.99, 0.99, 0.2), + getObjectsMarkerArray(debug_data.avoiding_objects, "avoiding_objects", 0.99, 0.99, 0.2), &vis_marker_array); // mpt footprints appendMarkerArray( getRectanglesMarkerArray( - debug_data_ptr->mpt_traj, vehicle_param, "mpt_footprints", 0.99, 0.99, 0.2, - debug_data_ptr->mpt_visualize_sampling_num), + debug_data.mpt_traj, vehicle_param, "mpt_footprints", 0.99, 0.99, 0.2, + debug_data.mpt_visualize_sampling_num), &vis_marker_array); // bounds appendMarkerArray( getBoundsLineMarkerArray( - debug_data_ptr->ref_points, 0.99, 0.99, 0.2, vehicle_param.width, - debug_data_ptr->mpt_visualize_sampling_num), + debug_data.ref_points, 0.99, 0.99, 0.2, vehicle_param.width, + debug_data.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, - vehicle_param.width, debug_data_ptr->mpt_visualize_sampling_num), + debug_data.ref_points, debug_data.sequential_bounds_candidates, 0.2, 0.99, 0.99, + vehicle_param.width, debug_data.mpt_visualize_sampling_num), &vis_marker_array); // vehicle circle line appendMarkerArray( getVehicleCircleLineMarkerArray( - debug_data_ptr->vehicle_circles_pose, vehicle_param.width, - debug_data_ptr->mpt_visualize_sampling_num, "vehicle_circle_lines", 0.99, 0.99, 0.2), + debug_data.vehicle_circles_pose, vehicle_param.width, debug_data.mpt_visualize_sampling_num, + "vehicle_circle_lines", 0.99, 0.99, 0.2), &vis_marker_array); // lateral error line appendMarkerArray( getLateralErrorsLineMarkerArray( - debug_data_ptr->mpt_ref_poses, debug_data_ptr->lateral_errors, - debug_data_ptr->mpt_visualize_sampling_num, "lateral_errors", 0.1, 0.1, 0.8), + debug_data.mpt_ref_poses, debug_data.lateral_errors, debug_data.mpt_visualize_sampling_num, + "lateral_errors", 0.1, 0.1, 0.8), &vis_marker_array); // current vehicle circles appendMarkerArray( getCurrentVehicleCirclesMarkerArray( - debug_data_ptr->current_ego_pose, debug_data_ptr->vehicle_circle_longitudinal_offsets, - debug_data_ptr->vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3), + debug_data.current_ego_pose, debug_data.vehicle_circle_longitudinal_offsets, + debug_data.vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3), &vis_marker_array); // vehicle circles appendMarkerArray( getVehicleCirclesMarkerArray( - debug_data_ptr->mpt_traj, debug_data_ptr->vehicle_circle_longitudinal_offsets, - debug_data_ptr->vehicle_circle_radiuses, debug_data_ptr->mpt_visualize_sampling_num, - "vehicle_circles", 1.0, 0.3, 0.3), + debug_data.mpt_traj, debug_data.vehicle_circle_longitudinal_offsets, + debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, "vehicle_circles", + 1.0, 0.3, 0.3), &vis_marker_array); return vis_marker_array; } visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( - const std::shared_ptr debug_data_ptr, const VehicleParam & vehicle_param) + DebugData & debug_data, const VehicleParam & vehicle_param) { visualization_msgs::msg::MarkerArray vis_marker_array; - if (debug_data_ptr->stop_pose_by_drivable_area) { + if (debug_data.stop_pose_by_drivable_area) { const auto virtual_wall_pose = - getVirtualWallPose(debug_data_ptr->stop_pose_by_drivable_area.get(), vehicle_param); + getVirtualWallPose(debug_data.stop_pose_by_drivable_area.get(), vehicle_param); appendMarkerArray( getVirtualWallMarkerArray(virtual_wall_pose, "virtual_wall", 1.0, 0, 0), &vis_marker_array); appendMarkerArray( @@ -829,4 +829,4 @@ nav_msgs::msg::OccupancyGrid getDebugCostmap( }); return clearance_map_in_og; } -} // namespace debug_visualization +} // namespace debug_utils diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp similarity index 99% rename from planning/obstacle_avoidance_planner/src/utils.cpp rename to planning/obstacle_avoidance_planner/src/utils/utils.cpp index f5f030dcd4ac8..cc3edd1c42223 100644 --- a/planning/obstacle_avoidance_planner/src/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/utils.hpp" +#include "obstacle_avoidance_planner/utils/utils.hpp" #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp"