diff --git a/.cspell-partial.json b/.cspell-partial.json index 209882ddc2f99..ae92ec76f9546 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -1,6 +1,5 @@ { "ignorePaths": [ - "**/common/**", "**/control/**", "**/evaluator/**", "**/launch/**", diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index ca03cd9a56adc..10b1309eac2ea 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -2,27 +2,28 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp -common/autoware_auto_common/** opensource@apex.ai +common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_auto_geometry/** satoshi.ota@tier4.jp common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp -common/autoware_auto_tf2/** jit.ray.c@gmail.com +common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_point_types/** taichi.higashide@tier4.jp -common/autoware_testing/** adam.dabrowski@robotec.ai +common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp common/component_interface_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp -common/fake_test_node/** opensource@apex.ai +common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp +common/perception_utils/** mingyu.li@tier4.jp satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yusuke.muramatsu@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @@ -45,8 +46,9 @@ common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp -common/time_utils/** christopherj.ho@gmail.com +common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp +common/traffic_light_utils/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp @@ -64,10 +66,9 @@ control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@ti control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp -evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai -evaluator/localization_evaluator/** dominik.jargot@robotec.ai +evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp -launch/map4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryohei.sasaki@map.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp launch/tier4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp @@ -116,11 +117,17 @@ perception/map_based_prediction/** shunsuke.miura@tier4.jp tomoya.kimura@tier4.j perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yukihiro.saito@tier4.jp +perception/object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/probabilistic_occupancy_grid_map/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +perception/radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +perception/radar_object_tracker/** satoshi.tanaka@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +perception/tensorrt_classifier/** mingyu.li@tier4.jp perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp @@ -128,17 +135,18 @@ perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@ti perception/traffic_light_fine_detector/** mingyu.li@tier4.jp perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp perception/traffic_light_multi_camera_fusion/** mingyu.li@tier4.jp +perception/traffic_light_occlusion_predictor/** mingyu.li@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_intersection_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai +planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @@ -146,6 +154,7 @@ planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakab planning/behavior_velocity_stop_line_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @@ -156,6 +165,7 @@ planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@ planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp +planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/planning_validator/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp @@ -180,6 +190,7 @@ sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com shunsuke.miura@tier4 sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +sensing/radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp @@ -201,7 +212,7 @@ system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp -tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai +tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp diff --git a/.github/workflows/openai-pr-reviewer.yaml b/.github/workflows/openai-pr-reviewer.yaml new file mode 100644 index 0000000000000..8445b0debb17e --- /dev/null +++ b/.github/workflows/openai-pr-reviewer.yaml @@ -0,0 +1,31 @@ +name: Code Review By ChatGPT + +permissions: + contents: read + pull-requests: write + +on: + pull_request: + pull_request_review_comment: + types: [created] + +concurrency: + group: ${{ github.repository }}-${{ github.event.number || github.head_ref || + github.sha }}-${{ github.workflow }}-${{ github.event_name == + 'pull_request_review_comment' && 'pr_comment' || 'pr' }} + cancel-in-progress: ${{ github.event_name != 'pull_request_review_comment' }} + +jobs: + review: + runs-on: ubuntu-latest + steps: + - uses: fluxninja/openai-pr-reviewer@latest + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + OPENAI_API_KEY: ${{ secrets.OPENAI_API_KEY }} + with: + debug: false + review_simple_changes: false + review_comment_lgtm: false + openai_light_model: gpt-3.5-turbo-0613 + openai_heavy_model: gpt-3.5-turbo-0613 # The default is to use GPT4, which needs to be changed to join ChatGPT Plus. diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt index 2437e8cacb386..e4a3683ea1fdc 100644 --- a/control/mpc_lateral_controller/CMakeLists.txt +++ b/control/mpc_lateral_controller/CMakeLists.txt @@ -17,7 +17,7 @@ ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED src/mpc_trajectory.cpp src/mpc_utils.cpp src/qp_solver/qp_solver_osqp.cpp - src/qp_solver/qp_solver_unconstr_fast.cpp + src/qp_solver/qp_solver_unconstraint_fast.cpp src/vehicle_model/vehicle_model_bicycle_dynamics.cpp src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp src/vehicle_model/vehicle_model_bicycle_kinematics.cpp diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp similarity index 92% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp index e9ef492c80c88..3a6bd2b25832b 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ #include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" @@ -55,4 +55,4 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ diff --git a/control/mpc_lateral_controller/model_predictive_control_algorithm.md b/control/mpc_lateral_controller/model_predictive_control_algorithm.md index 9dec0def2e477..4568fb9882817 100644 --- a/control/mpc_lateral_controller/model_predictive_control_algorithm.md +++ b/control/mpc_lateral_controller/model_predictive_control_algorithm.md @@ -344,7 +344,7 @@ $$ \end{align} $$ -Discretisizing $\dot{u}$ as $\left(u_{k} - u_{k-1}\right)/\text{d}t$ and multiply both sides by dt the resulting constraint become linear and convex +Discretizing $\dot{u}$ as $\left(u_{k} - u_{k-1}\right)/\text{d}t$ and multiply both sides by dt the resulting constraint become linear and convex $$ \begin{align} diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index c7fbce7411f1e..af680b5050037 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -16,7 +16,7 @@ #include "motion_utils/motion_utils.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp similarity index 94% rename from control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp rename to control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp index 3c54ba6ffdc48..b0357138cd646 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index 72066003696d8..6066cd3419ef3 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -15,7 +15,7 @@ #include "gtest/gtest.h" #include "mpc_lateral_controller/mpc.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp index 717c1ecac6c77..99b1a82ab2dce 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp @@ -47,7 +47,7 @@ namespace pure_pursuit class PurePursuit { public: - PurePursuit() : lookahead_distance_(0.0), clst_thr_dist_(3.0), clst_thr_ang_(M_PI / 4) {} + PurePursuit() : lookahead_distance_(0.0), closest_thr_dist_(3.0), closest_thr_ang_(M_PI / 4) {} ~PurePursuit() = default; rclcpp::Logger logger = rclcpp::get_logger("pure_pursuit"); @@ -55,10 +55,10 @@ class PurePursuit void setCurrentPose(const geometry_msgs::msg::Pose & msg); void setWaypoints(const std::vector & msg); void setLookaheadDistance(double ld) { lookahead_distance_ = ld; } - void setClosestThreshold(double clst_thr_dist, double clst_thr_ang) + void setClosestThreshold(double closest_thr_dist, double closest_thr_ang) { - clst_thr_dist_ = clst_thr_dist; - clst_thr_ang_ = clst_thr_ang; + closest_thr_dist_ = closest_thr_dist; + closest_thr_ang_ = closest_thr_ang; } // getter @@ -74,7 +74,7 @@ class PurePursuit geometry_msgs::msg::Point loc_next_tgt_; // variables got from outside - double lookahead_distance_, clst_thr_dist_, clst_thr_ang_; + double lookahead_distance_, closest_thr_dist_, closest_thr_ang_; std::shared_ptr> curr_wps_ptr_; std::shared_ptr curr_pose_ptr_; diff --git a/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp b/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp index 0b186a0c4514b..0ee96e970782c 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp +++ b/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp @@ -55,16 +55,17 @@ std::pair PurePursuit::run() return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - auto clst_pair = planning_utils::findClosestIdxWithDistAngThr( - *curr_wps_ptr_, *curr_pose_ptr_, clst_thr_dist_, clst_thr_ang_); + auto closest_pair = planning_utils::findClosestIdxWithDistAngThr( + *curr_wps_ptr_, *curr_pose_ptr_, closest_thr_dist_, closest_thr_ang_); - if (!clst_pair.first) { + if (!closest_pair.first) { RCLCPP_WARN( - logger, "cannot find, curr_bool: %d, clst_idx: %d", clst_pair.first, clst_pair.second); + logger, "cannot find, curr_bool: %d, closest_idx: %d", closest_pair.first, + closest_pair.second); return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - int32_t next_wp_idx = findNextPointIdx(clst_pair.second); + int32_t next_wp_idx = findNextPointIdx(closest_pair.second); if (next_wp_idx == -1) { RCLCPP_WARN(logger, "lost next waypoint"); return std::make_pair(false, std::numeric_limits::quiet_NaN()); diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml index ebee784510435..447219085df7f 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml @@ -21,33 +21,37 @@ - - - - + - - - - - + + + + + - - - - - - - - - + + + + + - - - - - - + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 4dae46cbacb0a..1f86a89ddfc7a 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -9,6 +9,7 @@ + @@ -27,7 +28,7 @@ - + @@ -77,7 +78,7 @@ - + @@ -130,7 +131,7 @@ - + diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp index 5253879a28937..79454e89b9ed0 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp @@ -39,7 +39,7 @@ class MapModule rclcpp::CallbackGroup::SharedPtr map_callback_group); private: - void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); + void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr); rclcpp::Subscription::SharedPtr map_points_sub_; std::shared_ptr ndt_ptr_; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 61034e02e9c91..4f630bb8c5898 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -63,7 +63,7 @@ class MapUpdateModule const std::vector & maps_to_add, const std::vector & map_ids_to_remove); void update_map(const geometry_msgs::msg::Point & position); - bool should_update_map(const geometry_msgs::msg::Point & position) const; + [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position) const; void publish_partial_pcd_map(); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp index e88862781225d..f1e7dfb3f544b 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp @@ -30,18 +30,18 @@ class PoseArrayInterpolator public: PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array, const double & pose_timeout_sec, const double & pose_distance_tolerance_meters); PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array); PoseWithCovarianceStamped get_current_pose(); PoseWithCovarianceStamped get_old_pose(); PoseWithCovarianceStamped get_new_pose(); - bool is_success(); + [[nodiscard]] bool is_success() const; private: rclcpp::Logger logger_; @@ -51,10 +51,10 @@ class PoseArrayInterpolator PoseWithCovarianceStamped::SharedPtr new_pose_ptr_; bool success_; - bool validate_time_stamp_difference( + [[nodiscard]] bool validate_time_stamp_difference( const rclcpp::Time & target_time, const rclcpp::Time & reference_time, const double time_tolerance_sec) const; - bool validate_position_difference( + [[nodiscard]] bool validate_position_difference( const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; }; diff --git a/localization/ndt_scan_matcher/src/debug.cpp b/localization/ndt_scan_matcher/src/debug.cpp index cec62c850372a..9c82e06d89b80 100644 --- a/localization/ndt_scan_matcher/src/debug.cpp +++ b/localization/ndt_scan_matcher/src/debug.cpp @@ -30,7 +30,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers( marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; marker.scale = scale; - marker.id = i; + marker.id = static_cast(i); marker.lifetime = rclcpp::Duration::from_seconds(10.0); // 10.0 is the lifetime in seconds. marker.ns = "initial_pose_transform_probability_color_marker"; @@ -45,7 +45,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers( marker.ns = "initial_pose_index_color_marker"; marker.pose = particle.initial_pose; - marker.color = exchange_color_crc((1.0 * i) / 100); + marker.color = exchange_color_crc(static_cast(i) / 100.0); marker_array.markers.push_back(marker); marker.ns = "result_pose_transform_probability_color_marker"; @@ -60,7 +60,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers( marker.ns = "result_pose_index_color_marker"; marker.pose = particle.result_pose; - marker.color = exchange_color_crc((1.0 * i) / 100); + marker.color = exchange_color_crc(static_cast(i) / 100.0); marker_array.markers.push_back(marker); return marker_array; diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp index e036f8f81d482..d6088a1b14949 100644 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ b/localization/ndt_scan_matcher/src/map_module.cpp @@ -18,7 +18,7 @@ MapModule::MapModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, std::shared_ptr ndt_ptr, rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex) +: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex) { auto map_sub_opt = rclcpp::SubscriptionOptions(); map_sub_opt.callback_group = map_callback_group; diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 26c2fd30d9198..9c94467dc94c0 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -28,13 +28,13 @@ MapUpdateModule::MapUpdateModule( std::shared_ptr tf2_listener_module, std::string map_frame, rclcpp::CallbackGroup::SharedPtr main_callback_group, std::shared_ptr> state_ptr) -: ndt_ptr_(ndt_ptr), +: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex), - map_frame_(map_frame), + map_frame_(std::move(map_frame)), logger_(node->get_logger()), clock_(node->get_clock()), - tf2_listener_module_(tf2_listener_module), - state_ptr_(state_ptr), + tf2_listener_module_(std::move(tf2_listener_module)), + state_ptr_(std::move(state_ptr)), dynamic_map_loading_update_distance_( node->declare_parameter("dynamic_map_loading_update_distance")), dynamic_map_loading_map_radius_( @@ -112,9 +112,9 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) { auto request = std::make_shared(); - request->area.center_x = position.x; - request->area.center_y = position.y; - request->area.radius = dynamic_map_loading_map_radius_; + request->area.center_x = static_cast(position.x); + request->area.center_y = static_cast(position.y); + request->area.radius = static_cast(dynamic_map_loading_map_radius_); request->cached_ids = ndt_ptr_->getCurrentMapIDs(); // // send a request to map_loader @@ -163,9 +163,9 @@ void MapUpdateModule::update_ndt( backup_ndt.createVoxelKdtree(); const auto exe_end_time = std::chrono::system_clock::now(); - const double exe_time = - std::chrono::duration_cast(exe_end_time - exe_start_time).count() / - 1000.0; + const auto duration_micro_sec = + std::chrono::duration_cast(exe_end_time - exe_start_time).count(); + const auto exe_time = static_cast(duration_micro_sec) / 1000.0; RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); // swap diff --git a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp index d7635c052142f..59ecedc18cb5a 100644 --- a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp +++ b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp @@ -15,7 +15,7 @@ #include "ndt_scan_matcher/pose_array_interpolator.hpp" PoseArrayInterpolator::PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array) : logger_(node->get_logger()), clock_(*node->get_clock()), @@ -33,7 +33,7 @@ PoseArrayInterpolator::PoseArrayInterpolator( } PoseArrayInterpolator::PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array, const double & pose_timeout_sec, const double & pose_distance_tolerance_meters) : PoseArrayInterpolator(node, target_ros_time, pose_msg_ptr_array) @@ -50,7 +50,7 @@ PoseArrayInterpolator::PoseArrayInterpolator( pose_distance_tolerance_meters); // all validations must be true - if (!(is_old_pose_valid & is_new_pose_valid & is_pose_diff_valid)) { + if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { RCLCPP_WARN(logger_, "Validation error."); } } @@ -70,7 +70,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_new_pos return *new_pose_ptr_; } -bool PoseArrayInterpolator::is_success() +bool PoseArrayInterpolator::is_success() const { return success_; } diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/ndt_scan_matcher/src/util_func.cpp index 2db6b51540d51..d947c56dfe183 100644 --- a/localization/ndt_scan_matcher/src/util_func.cpp +++ b/localization/ndt_scan_matcher/src/util_func.cpp @@ -28,19 +28,19 @@ std_msgs::msg::ColorRGBA exchange_color_crc(double x) if (x <= 0.25) { color.b = 1.0; - color.g = std::sin(x * 2.0 * M_PI); + color.g = static_cast(std::sin(x * 2.0 * M_PI)); color.r = 0; } else if (x > 0.25 && x <= 0.5) { - color.b = std::sin(x * 2 * M_PI); + color.b = static_cast(std::sin(x * 2 * M_PI)); color.g = 1.0; color.r = 0; } else if (x > 0.5 && x <= 0.75) { color.b = 0; color.g = 1.0; - color.r = -std::sin(x * 2.0 * M_PI); + color.r = static_cast(-std::sin(x * 2.0 * M_PI)); } else { color.b = 0; - color.g = -std::sin(x * 2.0 * M_PI); + color.g = static_cast(-std::sin(x * 2.0 * M_PI)); color.r = 1.0; } color.a = 0.999; @@ -58,9 +58,9 @@ double calc_diff_for_radian(const double lhs_rad, const double rhs_rad) return diff_rad; } -Eigen::Map makeEigenCovariance(const std::array & covariance) +Eigen::Map make_eigen_covariance(const std::array & covariance) { - return Eigen::Map(covariance.data(), 6, 6); + return {covariance.data(), 6, 6}; } // x: roll, y: pitch, z: yaw @@ -242,7 +242,7 @@ std::vector create_random_pose_array( { std::default_random_engine engine(seed_gen()); const Eigen::Map covariance = - makeEigenCovariance(base_pose_with_cov.pose.covariance); + make_eigen_covariance(base_pose_with_cov.pose.covariance); std::normal_distribution<> x_distribution(0.0, std::sqrt(covariance(0, 0))); std::normal_distribution<> y_distribution(0.0, std::sqrt(covariance(1, 1))); diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 5b7713d7c4afd..43e8d48c4fda0 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -16,7 +16,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml index bf9a0abc08f8e..150ffed58138d 100644 --- a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml @@ -1,9 +1,9 @@ - + - - + + diff --git a/localization/yabloc/yabloc_particle_filter/CMakeLists.txt b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt index 5f56186077d48..1284c52690795 100644 --- a/localization/yabloc/yabloc_particle_filter/CMakeLists.txt +++ b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt @@ -49,7 +49,6 @@ ament_auto_add_library( ) target_include_directories(abstract_corrector SYSTEM PRIVATE ${PCL_INCLUDE_DIRS}) target_link_libraries(abstract_corrector Sophus::Sophus ${PCL_LIBRARIES}) -rosidl_target_interfaces(abstract_corrector ${PROJECT_NAME} "rosidl_typesupport_cpp") # Prediction library ament_auto_add_library(predictor @@ -60,7 +59,16 @@ ament_auto_add_library(predictor ) target_include_directories(predictor SYSTEM PRIVATE ${PCL_INCLUDE_DIRS}) target_link_libraries(predictor Sophus::Sophus ${PCL_LIBRARIES}) -rosidl_target_interfaces(predictor ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# ros2idl typesupport +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(abstract_corrector ${PROJECT_NAME} "rosidl_typesupport_cpp") + rosidl_target_interfaces(predictor ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(abstract_corrector "${cpp_typesupport_target}") + target_link_libraries(predictor "${cpp_typesupport_target}") +endif() # Cost map Library ament_auto_add_library(ll2_cost_map diff --git a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml index 60bb501fd1ae7..f38264b828d34 100644 --- a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml +++ b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml @@ -9,7 +9,7 @@ - + @@ -19,9 +19,9 @@ - - - + + + @@ -47,7 +47,7 @@ - + diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt index a2aaba44b6626..cd54a3c5f2ef2 100644 --- a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -44,9 +44,16 @@ ament_auto_add_executable(${TARGET} src/camera/camera_pose_initializer_node.cpp) target_include_directories(${TARGET} PUBLIC include) target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -rosidl_target_interfaces(${TARGET} ${PROJECT_NAME} "rosidl_typesupport_cpp") target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) +# ros2idl typesupport +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(${TARGET} ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${TARGET} "${cpp_typesupport_target}") +endif() + # Semantic segmentation install(PROGRAMS src/semantic_segmentation/semantic_segmentation_core.py diff --git a/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml index 1f40fb619b0b7..a1d5d59b46094 100644 --- a/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml +++ b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml @@ -1,17 +1,17 @@ - - + + - - + + - + diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index f76af5060316c..9ce34d8482fbd 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -166,8 +166,8 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ if (sub_std_pair_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); timer_->cancel(); - publish(*(sub_std_pair_.second)); postprocess(*(sub_std_pair_.second)); + publish(*(sub_std_pair_.second)); sub_std_pair_.second = nullptr; std::fill(is_fused_.begin(), is_fused_.end(), false); @@ -260,8 +260,8 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ // Msg if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { timer_->cancel(); - publish(*output_msg); postprocess(*output_msg); + publish(*output_msg); std::fill(is_fused_.begin(), is_fused_.end(), false); sub_std_pair_.second = nullptr; @@ -322,8 +322,8 @@ void FusionNode::roiCallback( if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { timer_->cancel(); - publish(*(sub_std_pair_.second)); postprocess(*(sub_std_pair_.second)); + publish(*(sub_std_pair_.second)); std::fill(is_fused_.begin(), is_fused_.end(), false); sub_std_pair_.second = nullptr; @@ -362,8 +362,8 @@ void FusionNode::timer_callback() if (sub_std_pair_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); - publish(*(sub_std_pair_.second)); postprocess(*(sub_std_pair_.second)); + publish(*(sub_std_pair_.second)); // add processing time for debug if (debug_publisher_) { diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 3641b42db461a..01ec679a82efc 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -180,6 +180,12 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + sensor_msgs::msg::PointCloud2 tmp; tmp = painted_pointcloud_msg; @@ -232,6 +238,12 @@ void PointPaintingFusionNode::fuseOnSingleImage( const sensor_msgs::msg::CameraInfo & camera_info, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + auto num_bbox = (input_roi_msg.feature_objects).size(); if (num_bbox == 0) { return; @@ -342,6 +354,12 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte return; } + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + std::vector det_boxes3d; bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d); if (!is_success) { diff --git a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp index afb5984c288fc..485ce84c5fea6 100644 --- a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp +++ b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp @@ -22,6 +22,7 @@ #include +#include #include class TrafficLightArbiter : public rclcpp::Node @@ -45,7 +46,7 @@ class TrafficLightArbiter : public rclcpp::Node void onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg); void arbitrateAndPublish(const builtin_interfaces::msg::Time & stamp); - std::unordered_set map_regulatory_elements_set_; + std::unique_ptr> map_regulatory_elements_set_; double external_time_tolerance_; double perception_time_tolerance_; diff --git a/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml b/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml index 42944450378d6..f8c7b8a78fac6 100644 --- a/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml +++ b/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml @@ -1,11 +1,15 @@ + + + + - - - + + + diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp index eb940fc18c32d..dfa01878bb81f 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -72,9 +72,10 @@ void TrafficLightArbiter::onMap(const LaneletMapBin::ConstSharedPtr msg) lanelet::utils::conversion::fromBinMsg(*msg, map); const auto signals = lanelet::filter_traffic_signals(map); - map_regulatory_elements_set_.clear(); + map_regulatory_elements_set_ = std::make_unique>(); + for (const auto & signal : signals) { - map_regulatory_elements_set_.emplace(signal->id()); + map_regulatory_elements_set_->emplace(signal->id()); } } @@ -109,14 +110,22 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim using ElementAndPriority = std::pair; std::unordered_map> regulatory_element_signals_map; - if (map_regulatory_elements_set_.empty()) { + if (map_regulatory_elements_set_ == nullptr) { RCLCPP_WARN(get_logger(), "Received traffic signal messages before a map"); return; } + TrafficSignalArray output_signals_msg; + output_signals_msg.stamp = stamp; + + if (map_regulatory_elements_set_->empty()) { + pub_->publish(output_signals_msg); + return; + } + auto add_signal_function = [&](const auto & signal, bool priority) { const auto id = signal.traffic_signal_id; - if (!map_regulatory_elements_set_.count(id)) { + if (!map_regulatory_elements_set_->count(id)) { RCLCPP_WARN( get_logger(), "Received a traffic signal not present in the current map (%lu)", id); return; @@ -165,8 +174,6 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim return highest_score_elements_vector; }; - TrafficSignalArray output_signals_msg; - output_signals_msg.stamp = stamp; output_signals_msg.signals.reserve(regulatory_element_signals_map.size()); for (const auto & [regulatory_element_id, elements] : regulatory_element_signals_map) { diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index 36f09be943144..366820a725018 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -117,7 +117,7 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( if ( in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr || in_roi_msg->rois.size() != in_signal_msg->signals.size()) { - occlusion_ratios.resize(in_roi_msg->rois.size(), 0); + occlusion_ratios.resize(out_msg.signals.size(), 0); } else { cloud_occlusion_predictor_->predict( in_cam_info_msg, in_roi_msg, in_cloud_msg, tf_buffer_, traffic_light_position_map_, diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 3a36fcea62e41..3b9165194a3bf 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -13,7 +13,6 @@ enable_bound_clipping: false enable_update_path_when_object_is_gone: false enable_force_avoidance_for_stopped_vehicle: false - enable_safety_check: true enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false disable_path_update: false @@ -133,10 +132,18 @@ # For safety check safety_check: + # safety check configuration + enable: true # [-] + check_current_lane: false # [-] + check_shift_side_lane: true # [-] + check_other_side_lane: false # [-] + check_unavoidable_object: false # [-] + check_other_object: true # [-] + # collision check parameters + check_all_predicted_path: false # [-] + time_resolution: 0.5 # [s] + time_horizon: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] - safety_check_time_horizon: 10.0 # [s] - safety_check_idling_time: 1.5 # [s] - safety_check_accel_for_rss: 2.5 # [m/ss] safety_check_hysteresis_factor: 2.0 # [-] safety_check_ego_offset: 1.0 # [m] @@ -186,12 +193,6 @@ max_jerk: 1.0 # [m/sss] max_acceleration: 1.0 # [m/ss] - target_velocity_matrix: - col_size: 2 - matrix: - [2.78, 13.9, # velocity [m/s] - 0.50, 1.00] # margin [m] - shift_line_pipeline: trim: quantize_filter_threshold: 0.2 diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 845f9c38e269b..336879185f48e 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -1,6 +1,9 @@ /**: ros__parameters: dynamic_avoidance: + common: + enable_debug_info: true + # avoidance is performed for the object type with true target_object: car: true @@ -19,6 +22,10 @@ min_obj_lat_offset_to_ego_path: 0.0 # [m] max_obj_lat_offset_to_ego_path: 1.0 # [m] + cut_in_object: + min_time_to_start_cut_in: 1.0 # [s] + min_lon_offset_ego_to_object: 0.0 # [m] + crossing_object: min_object_vel: 1.0 max_object_angle: 1.05 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 7436c26533000..9be99f705f560 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -485,15 +485,22 @@ The hatched road marking is defined on Lanelet map. See [here](https://github.co ### Safety check -The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable_safety_check` as `true`. +The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable` as `true`. ```yaml -enable_safety_check: false - -# For safety check +# safety check configuration +enable: true # [-] +check_current_lane: false # [-] +check_shift_side_lane: true # [-] +check_other_side_lane: false # [-] +check_unavoidable_object: false # [-] +check_other_object: true # [-] + +# collision check parameters +check_all_predicted_path: false # [-] +time_horizon: 10.0 # [s] +idling_time: 1.5 # [s] safety_check_backward_distance: 50.0 # [m] -safety_check_time_horizon: 10.0 # [s] -safety_check_idling_time: 1.5 # [s] safety_check_accel_for_rss: 2.5 # [m/ss] ``` @@ -621,12 +628,9 @@ namespace: `avoidance.` | detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | | detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | | enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | -| enable_safety_check | [-] | bool | Flag to enable safety check. | false | | enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | | enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | -**NOTE:** It has to set both `enable_safety_check` and `enable_yield_maneuver` to enable yield maneuver. - | Name | Unit | Type | Description | Default value | | :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- | | enable_bound_clipping | [-] | bool | Enable clipping left and right bound of drivable area when obstacles are in the drivable area | false | @@ -695,14 +699,20 @@ namespace: `avoidance.target_filtering.` namespace: `avoidance.safety_check.` -| Name | Unit | Type | Description | Default value | -| :----------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | -| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 | -| safety_check_time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 | -| safety_check_idling_time | [t] | double | Time delay constant that be use for longitudinal margin calculation based on RSS. | 1.5 | -| safety_check_accel_for_rss | [m/ss] | double | Accel constant that be used for longitudinal margin calculation based on RSS. | 2.5 | -| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 | -| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 | +| Name | Unit | Type | Description | Default value | +| :----------------------------- | ---- | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | +| enable | [-] | bool | Enable to use safety check feature. | true | +| check_current_lane | [-] | bool | Check objects on current driving lane. | false | +| check_shift_side_lane | [-] | bool | Check objects on shift side lane. | true | +| check_other_side_lane | [-] | bool | Check objects on other side lane. | false | +| check_unavoidable_object | [-] | bool | Check collision between ego and unavoidable objects. | false | +| check_other_object | [-] | bool | Check collision between ego and non avoidance target objects. | false | +| check_all_predicted_path | [-] | bool | Check all prediction path of safety check target objects. | false | +| time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 | +| time_resolution | [s] | double | Time resolution to check lateral/longitudinal margin is enough or not. | 0.5 | +| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 | +| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 | +| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 | ### Avoidance maneuver parameters diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index ee81915ea7944..e4349c68cc0f5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/helper.hpp" +#include "behavior_path_planner/utils/safety_check.hpp" #include @@ -51,7 +52,6 @@ class AvoidanceModule : public SceneModuleInterface const std::unordered_map > & rtc_interface_ptr_map); ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; @@ -256,12 +256,6 @@ class AvoidanceModule : public SceneModuleInterface ObjectData createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const; - /** - * @brief get objects that are driving on adjacent lanes. - * @param left or right lanelets. - */ - ObjectDataArray getAdjacentLaneObjects(const lanelet::ConstLanelets & adjacent_lanes) const; - /** * @brief fill additional data so that the module judges target objects. * @param avoidance data. @@ -481,65 +475,13 @@ class AvoidanceModule : public SceneModuleInterface // safety check - /** - * @brief get shift side adjacent lanes. - * @param path shifter. - * @param forward distance to get. - * @param backward distance to get. - * @return adjacent lanes. - */ - lanelet::ConstLanelets getAdjacentLane( - const PathShifter & path_shifter, const double forward_distance, - const double backward_distance) const; - - /** - * @brief calculate lateral margin from avoidance velocity for safety check. - * @param target velocity. - */ - double getLateralMarginFromVelocity(const double velocity) const; - - /** - * @brief calculate rss longitudinal distance for safety check. - * @param ego velocity. - * @param object velocity. - * @param option for rss calculation. - * @return rss longitudinal distance. - */ - double getRSSLongitudinalDistance( - const double v_ego, const double v_obj, const bool is_front_object) const; - - /** - * @brief check avoidance path safety for surround moving objects. - * @param path shifter. - * @param avoidance path. - * @param debug data. - * @return result. - */ - bool isSafePath( - const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const; - /** * @brief check avoidance path safety for surround moving objects. * @param avoidance path. - * @param shift side lanes. * @param debug data. * @return result. */ - bool isSafePath( - const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, - DebugData & debug) const; - - /** - * @brief check predicted points safety. - * @param predicted points. - * @param future time. - * @param object data. - * @param margin data for debug. - * @return result. - */ - bool isEnoughMargin( - const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, - MarginData & margin_data) const; + bool isSafePath(ShiftedPath & shifted_path, DebugData & debug) const; // post process diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index d43e1d08050e7..f8830212a8222 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -37,6 +37,9 @@ namespace behavior_path_planner { struct DynamicAvoidanceParameters { + // common + bool enable_debug_info{true}; + // obstacle types to avoid bool avoid_car{true}; bool avoid_truck{true}; @@ -52,6 +55,8 @@ struct DynamicAvoidanceParameters double min_obj_lat_offset_to_ego_path{0.0}; double max_obj_lat_offset_to_ego_path{0.0}; + double min_time_to_start_cut_in{0.0}; + double min_lon_offset_ego_to_cut_in_object{0.0}; double max_front_object_angle{0.0}; double min_crossing_object_vel{0.0}; double max_crossing_object_angle{0.0}; @@ -132,7 +137,6 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool isExecutionRequested() const override; bool isExecutionReady() const override; ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } BehaviorModuleOutput plan() override; CandidateOutput planCandidate() const override; BehaviorModuleOutput planWaitingApproval() override; @@ -143,11 +147,20 @@ class DynamicAvoidanceModule : public SceneModuleInterface } private: + struct LatLonOffset + { + const size_t nearest_idx; + const double max_lat_offset; + const double min_lat_offset; + const double max_lon_offset; + const double min_lon_offset; + }; + bool isLabelTargetObstacle(const uint8_t label) const; std::vector calcTargetObjectsCandidate(); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel) const; + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; bool willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_left) const; bool isObjectFarFromPath( @@ -157,6 +170,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface const double obj_tangent_vel) const; std::optional> calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const; + LatLonOffset getLateralLongitudinalOffset( + const std::vector & ego_path, const PredictedObject & object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 178ae5bc79e2b..abad4b9f2959f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -101,7 +101,6 @@ class GoalPlannerModule : public SceneModuleInterface bool isExecutionRequested() const override; bool isExecutionReady() const override; ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; void processOnEntry() override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 98dd98c707a23..371169c86db4e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -100,12 +100,6 @@ class SceneModuleInterface */ virtual void updateCurrentState() { current_state_ = updateState(); } - /** - * @brief If the module plan customized reference path while waiting approval, it should output - * SUCCESS. Otherwise, it should output FAILURE to check execution request of next module. - */ - virtual ModuleStatus getNodeStatusWhileWaitingApproval() const { return ModuleStatus::FAILURE; } - /** * @brief Return true if the module has request for execution (not necessarily feasible) */ @@ -135,8 +129,8 @@ class SceneModuleInterface // for new architecture const auto lanes = utils::getLaneletsFromPath(*out.path, planner_data_->route_handler); const auto drivable_lanes = utils::generateDrivableLanes(lanes); - out.drivable_area_info.drivable_lanes = - getNonOverlappingExpandedLanes(*out.path, drivable_lanes); + out.drivable_area_info.drivable_lanes = utils::getNonOverlappingExpandedLanes( + *out.path, drivable_lanes, planner_data_->drivable_area_expansion_parameters); return out; } @@ -381,21 +375,6 @@ class SceneModuleInterface BehaviorModuleOutput previous_module_output_; protected: - // TODO(murooka) Remove this function when BT-based architecture will be removed - std::unordered_map> createRTCInterfaceMap( - rclcpp::Node & node, const std::string & name, const std::vector & rtc_types) - { - std::unordered_map> rtc_interface_ptr_map; - for (const auto & rtc_type : rtc_types) { - const auto snake_case_name = utils::convertToSnakeCase(name); - const auto rtc_interface_name = - rtc_type.empty() ? snake_case_name : (snake_case_name + "_" + rtc_type); - rtc_interface_ptr_map.emplace( - rtc_type, std::make_shared(&node, rtc_interface_name)); - } - return rtc_interface_ptr_map; - } - virtual void updateRTCStatus(const double start_distance, const double finish_distance) { for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { @@ -460,18 +439,6 @@ class SceneModuleInterface return std::abs(planner_data_->self_odometry->twist.twist.linear.x); } - std::vector getNonOverlappingExpandedLanes( - PathWithLaneId & path, const std::vector & lanes) const - { - const auto & dp = planner_data_->drivable_area_expansion_parameters; - - const auto shorten_lanes = utils::cutOverlappedLanes(path, lanes); - const auto expanded_lanes = utils::expandLanelets( - shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); - return expanded_lanes; - } - bool is_simultaneously_executable_as_approved_module_{false}; bool is_simultaneously_executable_as_candidate_module_{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index 8118d05ef225d..bc6f55008fa93 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -51,7 +51,6 @@ class SideShiftModule : public SceneModuleInterface const double & min_request_time_sec, bool override_requests = false) const noexcept; ModuleStatus updateState() override; void updateData() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 205b3c6d4db46..e17834326965d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -81,7 +81,6 @@ class StartPlannerModule : public SceneModuleInterface bool isExecutionRequested() const override; bool isExecutionReady() const override; ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; @@ -152,8 +151,8 @@ class StartPlannerModule : public SceneModuleInterface // check if the goal is located behind the ego in the same route segment. bool IsGoalBehindOfEgoInSameRouteSegment() const; - // generate BehaviorPathOutput with stopping path. - BehaviorModuleOutput generateStopOutput() const; + // generate BehaviorPathOutput with stopping path and update status + BehaviorModuleOutput generateStopOutput(); void setDebugData() const; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 3c01ad20bcebd..b590d452ae03e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include @@ -46,6 +47,8 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; +using marker_utils::CollisionCheckDebug; + struct ObjectParameter { bool is_target{false}; @@ -95,9 +98,6 @@ struct AvoidanceParameters // enable avoidance for all parking vehicle bool enable_force_avoidance_for_stopped_vehicle{false}; - // enable safety check. if avoidance path is NOT safe, the ego will execute yield maneuver - bool enable_safety_check{false}; - // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -177,20 +177,23 @@ struct AvoidanceParameters // for longitudinal direction double longitudinal_collision_margin_time; - // find adjacent lane vehicles - double safety_check_backward_distance; - - // minimum longitudinal margin for vehicles in adjacent lane - double safety_check_min_longitudinal_margin; + // parameters for safety check area + bool enable_safety_check{false}; + bool check_current_lane{false}; + bool check_shift_side_lane{false}; + bool check_other_side_lane{false}; - // safety check time horizon - double safety_check_time_horizon; + // parameters for safety check target. + bool check_unavoidable_object{false}; + bool check_other_object{false}; - // use in RSS calculation - double safety_check_idling_time; + // parameters for collision check. + bool check_all_predicted_path{false}; + double safety_check_time_horizon{0.0}; + double safety_check_time_resolution{0.0}; - // use in RSS calculation - double safety_check_accel_for_rss; + // find adjacent lane vehicles + double safety_check_backward_distance; // transit hysteresis (unsafe to safe) double safety_check_hysteresis_factor; @@ -280,12 +283,6 @@ struct AvoidanceParameters // Maximum lateral acceleration limitation map. std::vector lateral_max_accel_map; - // target velocity matrix - std::vector target_velocity_matrix; - - // matrix col size - size_t col_size; - // parameters depend on object class std::unordered_map object_parameters; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 69803e4cc4dec..8c229b4f26949 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -17,7 +17,9 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/utils/safety_check.hpp" +#include #include #include #include @@ -25,6 +27,10 @@ namespace behavior_path_planner::utils::avoidance { using behavior_path_planner::PlannerData; +using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; +using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; +using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; bool isOnRight(const ObjectData & obj); @@ -79,11 +85,19 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); +std::vector convertToPredictedPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); bool isCentroidWithinLanelets( const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); +lanelet::ConstLanelets getAdjacentLane( + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift); + lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset); @@ -139,6 +153,13 @@ AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine AvoidLineArray combineRawShiftLinesWithUniqueCheck( const AvoidLineArray & base_lines, const AvoidLineArray & added_lines); + +ExtendedPredictedObject transform( + const PredictedObject & object, const std::shared_ptr & parameters); + +std::vector getSafetyCheckTargetObjects( + const AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index b95d247b6d23f..3a2087083b427 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -39,6 +39,7 @@ enum class PlannerType { NONE = 0, SHIFT = 1, GEOMETRIC = 2, + STOP = 3, }; class PullOutPlannerBase diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index ea217c8f7d0d4..07ec7c5db6605 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -70,6 +70,7 @@ using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; +using drivable_area_expansion::DrivableAreaExpansionParameters; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; @@ -229,6 +230,9 @@ boost::optional getLeftLanelet( std::vector generateDrivableLanes(const lanelet::ConstLanelets & current_lanes); std::vector generateDrivableLanesWithShoulderLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); +std::vector getNonOverlappingExpandedLanes( + PathWithLaneId & path, const std::vector & lanes, + const DrivableAreaExpansionParameters & parameters); std::vector calcBound( const std::shared_ptr route_handler, const std::vector & drivable_lanes, diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 34977b0933019..06ebd6e01b48b 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -44,6 +44,7 @@ namespace behavior_path_planner { +using marker_utils::CollisionCheckDebug; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -465,7 +466,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * Check avoidance path safety. For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ - data.safe = isSafePath(path_shifter, data.candidate_path, debug); + data.safe = isSafePath(data.candidate_path, debug); } void AvoidanceModule::fillEgoStatus( @@ -1832,371 +1833,55 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) } bool AvoidanceModule::isSafePath( - const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const + ShiftedPath & shifted_path, [[maybe_unused]] DebugData & debug) const { - const auto & p = parameters_; + const auto & p = planner_data_->parameters; - if (!p->enable_safety_check) { + if (!parameters_->enable_safety_check) { return true; // if safety check is disabled, it always return safe. } - const auto & forward_check_distance = p->object_check_forward_distance; - const auto & backward_check_distance = p->safety_check_backward_distance; - const auto check_lanes = - getAdjacentLane(path_shifter, forward_check_distance, backward_check_distance); - - auto path_with_current_velocity = shifted_path.path; - - const size_t ego_idx = planner_data_->findEgoIndex(path_with_current_velocity.points); - utils::clipPathLength(path_with_current_velocity, ego_idx, forward_check_distance, 0.0); - - constexpr double MIN_EGO_VEL_IN_PREDICTION = 1.38; // 5km/h - for (auto & p : path_with_current_velocity.points) { - p.point.longitudinal_velocity_mps = std::max(getEgoSpeed(), MIN_EGO_VEL_IN_PREDICTION); - } - - { - debug_data_.path_with_planned_velocity = path_with_current_velocity; - } - - return isSafePath(path_with_current_velocity, check_lanes, debug); -} - -bool AvoidanceModule::isSafePath( - const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, DebugData & debug) const -{ - if (path.points.empty()) { - return true; - } - - const auto path_with_time = [&path]() { - std::vector> ret{}; - - float travel_time = 0.0; - ret.emplace_back(path.points.front(), travel_time); + const auto ego_predicted_path = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, parameters_); - for (size_t i = 1; i < path.points.size(); ++i) { - const auto & p1 = path.points.at(i - 1); - const auto & p2 = path.points.at(i); + const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); + const auto is_right_shift = [&]() -> std::optional { + for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { + const auto length = shifted_path.shift_length.at(i); - const auto v = std::max(p1.point.longitudinal_velocity_mps, float{1.0}); - const auto ds = calcDistance2d(p1, p2); - - travel_time += ds / v; - - ret.emplace_back(p2, travel_time); - } - - return ret; - }(); - - const auto move_objects = getAdjacentLaneObjects(check_lanes); - - { - debug.unsafe_objects.clear(); - debug.margin_data_array.clear(); - debug.exist_adjacent_objects = !move_objects.empty(); - } - - bool is_safe = true; - for (const auto & p : path_with_time) { - MarginData margin_data{}; - margin_data.pose = getPose(p.first); - - if (p.second > parameters_->safety_check_time_horizon) { - break; - } - - for (const auto & o : move_objects) { - const auto is_enough_margin = isEnoughMargin(p.first, p.second, o, margin_data); - - if (!is_enough_margin) { - debug.unsafe_objects.push_back(o); + if (parameters_->lateral_avoid_check_threshold < length) { + return false; } - is_safe = is_safe && is_enough_margin; - } - - debug.margin_data_array.push_back(margin_data); - } - - return is_safe; -} - -bool AvoidanceModule::isEnoughMargin( - const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, - MarginData & margin_data) const -{ - const auto & common_param = planner_data_->parameters; - const auto & vehicle_width = common_param.vehicle_width; - const auto & base_link2front = common_param.base_link2front; - const auto & base_link2rear = common_param.base_link2rear; - - const auto p_ref = [this, &p_ego]() { - const auto idx = findNearestIndex(avoidance_data_.reference_path.points, getPoint(p_ego)); - return getPose(avoidance_data_.reference_path.points.at(idx)); - }(); - - const auto & v_ego = p_ego.point.longitudinal_velocity_mps; - const auto & v_ego_lon = utils::avoidance::getLongitudinalVelocity(p_ref, getPose(p_ego), v_ego); - const auto & v_obj = object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - - if (!utils::avoidance::isTargetObjectType(object.object, parameters_)) { - return true; - } - - // | centerline - // | ^ x - // | +-------+ | - // | | | | - // | | | D1 | D2 D4 - // | | obj |<-->|<---------->|<->| - // | | | D3 | +-------+ - // | | |<----------->| | - // | +-------+ | | | - // | | | ego | - // | | | | - // | | | | - // | | +-------+ - // | y <----+ - // D1: overhang_dist (signed value) - // D2: shift_length (signed value) - // D3: lateral_distance (should be larger than margin that's calculated from relative velocity.) - // D4: vehicle_width (unsigned value) - - const auto reliable_path = std::max_element( - object.object.kinematics.predicted_paths.begin(), - object.object.kinematics.predicted_paths.end(), - [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); - - if (reliable_path == object.object.kinematics.predicted_paths.end()) { - return true; - } - - const auto p_obj = [&t, &reliable_path]() { - boost::optional ret{boost::none}; - - const auto dt = rclcpp::Duration(reliable_path->time_step).seconds(); - const auto idx = static_cast(std::floor(t / dt)); - const auto res = t - dt * idx; - - if (idx > reliable_path->path.size() - 2) { - return ret; + if (parameters_->lateral_avoid_check_threshold < -1.0 * length) { + return true; + } } - const auto & p_src = reliable_path->path.at(idx); - const auto & p_dst = reliable_path->path.at(idx + 1); - ret = calcInterpolatedPose(p_src, p_dst, res / dt); - return ret; + return std::nullopt; }(); - if (!p_obj) { - return true; - } - - const auto v_obj_lon = utils::avoidance::getLongitudinalVelocity(p_ref, p_obj.get(), v_obj); - - double hysteresis_factor = 1.0; - if (avoidance_data_.state == AvoidanceState::YIELD) { - hysteresis_factor = parameters_->safety_check_hysteresis_factor; - } - - const auto shift_length = calcLateralDeviation(p_ref, getPoint(p_ego)); - const auto lateral_distance = std::abs(object.overhang_dist - shift_length) - 0.5 * vehicle_width; - const auto lateral_margin = getLateralMarginFromVelocity(std::abs(v_ego_lon - v_obj_lon)); - - if (lateral_distance > lateral_margin * hysteresis_factor) { - return true; - } - - const auto lon_deviation = calcLongitudinalDeviation(getPose(p_ego), p_obj.get().position); - const auto is_front_object = lon_deviation > 0.0; - const auto longitudinal_margin = - getRSSLongitudinalDistance(v_ego_lon, v_obj_lon, is_front_object); - const auto vehicle_offset = is_front_object ? base_link2front : base_link2rear; - const auto longitudinal_distance = - std::abs(lon_deviation) - vehicle_offset - 0.5 * object.object.shape.dimensions.x; - - { - margin_data.pose.orientation = p_ref.orientation; - margin_data.enough_lateral_margin = false; - margin_data.longitudinal_distance = - std::min(margin_data.longitudinal_distance, longitudinal_distance); - margin_data.longitudinal_margin = - std::max(margin_data.longitudinal_margin, longitudinal_margin); - margin_data.vehicle_width = vehicle_width; - margin_data.base_link2front = base_link2front; - margin_data.base_link2rear = base_link2rear; - } - - if (longitudinal_distance > longitudinal_margin * hysteresis_factor) { + if (!is_right_shift.has_value()) { return true; } - return false; -} - -double AvoidanceModule::getLateralMarginFromVelocity(const double velocity) const -{ - const auto & p = parameters_; - - if (p->col_size < 2 || p->col_size * 2 != p->target_velocity_matrix.size()) { - throw std::logic_error("invalid matrix col size."); - } - - if (velocity < p->target_velocity_matrix.front()) { - return p->target_velocity_matrix.at(p->col_size); - } - - if (velocity > p->target_velocity_matrix.at(p->col_size - 1)) { - return p->target_velocity_matrix.back(); - } - - for (size_t i = 1; i < p->col_size; ++i) { - if (velocity < p->target_velocity_matrix.at(i)) { - const auto v1 = p->target_velocity_matrix.at(i - 1); - const auto v2 = p->target_velocity_matrix.at(i); - const auto m1 = p->target_velocity_matrix.at(i - 1 + p->col_size); - const auto m2 = p->target_velocity_matrix.at(i + p->col_size); - - const auto v_clamp = std::clamp(velocity, v1, v2); - return m1 + (m2 - m1) * (v_clamp - v1) / (v2 - v1); - } - } - - return p->target_velocity_matrix.back(); -} + const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( + avoidance_data_, planner_data_, parameters_, is_right_shift.value()); -double AvoidanceModule::getRSSLongitudinalDistance( - const double v_ego, const double v_obj, const bool is_front_object) const -{ - const auto & accel_for_rss = parameters_->safety_check_accel_for_rss; - const auto & idling_time = parameters_->safety_check_idling_time; - - const auto opposite_lane_vehicle = v_obj < 0.0; - - /** - * object and ego already pass each other. - * ======================================= - * Ego--> - * --------------------------------------- - * <--Obj - * ======================================= - */ - if (!is_front_object && opposite_lane_vehicle) { - return 0.0; - } - - /** - * object drive opposite direction. - * ======================================= - * Ego--> - * --------------------------------------- - * <--Obj - * ======================================= - */ - if (is_front_object && opposite_lane_vehicle) { - return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) + - std::abs(v_obj) * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss); - } - - /** - * object is in front of ego, and drive same direction. - * ======================================= - * Ego--> - * --------------------------------------- - * Obj--> - * ======================================= - */ - if (is_front_object && !opposite_lane_vehicle) { - return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - - std::pow(v_obj, 2.0) / (2.0 * accel_for_rss); - } - - /** - * object is behind ego, and drive same direction. - * ======================================= - * Ego--> - * --------------------------------------- - * Obj--> - * ======================================= - */ - if (!is_front_object && !opposite_lane_vehicle) { - return v_obj * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - - std::pow(v_ego, 2.0) / (2.0 * accel_for_rss); - } - - return 0.0; -} - -lanelet::ConstLanelets AvoidanceModule::getAdjacentLane( - const PathShifter & path_shifter, const double forward_distance, - const double backward_distance) const -{ - const auto & rh = planner_data_->route_handler; - - bool has_left_shift = false; - bool has_right_shift = false; - - for (const auto & sp : path_shifter.getShiftLines()) { - if (sp.end_shift_length > 0.01) { - has_left_shift = true; - continue; - } - - if (sp.end_shift_length < -0.01) { - has_right_shift = true; - continue; - } - } - - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), - "failed to find closest lanelet within route!!!"); - return {}; // TODO(Satoshi Ota) - } - - const auto ego_succeeding_lanes = - rh->getLaneletSequence(current_lane, getEgoPose(), backward_distance, forward_distance); - - lanelet::ConstLanelets check_lanes{}; - for (const auto & lane : ego_succeeding_lanes) { - const auto opt_left_lane = rh->getLeftLanelet(lane); - if (has_left_shift && opt_left_lane) { - check_lanes.push_back(opt_left_lane.get()); - } - - const auto opt_right_lane = rh->getRightLanelet(lane); - if (has_right_shift && opt_right_lane) { - check_lanes.push_back(opt_right_lane.get()); - } - - const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); - if (has_right_shift && !right_opposite_lanes.empty()) { - check_lanes.push_back(right_opposite_lanes.front()); - } - } - - return check_lanes; -} - -ObjectDataArray AvoidanceModule::getAdjacentLaneObjects( - const lanelet::ConstLanelets & adjacent_lanes) const -{ - ObjectDataArray objects; - for (const auto & o : avoidance_data_.other_objects) { - if (utils::avoidance::isCentroidWithinLanelets(o.object, adjacent_lanes)) { - objects.push_back(o); + for (const auto & object : safety_check_target_objects) { + const auto obj_predicted_paths = + utils::getPredictedPathFromObj(object, parameters_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::safety_check::checkCollision( + shifted_path.path, ego_predicted_path, object, obj_path, p, + p.expected_front_deceleration, p.expected_rear_deceleration, collision)) { + return false; + } } } - return objects; + return true; } void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 1ba7c588c84d6..29b3535331547 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -59,7 +59,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( get_parameter(node, ns + "enable_update_path_when_object_is_gone"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); - p.enable_safety_check = get_parameter(node, ns + "enable_safety_check"); p.enable_yield_maneuver = get_parameter(node, ns + "enable_yield_maneuver"); p.enable_yield_maneuver_during_shifting = get_parameter(node, ns + "enable_yield_maneuver_during_shifting"); @@ -140,11 +139,17 @@ AvoidanceModuleManager::AvoidanceModuleManager( // safety check { std::string ns = "avoidance.safety_check."; + p.enable_safety_check = get_parameter(node, ns + "enable"); + p.check_current_lane = get_parameter(node, ns + "check_current_lane"); + p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); + p.check_other_side_lane = get_parameter(node, ns + "check_other_side_lane"); + p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); + p.check_other_object = get_parameter(node, ns + "check_other_object"); + p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); + p.safety_check_time_horizon = get_parameter(node, ns + "time_horizon"); + p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); p.safety_check_backward_distance = get_parameter(node, ns + "safety_check_backward_distance"); - p.safety_check_time_horizon = get_parameter(node, ns + "safety_check_time_horizon"); - p.safety_check_idling_time = get_parameter(node, ns + "safety_check_idling_time"); - p.safety_check_accel_for_rss = get_parameter(node, ns + "safety_check_accel_for_rss"); p.safety_check_hysteresis_factor = get_parameter(node, ns + "safety_check_hysteresis_factor"); p.safety_check_ego_offset = get_parameter(node, ns + "safety_check_ego_offset"); @@ -227,13 +232,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( } } - // velocity matrix - { - std::string ns = "avoidance.target_velocity_matrix."; - p.col_size = get_parameter(node, ns + "col_size"); - p.target_velocity_matrix = get_parameter>(node, ns + "matrix"); - } - // shift line pipeline { std::string ns = "avoidance.shift_line_pipeline."; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index e4f121bb9d62d..b1aa372c42ac5 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -364,6 +364,10 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const bool is_crossing_object_to_ignore = parameters_->min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path; if (is_crossing_object_to_ignore) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since it crosses the ego's path.", + obj_uuid.c_str()); continue; } @@ -376,29 +380,59 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() std::abs(obj_angle) < parameters_->max_front_object_angle || M_PI - parameters_->max_front_object_angle < std::abs(obj_angle); if (is_object_on_ego_path && is_object_aligned_to_path) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since it is to be followed.", obj_uuid.c_str()); continue; } // 5. check if object lateral offset to ego's path is large enough const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path); if (is_object_far_from_path) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since lateral offset is large.", obj_uuid.c_str()); continue; } // 6. calculate which side object exists against ego's path - const bool is_left = isLeft(prev_module_path->points, obj_pose.position); + // TODO(murooka) use obj_path.back() instead of obj_pose for the case where the object crosses + // the ego's path + const bool is_left = isLeft(prev_module_path->points, obj_path.path.back().position); + const auto lat_lon_offset = + getLateralLongitudinalOffset(prev_module_path->points, predicted_object); // 6. check if object will not cut in or cut out const bool will_object_cut_in = - willObjectCutIn(prev_module_path->points, obj_path, obj_tangent_vel); + willObjectCutIn(prev_module_path->points, obj_path, obj_tangent_vel, lat_lon_offset); const bool will_object_cut_out = willObjectCutOut(obj_tangent_vel, obj_normal_vel, is_left); - if (will_object_cut_in || will_object_cut_out) { + if (will_object_cut_in) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since it will cut in.", obj_uuid.c_str()); + continue; + } + if (will_object_cut_out) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since it will cut out.", obj_uuid.c_str()); continue; } // 7. check if time to collision const double time_to_collision = calcTimeToCollision(prev_module_path->points, obj_pose, obj_tangent_vel); + if ( + (0 <= obj_tangent_vel && + parameters_->max_time_to_collision_overtaking_object < time_to_collision) || + (obj_tangent_vel <= 0 && + parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision is large.", + obj_uuid.c_str()); + continue; + } // 8. calculate alive counter for filtering objects const auto prev_target_object_candidate = @@ -484,7 +518,7 @@ bool DynamicAvoidanceModule::isObjectFarFromPath( bool DynamicAvoidanceModule::willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel) const + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const { constexpr double epsilon_path_lat_diff = 0.3; @@ -493,6 +527,20 @@ bool DynamicAvoidanceModule::willObjectCutIn( return false; } + // Ignore object close to the ego + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); + const double relative_velocity = getEgoSpeed() - obj_tangent_vel; + const double lon_offset_ego_to_obj = + motion_utils::calcSignedArcLength( + ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) + + lat_lon_offset.min_lon_offset; + if ( + lon_offset_ego_to_obj < std::max( + parameters_->min_lon_offset_ego_to_cut_in_object, + relative_velocity * parameters_->min_time_to_start_cut_in)) { + return false; + } + for (const auto & predicted_path_point : predicted_path.path) { const double paths_lat_diff = motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); @@ -564,6 +612,41 @@ std::pair DynamicAvoidanceModule return std::make_pair(right_lanes, left_lanes); } +DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudinalOffset( + const std::vector & ego_path, const PredictedObject & object) const +{ + const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; + + const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); + const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, object.shape); + + // TODO(murooka) calculation is not so accurate. + std::vector obj_lat_offset_vec; + std::vector obj_lon_offset_vec; + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const size_t obj_point_seg_idx = + motion_utils::findNearestSegmentIndex(ego_path, geom_obj_point); + + // calculate lateral offset + const double obj_point_lat_offset = + motion_utils::calcLateralOffset(ego_path, geom_obj_point, obj_point_seg_idx); + obj_lat_offset_vec.push_back(obj_point_lat_offset); + + // calculate longitudinal offset + const double lon_offset = + motion_utils::calcLongitudinalOffsetToSegment(ego_path, obj_seg_idx, geom_obj_point); + obj_lon_offset_vec.push_back(lon_offset); + } + + const auto obj_lat_min_max_offset = getMinMaxValues(obj_lat_offset_vec); + const auto obj_lon_min_max_offset = getMinMaxValues(obj_lon_offset_vec); + + return LatLonOffset{ + obj_seg_idx, obj_lat_min_max_offset.second, obj_lat_min_max_offset.first, + obj_lon_min_max_offset.second, obj_lon_min_max_offset.first}; +} + // NOTE: object does not have const only to update min_bound_lat_offset. std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const @@ -625,18 +708,9 @@ std::optional DynamicAvoidanceModule::calcDynam return std::nullopt; } - if (0 <= object.vel) { - const double limited_time_to_collision = - std::min(parameters_->max_time_to_collision_overtaking_object, object.time_to_collision); - return std::make_pair( - raw_min_obj_lon_offset + object.vel * limited_time_to_collision, - raw_max_obj_lon_offset + object.vel * limited_time_to_collision); - } - - const double limited_time_to_collision = - std::min(parameters_->max_time_to_collision_oncoming_object, object.time_to_collision); return std::make_pair( - raw_min_obj_lon_offset + object.vel * limited_time_to_collision, raw_max_obj_lon_offset); + raw_min_obj_lon_offset + object.vel * object.time_to_collision, + raw_max_obj_lon_offset + object.vel * object.time_to_collision); }(); if (!obj_lon_offset) { @@ -647,6 +721,7 @@ std::optional DynamicAvoidanceModule::calcDynam // calculate bound start and end index const bool is_object_overtaking = (0.0 <= object.vel); + // TODO(murooka) use getEgoSpeed() instead of object.vel const double start_length_to_avoid = std::abs(object.vel) * (is_object_overtaking ? parameters_->start_duration_to_avoid_overtaking_object diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index eddb93094574d..27b467b3c9a74 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -29,6 +29,11 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( { DynamicAvoidanceParameters p{}; + { // common + std::string ns = "dynamic_avoidance.common."; + p.enable_debug_info = node->declare_parameter(ns + "enable_debug_info"); + } + { // target object std::string ns = "dynamic_avoidance.target_object."; p.avoid_car = node->declare_parameter(ns + "car"); @@ -48,6 +53,11 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.max_obj_lat_offset_to_ego_path = node->declare_parameter(ns + "max_obj_lat_offset_to_ego_path"); + p.min_time_to_start_cut_in = + node->declare_parameter(ns + "cut_in_object.min_time_to_start_cut_in"); + p.min_lon_offset_ego_to_cut_in_object = + node->declare_parameter(ns + "cut_in_object.min_lon_offset_ego_to_object"); + p.max_front_object_angle = node->declare_parameter(ns + "front_object.max_object_angle"); @@ -88,6 +98,11 @@ void DynamicAvoidanceModuleManager::updateModuleParams( using tier4_autoware_utils::updateParam; auto & p = parameters_; + { // common + const std::string ns = "dynamic_avoidance.common."; + updateParam(parameters, ns + "enable_debug_info", p->enable_debug_info); + } + { // target object const std::string ns = "dynamic_avoidance.target_object."; @@ -111,6 +126,12 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "max_obj_lat_offset_to_ego_path", p->max_obj_lat_offset_to_ego_path); + updateParam( + parameters, ns + "cut_in_object.min_time_to_start_cut_in", p->min_time_to_start_cut_in); + updateParam( + parameters, ns + "cut_in_object.min_lon_offset_ego_to_object", + p->min_lon_offset_ego_to_cut_in_object); + updateParam( parameters, ns + "front_object.max_object_angle", p->max_front_object_angle); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f0aec7ba62d2f..6915b2be16819 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -664,7 +664,8 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*output.path, status_.lanes); + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *output.path, status_.lanes, planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -854,7 +855,8 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*out.path, status_.lanes); + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *out.path, status_.lanes, planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -1108,8 +1110,15 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const const double distance_to_end = calcSignedArcLength(full_path.points, current_pose.position, end_pose.position); const bool is_before_end_pose = distance_to_end >= 0.0; - turn_signal.turn_signal.command = - is_before_end_pose ? TurnIndicatorsCommand::ENABLE_LEFT : TurnIndicatorsCommand::NO_COMMAND; + if (is_before_end_pose) { + if (left_side_parking_) { + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else { + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + } else { + turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + } } // calc desired/required start/end point diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index d43d73318c570..bc3ce480476ee 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -166,7 +166,9 @@ BehaviorModuleOutput StartPlannerModule::plan() if (IsGoalBehindOfEgoInSameRouteSegment()) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - return generateStopOutput(); + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() + return output; } if (isWaitingApproval()) { @@ -181,18 +183,8 @@ BehaviorModuleOutput StartPlannerModule::plan() if (!status_.is_safe) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); - // the path of getCurrent() is generated by generateStopPath() - const PathWithLaneId stop_path = getCurrentPath(); - output.path = std::make_shared(stop_path); - - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = status_.lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - - output.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = std::make_shared(stop_path); - path_reference_ = getPreviousModuleOutput().reference_path; + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() return output; } @@ -207,7 +199,8 @@ BehaviorModuleOutput StartPlannerModule::plan() path = status_.backward_path; } - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + path, status_.lanes, planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -312,18 +305,20 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() if (IsGoalBehindOfEgoInSameRouteSegment()) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - path_reference_ = getPreviousModuleOutput().reference_path; clearWaitingApproval(); - return generateStopOutput(); + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() + return output; } BehaviorModuleOutput output; if (!status_.is_safe) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); - path_reference_ = getPreviousModuleOutput().reference_path; clearWaitingApproval(); - return generateStopOutput(); + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() + return output; } waitApproval(); @@ -535,7 +530,8 @@ PathWithLaneId StartPlannerModule::generateStopPath() const path.points.push_back(toPathPointWithLaneId(moved_pose)); // generate drivable area - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + path, status_.lanes, planner_data_->drivable_area_expansion_parameters); return path; } @@ -578,16 +574,6 @@ void StartPlannerModule::updatePullOutStatus() std::vector start_pose_candidates = searchPullOutStartPoses(); planWithPriority(start_pose_candidates, goal_pose, parameters_->search_priority); - if (!status_.is_safe) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull out path, generate stop path"); - status_.back_finished = true; // no need to drive backward - status_.pull_out_path.partial_paths.clear(); - status_.pull_out_path.partial_paths.push_back(generateStopPath()); - status_.pull_out_path.start_pose = current_pose; - status_.pull_out_path.end_pose = current_pose; - } - checkBackFinished(); if (!status_.back_finished) { status_.backward_path = start_planner_utils::getBackwardPath( @@ -881,10 +867,11 @@ bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const } // NOTE: this must be called after updatePullOutStatus(). This must be fixed. -BehaviorModuleOutput StartPlannerModule::generateStopOutput() const +BehaviorModuleOutput StartPlannerModule::generateStopOutput() { BehaviorModuleOutput output; - output.path = std::make_shared(generateStopPath()); + const PathWithLaneId stop_path = generateStopPath(); + output.path = std::make_shared(stop_path); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = status_.lanes; @@ -892,6 +879,19 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() const current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.reference_path = getPreviousModuleOutput().reference_path; + + status_.back_finished = true; + status_.planner_type = PlannerType::STOP; + status_.pull_out_path.partial_paths.clear(); + status_.pull_out_path.partial_paths.push_back(stop_path); + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + status_.pull_out_start_pose = current_pose; + status_.pull_out_path.start_pose = current_pose; + status_.pull_out_path.end_pose = current_pose; + + path_candidate_ = std::make_shared(stop_path); + path_reference_ = getPreviousModuleOutput().reference_path; + return output; } diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index d3c31d688557d..58beaf9cf3a87 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1367,4 +1367,181 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( return combined; } + +std::vector convertToPredictedPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (path.points.empty()) { + return {}; + } + + const auto & acceleration = parameters->max_acceleration; + const auto & vehicle_pose = planner_data->self_odometry->pose.pose; + const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); + const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_resolution = parameters->safety_check_time_resolution; + + const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); + std::vector predicted_path; + const auto vehicle_pose_frenet = + convertToFrenetPoint(path.points, vehicle_pose.position, ego_seg_idx); + + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const double velocity = + std::max(initial_velocity + acceleration * t, parameters->min_slow_down_speed); + const double length = initial_velocity * t + 0.5 * acceleration * t * t; + const auto pose = + motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + return predicted_path; +} + +ExtendedPredictedObject transform( + const PredictedObject & object, const std::shared_ptr & parameters) +{ + ExtendedPredictedObject extended_object; + extended_object.uuid = object.object_id; + extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; + extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; + extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; + extended_object.shape = object.shape; + + const auto & obj_velocity = extended_object.initial_twist.twist.linear.x; + const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_resolution = parameters->safety_check_time_resolution; + + extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); + for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { + const auto & path = object.kinematics.predicted_paths.at(i); + extended_object.predicted_paths.at(i).confidence = path.confidence; + + // create path + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + if (obj_pose) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); + extended_object.predicted_paths.at(i).path.emplace_back( + t, *obj_pose, obj_velocity, obj_polygon); + } + } + } + + return extended_object; +} + +lanelet::ConstLanelets getAdjacentLane( + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift) +{ + const auto & rh = planner_data->route_handler; + const auto & forward_distance = parameters->object_check_forward_distance; + const auto & backward_distance = parameters->safety_check_backward_distance; + const auto & vehicle_pose = planner_data->self_odometry->pose.pose; + + lanelet::ConstLanelet current_lane; + if (!rh->getClosestLaneletWithinRoute(vehicle_pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "failed to find closest lanelet within route!!!"); + return {}; // TODO(Satoshi Ota) + } + + const auto ego_succeeding_lanes = + rh->getLaneletSequence(current_lane, vehicle_pose, backward_distance, forward_distance); + + lanelet::ConstLanelets lanes{}; + for (const auto & lane : ego_succeeding_lanes) { + const auto opt_left_lane = rh->getLeftLanelet(lane); + if (!is_right_shift && opt_left_lane) { + lanes.push_back(opt_left_lane.get()); + } + + const auto opt_right_lane = rh->getRightLanelet(lane); + if (is_right_shift && opt_right_lane) { + lanes.push_back(opt_right_lane.get()); + } + + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); + if (is_right_shift && !right_opposite_lanes.empty()) { + lanes.push_back(right_opposite_lanes.front()); + } + } + + return lanes; +} + +std::vector getSafetyCheckTargetObjects( + const AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift) +{ + const auto & p = parameters; + const auto check_right_lanes = + (is_right_shift && p->check_shift_side_lane) || (!is_right_shift && p->check_other_side_lane); + const auto check_left_lanes = + (!is_right_shift && p->check_shift_side_lane) || (is_right_shift && p->check_other_side_lane); + + std::vector target_objects; + + const auto append_target_objects = [&](const auto & check_lanes, const auto & objects) { + std::for_each(objects.begin(), objects.end(), [&](const auto & object) { + if (isCentroidWithinLanelets(object.object, check_lanes)) { + target_objects.push_back(utils::avoidance::transform(object.object, p)); + } + }); + }; + + const auto unavoidable_objects = [&data]() { + ObjectDataArray ret; + std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { + if (!object.is_avoidable) { + ret.push_back(object); + } + }); + return ret; + }(); + + // check right lanes + if (check_right_lanes) { + const auto check_lanes = getAdjacentLane(planner_data, p, true); + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + // check left lanes + if (check_left_lanes) { + const auto check_lanes = getAdjacentLane(planner_data, p, false); + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + // check current lanes + if (p->check_current_lane) { + const auto check_lanes = data.current_lanelets; + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + return target_objects; +} } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index dcab96069a0a8..44b575e2cdf44 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1212,6 +1212,16 @@ std::vector generateDrivableLanesWithShoulderLanes( return drivable_lanes; } +std::vector getNonOverlappingExpandedLanes( + PathWithLaneId & path, const std::vector & lanes, + const DrivableAreaExpansionParameters & parameters) +{ + const auto shorten_lanes = utils::cutOverlappedLanes(path, lanes); + return utils::expandLanelets( + shorten_lanes, parameters.drivable_area_left_bound_offset, + parameters.drivable_area_right_bound_offset, parameters.drivable_area_types_to_skip); +} + boost::optional getOverlappedLaneletId(const std::vector & lanes) { auto overlaps = [](const DrivableLanes & lanes, const DrivableLanes & target_lanes) { diff --git a/planning/behavior_velocity_stop_line_module/README.md b/planning/behavior_velocity_stop_line_module/README.md index 3db639c77a7ca..f373afc2351bf 100644 --- a/planning/behavior_velocity_stop_line_module/README.md +++ b/planning/behavior_velocity_stop_line_module/README.md @@ -25,7 +25,7 @@ This module is activated when there is a stop line in a target lane. - Gets a stop line from map information. - insert a stop point on the path from the stop line defined in the map and the ego vehicle length. - Sets velocities of the path after the stop point to 0[m/s]. -- Release the inserted stop velocity when the vehicle stops within a radius of 2[m] from the stop point. +- Release the inserted stop velocity when the vehicle stops at the stop point for `stop_duration_sec` seconds. #### Flowchart @@ -50,13 +50,13 @@ endif if (state is APPROACH) then (yes) :set stop velocity; - if (vehicle is within stop_check_dist?) then (yes) + if (vehicle is within hold_stop_margin_distance?) then (yes) if (vehicle is stopped?) then (yes) :change state to STOPPED; endif endif else if (state is STOPPED) then (yes) - if (vehicle started to move?) then (yes) + if (stopping time is longer than stop_duration_sec ?) then (yes) :change state to START; endif else if (state is START) then (yes) diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index b77af629ad7c3..d3c77aa0faa84 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -62,11 +62,12 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions "~/input/external_velocity_limit_mps", 1, std::bind(&MotionVelocitySmootherNode::onExternalVelocityLimit, this, _1)); sub_current_acceleration_ = create_subscription( - "~/input/acceleration", 1, - [this](const AccelWithCovarianceStamped::SharedPtr msg) { current_acceleration_ptr_ = msg; }); + "~/input/acceleration", 1, [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { + current_acceleration_ptr_ = msg; + }); sub_operation_mode_ = create_subscription( "~/input/operation_mode_state", 1, - [this](const OperationModeState::SharedPtr msg) { operation_mode_ = *msg; }); + [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; }); // parameter update set_param_res_ = this->add_on_set_parameters_callback( 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 68e9733bcc12f..97607be30de3f 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -76,7 +76,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node EgoNearestParam ego_nearest_param_{}; // variables for subscribers - Odometry::SharedPtr ego_state_ptr_; + Odometry::ConstSharedPtr ego_state_ptr_; // interface publisher rclcpp::Publisher::SharedPtr traj_pub_; @@ -97,7 +97,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node OnSetParametersCallbackHandle::SharedPtr set_param_res_; // subscriber callback function - void onPath(const Path::SharedPtr); + void onPath(const Path::ConstSharedPtr path_ptr); // reset functions void initializePlanning(); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 42a4c7ab78e65..a135ffdcfa4b7 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -75,7 +75,7 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::SharedPtr msg) { ego_state_ptr_ = msg; }); + "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -189,7 +189,7 @@ void ObstacleAvoidancePlanner::resetPreviousData() mpt_optimizer_ptr_->resetPreviousData(); } -void ObstacleAvoidancePlanner::onPath(const Path::SharedPtr path_ptr) +void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) { time_keeper_ptr_->init(); time_keeper_ptr_->tic(__func__); diff --git a/planning/path_smoother/CMakeLists.txt b/planning/path_smoother/CMakeLists.txt index 30f06f7b87f81..b2fcf9076a7b3 100644 --- a/planning/path_smoother/CMakeLists.txt +++ b/planning/path_smoother/CMakeLists.txt @@ -7,12 +7,7 @@ autoware_package() find_package(Eigen3 REQUIRED) ament_auto_add_library(path_smoother SHARED - # node - src/elastic_band_smoother.cpp - # algorithms - src/elastic_band.cpp - # utils - src/utils/trajectory_utils.cpp + DIRECTORY src ) target_include_directories(path_smoother diff --git a/planning/path_smoother/config/elastic_band_smoother.param.yaml b/planning/path_smoother/config/elastic_band_smoother.param.yaml index 730bc3053e7a3..8e77420dd4ae9 100644 --- a/planning/path_smoother/config/elastic_band_smoother.param.yaml +++ b/planning/path_smoother/config/elastic_band_smoother.param.yaml @@ -35,3 +35,13 @@ # nearest search ego_nearest_dist_threshold: 3.0 # [m] ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] + # replanning & trimming trajectory param outside algorithm + replan: + enable: true # if true, only perform smoothing when the input changes significantly + max_path_shape_around_ego_lat_dist: 2.0 # threshold of path shape change around ego [m] + max_path_shape_forward_lon_dist: 100.0 # forward point to check lateral distance difference [m] + max_path_shape_forward_lat_dist: 0.1 # threshold of path shape change around forward point [m] + max_ego_moving_dist: 5.0 # threshold of ego's moving distance for replan [m] + # make max_goal_moving_dist long to keep start point fixed for pull over + max_goal_moving_dist: 15.0 # threshold of goal's moving distance for replan [m] + max_delta_time_sec: 1.0 # threshold of delta time for replan [second] diff --git a/planning/path_smoother/include/path_smoother/common_structs.hpp b/planning/path_smoother/include/path_smoother/common_structs.hpp index 17c6bc6f162af..94f4d62c4a6fd 100644 --- a/planning/path_smoother/include/path_smoother/common_structs.hpp +++ b/planning/path_smoother/include/path_smoother/common_structs.hpp @@ -30,15 +30,15 @@ struct Bounds; struct PlannerData { - // input - Header header; - std::vector traj_points; // converted from the input path - std::vector left_bound; - std::vector right_bound; - - // ego + std::vector traj_points; geometry_msgs::msg::Pose ego_pose; double ego_vel{}; + + PlannerData( + std::vector traj_points_, geometry_msgs::msg::Pose ego_pose_, double ego_vel_) + : traj_points(traj_points_), ego_pose(ego_pose_), ego_vel(ego_vel_) + { + } }; struct TimeKeeper diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index 26bc3b71d8245..1085ca3815728 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -18,6 +18,7 @@ #include "motion_utils/motion_utils.hpp" #include "path_smoother/common_structs.hpp" #include "path_smoother/elastic_band.hpp" +#include "path_smoother/replan_checker.hpp" #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -59,13 +60,14 @@ class ElasticBandSmoother : public rclcpp::Node // algorithms std::shared_ptr eb_path_smoother_ptr_{nullptr}; + std::shared_ptr replan_checker_ptr_{nullptr}; // parameters CommonParam common_param_{}; EgoNearestParam ego_nearest_param_{}; // variables for subscribers - Odometry::SharedPtr ego_state_ptr_; + Odometry::ConstSharedPtr ego_state_ptr_; // variables for previous information std::shared_ptr> prev_optimized_traj_points_ptr_; @@ -88,7 +90,7 @@ class ElasticBandSmoother : public rclcpp::Node OnSetParametersCallbackHandle::SharedPtr set_param_res_; // subscriber callback function - void onPath(const Path::SharedPtr); + void onPath(const Path::ConstSharedPtr path_ptr); // reset functions void initializePlanning(); diff --git a/planning/path_smoother/include/path_smoother/replan_checker.hpp b/planning/path_smoother/include/path_smoother/replan_checker.hpp new file mode 100644 index 0000000000000..d06cbc093a0c8 --- /dev/null +++ b/planning/path_smoother/include/path_smoother/replan_checker.hpp @@ -0,0 +1,71 @@ +// Copyright 2023 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 PATH_SMOOTHER__REPLAN_CHECKER_HPP_ +#define PATH_SMOOTHER__REPLAN_CHECKER_HPP_ + +#include "path_smoother/common_structs.hpp" +#include "path_smoother/type_alias.hpp" + +#include + +#include +#include + +namespace path_smoother +{ +class ReplanChecker +{ +public: + explicit ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param); + void onParam(const std::vector & parameters); + + bool isResetRequired(const PlannerData & planner_data) const; + + bool isReplanRequired(const PlannerData & planner_data, const rclcpp::Time & current_time) const; + + void updateData( + const PlannerData & planner_data, const bool is_replan_required, + const rclcpp::Time & current_time); + +private: + EgoNearestParam ego_nearest_param_; + rclcpp::Logger logger_; + + // previous variables for isResetRequired + std::shared_ptr> prev_traj_points_ptr_{nullptr}; + std::shared_ptr prev_ego_pose_ptr_{nullptr}; + + // previous variable for isReplanRequired + std::shared_ptr prev_replanned_time_ptr_{nullptr}; + + // algorithm parameters + bool enable_; + double max_path_shape_around_ego_lat_dist_; + double max_path_shape_forward_lat_dist_; + double max_path_shape_forward_lon_dist_; + double max_ego_moving_dist_; + double max_goal_moving_dist_; + double max_delta_time_sec_; + + bool isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; + bool isPathForwardChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; + bool isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; +}; +} // namespace path_smoother + +#endif // PATH_SMOOTHER__REPLAN_CHECKER_HPP_ diff --git a/planning/path_smoother/include/path_smoother/type_alias.hpp b/planning/path_smoother/include/path_smoother/type_alias.hpp index d6434c3935d1e..75bf1cff20857 100644 --- a/planning/path_smoother/include/path_smoother/type_alias.hpp +++ b/planning/path_smoother/include/path_smoother/type_alias.hpp @@ -20,8 +20,6 @@ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" #include "tier4_debug_msgs/msg/string_stamped.hpp" diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 4120b8aba75b8..dbd0f2de92f29 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -71,7 +71,7 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ElasticBandSmoother::onPath, this, std::placeholders::_1)); odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::SharedPtr msg) { ego_state_ptr_ = msg; }); + "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -87,6 +87,7 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option eb_path_smoother_ptr_ = std::make_shared( this, enable_debug_info_, ego_nearest_param_, common_param_, time_keeper_ptr_); + replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); // reset planners initializePlanning(); @@ -109,6 +110,7 @@ rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( // parameters for core algorithms eb_path_smoother_ptr_->onParam(parameters); + replan_checker_ptr_->onParam(parameters); // reset planners initializePlanning(); @@ -134,7 +136,7 @@ void ElasticBandSmoother::resetPreviousData() prev_optimized_traj_points_ptr_ = nullptr; } -void ElasticBandSmoother::onPath(const Path::SharedPtr path_ptr) +void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) { time_keeper_ptr_->init(); time_keeper_ptr_->tic(__func__); @@ -162,11 +164,29 @@ void ElasticBandSmoother::onPath(const Path::SharedPtr path_ptr) const auto input_traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); // 1. calculate trajectory with Elastic Band + // 1.a check if replan (= optimization) is required + PlannerData planner_data( + input_traj_points, ego_state_ptr_->pose.pose, ego_state_ptr_->twist.twist.linear.x); + const bool is_replan_required = [&]() { + if (replan_checker_ptr_->isResetRequired(planner_data)) { + // NOTE: always replan when resetting previous optimization + resetPreviousData(); + return true; + } + // check replan when not resetting previous optimization + return !prev_optimized_traj_points_ptr_ || + replan_checker_ptr_->isReplanRequired(planner_data, now()); + }(); + replan_checker_ptr_->updateData(planner_data, is_replan_required, now()); time_keeper_ptr_->tic(__func__); - auto smoothed_traj_points = - eb_path_smoother_ptr_->smoothTrajectory(input_traj_points, ego_state_ptr_->pose.pose); + auto smoothed_traj_points = is_replan_required ? eb_path_smoother_ptr_->smoothTrajectory( + input_traj_points, ego_state_ptr_->pose.pose) + : *prev_optimized_traj_points_ptr_; time_keeper_ptr_->toc(__func__, " "); + prev_optimized_traj_points_ptr_ = + std::make_shared>(smoothed_traj_points); + // 2. update velocity applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose); diff --git a/planning/path_smoother/src/replan_checker.cpp b/planning/path_smoother/src/replan_checker.cpp new file mode 100644 index 0000000000000..95c4e1eb1c002 --- /dev/null +++ b/planning/path_smoother/src/replan_checker.cpp @@ -0,0 +1,210 @@ +// Copyright 2023 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 "path_smoother/replan_checker.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "path_smoother/utils/trajectory_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +namespace path_smoother +{ +ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) +: ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) +{ + enable_ = node->declare_parameter("replan.enable"); + max_path_shape_around_ego_lat_dist_ = + node->declare_parameter("replan.max_path_shape_around_ego_lat_dist"); + max_path_shape_forward_lat_dist_ = + node->declare_parameter("replan.max_path_shape_forward_lat_dist"); + max_path_shape_forward_lon_dist_ = + node->declare_parameter("replan.max_path_shape_forward_lon_dist"); + max_ego_moving_dist_ = node->declare_parameter("replan.max_ego_moving_dist"); + max_goal_moving_dist_ = node->declare_parameter("replan.max_goal_moving_dist"); + max_delta_time_sec_ = node->declare_parameter("replan.max_delta_time_sec"); +} + +void ReplanChecker::onParam(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + updateParam(parameters, "replan.enable", enable_); + updateParam( + parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); + updateParam( + parameters, "replan.max_path_shape_forward_lat_dist", max_path_shape_forward_lat_dist_); + updateParam( + parameters, "replan.max_path_shape_forward_lon_dist", max_path_shape_forward_lon_dist_); + updateParam(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); + updateParam(parameters, "replan.max_goal_moving_dist", max_goal_moving_dist_); + updateParam(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); +} + +bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const +{ + const auto & p = planner_data; + + const bool reset_required = [&]() { + // guard for invalid variables + if (!prev_traj_points_ptr_ || !prev_ego_pose_ptr_) { + return true; + } + const auto & prev_traj_points = *prev_traj_points_ptr_; + + // path shape changes + if (isPathAroundEgoChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO( + logger_, "Replan with resetting optimization since path shape around ego changed."); + return true; + } + + // path goal changes + if (isPathGoalChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan with resetting optimization since path goal changed."); + return true; + } + + // 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 (max_ego_moving_dist_ < delta_dist) { + RCLCPP_INFO( + logger_, + "Replan with resetting optimization since current ego pose is far from previous ego pose."); + return true; + } + + return false; + }(); + + return reset_required; +} + +bool ReplanChecker::isReplanRequired( + const PlannerData & planner_data, const rclcpp::Time & current_time) const +{ + if (!enable_) return true; + + // guard for invalid variables + if (!prev_replanned_time_ptr_ || !prev_traj_points_ptr_) return true; + const auto & prev_traj_points = *prev_traj_points_ptr_; + + // time elapses + const double delta_time_sec = (current_time - *prev_replanned_time_ptr_).seconds(); + if (max_delta_time_sec_ < delta_time_sec) return true; + + // path shape changes + if (isPathForwardChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan since path forward shape changed."); + return true; + } + + return false; +} + +void ReplanChecker::updateData( + const PlannerData & planner_data, const bool is_replan_required, + const rclcpp::Time & current_time) +{ + const auto & p = planner_data; + + // update previous information required in this function + prev_traj_points_ptr_ = std::make_shared>(p.traj_points); + prev_ego_pose_ptr_ = std::make_shared(p.ego_pose); + + // update previous information required in this function + if (is_replan_required) { + prev_replanned_time_ptr_ = std::make_shared(current_time); + } +} + +bool ReplanChecker::isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // calculate ego's lateral offset to previous trajectory points + const auto prev_ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); + const double prev_ego_lat_offset = + motion_utils::calcLateralOffset(prev_traj_points, p.ego_pose.position, prev_ego_seg_idx); + + // calculate ego's lateral offset to current trajectory points + const auto ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); + const double ego_lat_offset = + motion_utils::calcLateralOffset(p.traj_points, p.ego_pose.position, ego_seg_idx); + + const double diff_ego_lat_offset = prev_ego_lat_offset - ego_lat_offset; + if (std::abs(diff_ego_lat_offset) < max_path_shape_around_ego_lat_dist_) { + return false; + } + + return true; +} + +bool ReplanChecker::isPathForwardChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // calculate forward point of previous trajectory points + const size_t prev_ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); + + // check if distance is larger than the threshold + constexpr double lon_dist_interval = 10.0; + for (double lon_dist = lon_dist_interval; lon_dist <= max_path_shape_forward_lon_dist_; + lon_dist += lon_dist_interval) { + const auto prev_forward_point = + motion_utils::calcLongitudinalOffsetPoint(prev_traj_points, prev_ego_seg_idx, lon_dist); + if (!prev_forward_point) { + continue; + } + + // calculate lateral offset of current trajectory points to prev forward point + const auto forward_seg_idx = + motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); + const double forward_lat_offset = + motion_utils::calcLateralOffset(p.traj_points, *prev_forward_point, forward_seg_idx); + if (max_path_shape_forward_lat_dist_ < std::abs(forward_lat_offset)) { + return true; + } + } + + return false; +} + +bool ReplanChecker::isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // check if the vehicle is stopping + constexpr double min_vel = 1e-3; + if (min_vel < std::abs(p.ego_vel)) { + return false; + } + + const double goal_moving_dist = + tier4_autoware_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + if (goal_moving_dist < max_goal_moving_dist_) { + return false; + } + + return true; +} +} // namespace path_smoother diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 1661ac9ebef3a..4929d0f8e27f3 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -53,8 +53,8 @@ class RTCInterface const rclcpp::Time & stamp); void removeCooperateStatus(const UUID & uuid); void clearCooperateStatus(); - bool isActivated(const UUID & uuid); - bool isRegistered(const UUID & uuid); + bool isActivated(const UUID & uuid) const; + bool isRegistered(const UUID & uuid) const; void lockCommandUpdate(); void unlockCommandUpdate(); @@ -74,10 +74,9 @@ class RTCInterface rclcpp::Publisher::SharedPtr pub_statuses_; rclcpp::Service::SharedPtr srv_commands_; rclcpp::Service::SharedPtr srv_auto_mode_; - - std::mutex mutex_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::Logger logger_; + Module module_; CooperateStatusArray registered_status_; std::vector stored_commands_; @@ -87,6 +86,8 @@ class RTCInterface std::string cooperate_status_namespace_ = "/planning/cooperate_status"; std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode/internal"; + + mutable std::mutex mutex_; }; } // namespace rtc_interface diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index f4a95d0acf9d1..dbc82113d403b 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -251,7 +251,7 @@ void RTCInterface::clearCooperateStatus() stored_commands_.clear(); } -bool RTCInterface::isActivated(const UUID & uuid) +bool RTCInterface::isActivated(const UUID & uuid) const { std::lock_guard lock(mutex_); const auto itr = std::find_if( @@ -271,7 +271,7 @@ bool RTCInterface::isActivated(const UUID & uuid) return false; } -bool RTCInterface::isRegistered(const UUID & uuid) +bool RTCInterface::isRegistered(const UUID & uuid) const { std::lock_guard lock(mutex_); const auto itr = std::find_if( diff --git a/system/default_ad_api/document/images/autoware-state-table.drawio.svg b/system/default_ad_api/document/images/autoware-state-table.drawio.svg index ab21c1b865407..2b57830f6b869 100644 --- a/system/default_ad_api/document/images/autoware-state-table.drawio.svg +++ b/system/default_ad_api/document/images/autoware-state-table.drawio.svg @@ -3,14 +3,14 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="721px" + width="921px" height="201px" - viewBox="-0.5 -0.5 721 201" - content="<mxfile><diagram id="DGwK_OstZrDMA8GPxkck" name="Page-1">3Zpdk5owFIZ/DffyIbqXravbXnSm073Y61QiZBoTJwY/9tc3QEDMWafMmkYmOuPACVF4OEne92AQL7anF4F2xQ+eYRpEk+wUxM9BFM1mU/VZBc5NIJmlTSAXJGtC4SXwSt6xDk50tCQZ3l8dKDmnkuyug2vOGF7LqxgSgh+vD9twev2rO5RjEHhdIwqjbySThY6G6dOl4RsmeaF/eh7NmoYtag/WV7IvUMaPvVC8DOKF4Fw2W9vTAtOKXcul6be60dqdmMBMDukQ6TM+IFrqi9MnJs/t1QpesgxXHSZB/PVYEIlfd2hdtR7V7VWxQm6p2gvV5oZQuuCUi7pvvJlW7yrOmezFm5eK76Xgf3CvJa1fqkWfGBYSn25eXdgxU7mG+RZLcVaH6A5pojHrPEv17rF309o7UfTu15OOIZ0neffNF5RqQ9O8QRaAfUNEEpavuPjFS4k94mxg7pj+i3NkgXMMOFOuRip5R5JwVhNGXsGO5te0E4ewEwBbcayS2kPOyeRxnKeAM99h0Wb0Vq2nHoGeRo8DnQLQqJS8YzxBB0Qo+k194m2uii55zwBvwtSqWM3X9SxyD2QLaML0c4LBBpo5nFtLxsZIZT6QSphYwAIFasm6nFEQHg3HXI6HwrGRMq0fGutwMtmEE5dwwttwRpg3g0VzB/EuONCelGyPZRAtGo9MDiNgZEowp8YihM6iBjQ2JkOX79QGEyhMx5orkdO5BuqaDWFjmYbN9TtyOoygrtlLvns4FNN1DB5HNkRNCFUN47IGMkY0LjVN+x39oYTo/k4DZoGKaZsGJ4wVKlDMSFGOEIpLxxRBEfPdmvwdlWX/ZB3bxpL/UYEVlWxdBP6V/MyF0ukIh7XVnxRZKACMivBD51AoXi+PZJYsrx76+Uva6cQMy6rPyiX4ncpORRL0G18aH/bCEfUXslNT90FFdmXJ1I2asdOnt1Dte0QWqOOhc4QN4RZDy+ARWrPc41ISx9B3eETWdP9Dyc5tkIVmw2Oyg1czGyWnGPoLj9CC6q/TRQwaC4/Qmg+q3KKFTsJjtP/Ro6ndy79K67beX3Pj5V8=</diagram></mxfile>" + viewBox="-0.5 -0.5 921 201" + content="<mxfile><diagram id="DGwK_OstZrDMA8GPxkck" name="Page-1">3ZpNc5swEIZ/Sw/cAdnEObbOR3voTKc55JhRjWw0FZJHFraTX18JhI1ZMmESVWFkz9iw4ksPi3bfRRFalsd7ibfFT5ETFqVxfozQTZSmyXw+13/G8txYsuukMWwkze1GZ8MDfSHWGFtrRXOyu9hQCcEU3V4aV4JzslIXNiylOFxuthbs8qxbvCHA8LDCDFofaa4Ka02y63PDd0I3hT31Ir1qGkrcbmx7sitwLg4dE7qN0FIKoZql8rgkzMBruTT73b3SerowSbgas0Nqr3iPWWU7Zy9MPbe9laLiOTE7xBH6diioIg9bvDKtB31/ta1QJdNriV5cU8aWgglZ74vWc/M1dsFVx958tH2npPhLOi1Z/dEtsCu2d3siFTl2TLZr90SURMlnvYltXcws5tbP7Oqhc9PaO1F07te1tWHrJ5vTkc8o9YKl+QpZAPYRU0X55k7I36JSJCDOPcwnpm9xTh1wRoAzE/pJpS9YUcFrwjgo2OnikvbMI+wZgK05GqcOkPMs/jzOc8BZbIlsPbrUATUg0PP0TdDof4HOAGhcKXFiHOM9pgz/YSHx7kdFn459BXhTrqOiGa/rUeQjkB2gSbL3JQwu0Czg2FpxPkUqi5FUkpkDLDBBrfjJZzSEz4bTD8dj4bhwmVYPTfVx6rNJYp9wktfhTNBvRifNJ4gfggPlScV3REXpstHIdD8BRv0UzKuwSKCyqAFNjcnY8J25YAIT06n6Sup1rIF5zZryqQzD/fiden2MYF5D2O6D+bQDKH3VMfQcITQAxUVSk8LAvcZToNLXBqNHFxeuksKIrWQ1QSg+ZUEKI/UPZznepHTpO4u1LuLaUBURV3xVROHVtfrRwOsTDguIvxh2oHInRfhTx1CYoZ3fO9zyjXmzFS5prwMzrB3e6FQ4bFf2Wd0YKBZ+bcTGvcAsXMhelctA2fHOkXKZNGOvryhhth8QWZAdjx0jXCRuCEqGgND2axo+U2IEdUdAZPvqf4Ds4DvHhQuyUGwETHYomg2idVFXQVBfBIQWlDi9BjEoLAJC238b4xctVBIBo/Wp0RCUEHZix5d6TRMW28icKcOlgdj8gtXYzAo5aApPK41QCvZEuJkVkpt99YHqY7mqVQK0AzdgfFl7IBtzNZtGr55nqtZtnfm+6PYf</diagram></mxfile>" > - + @@ -62,13 +62,13 @@ routing state - +
@@ -77,16 +77,16 @@
- operation mode + operation mode - +
@@ -95,7 +95,7 @@
- auto mode available + auto mode available @@ -260,49 +260,31 @@ finalizing - +
- stop + else
- stop + else - +
-
-
- not stop -
-
-
- - not stop - - - - - - -
@@ -311,16 +293,16 @@
- false + false - +
@@ -329,7 +311,7 @@
- true + true @@ -440,18 +422,38 @@ Finalizing - + - - + + + + + + +
+
+
+ mode != stop && autoware_control_enabled == true +
+
+
+ + + mode != stop && autoware_control_enabled == true + + + - Viewer does not support full SVG 1.1 + Text is not SVG - cannot display