diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 5b140152e7f08..3c2cdb8aafb20 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -1,20 +1,17 @@ +*:*/test/* + arrayIndexThenCheck -assignBoolToFloat checkersReport constParameterPointer constParameterReference constStatement constVariable -constVariablePointer constVariableReference containerOutOfBounds cstyleCast -ctuOneDefinitionRuleViolation -current_deleted_index duplicateAssignExpression duplicateBranch duplicateBreak -duplicateCondition duplicateExpression funcArgNamesDifferent functionConst @@ -23,37 +20,28 @@ invalidPointerCast knownConditionTrueFalse missingInclude missingIncludeSystem -multiCondition noConstructor noExplicitConstructor noValidConfiguration -obstacle_cruise_planner passedByValue preprocessorErrorDirective -redundantAssignment redundantContinue -redundantIfRemove redundantInitialization returnByReference -selfAssignment shadowArgument shadowFunction shadowVariable -stlFindInsert syntaxError uninitMemberVar unknownMacro unmatchedSuppression -unpreciseMathCall unreadVariable -unsignedLessThanZero unusedFunction unusedScopedObject unusedStructMember unusedVariable useInitializationList useStlAlgorithm -uselessCallsSubstr uselessOverride variableScope virtualCallInConstructor diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 45fc1dd5176d6..ade750db259b9 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,11 +1,15 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +common/autoware_grid_map_utils/** maxime.clement@tier4.jp +common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_point_types/** taichi.higashide@tier4.jp +common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp common/component_interface_specs/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp common/component_interface_tools/** isamu.takagi@tier4.jp @@ -16,10 +20,8 @@ common/geography_utils/** koji.minoda@tier4.jp common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/glog_component/** takamasa.horibe@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 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 mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@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 @@ -30,7 +32,6 @@ common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi common/tensorrt_common/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp -common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp @@ -50,24 +51,23 @@ control/autoware_external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@t control/autoware_joy_controller/** fumiya.watanabe@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 control/autoware_lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp zulfaqar.azmi@tier4.jp control/autoware_mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_operation_mode_transition_manager/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_pure_pursuit/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_shift_decider/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_smart_mpc_trajectory_follower/** kosuke.takeuchi@tier4.jp masayuki.aino@proxima-ai-tech.com takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@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 control/obstacle_collision_checker/** fumiya.watanabe@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 -control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/predicted_path_checker/** berkay@leodrive.ai -control/autoware_smart_mpc_trajectory_follower/** masayuki.aino@proxima-ai-tech.com -control/autoware_trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/pure_pursuit/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/shift_decider/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/autoware_trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -evaluator/control_evaluator/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp +evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp +evaluator/autoware_planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp 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/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp -evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @@ -78,21 +78,20 @@ launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp launch/tier4_system_launch/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp +localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_landmark_based_localizer/autoware_landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_pose_covariance_modifier/** melike@leodrive.ai localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/landmark_based_localizer/ar_tag_based_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/landmark_based_localizer/landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/pose_estimator_arbiter/** kento.yabuuchi.2@tier4.jp +localization/pose_estimator_arbiter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/tree_structured_parzen_estimator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @@ -146,71 +145,58 @@ perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp -planning/autoware_behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp -planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/autoware_behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/autoware_behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_path_static_obstacle_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/autoware_behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_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 -planning/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp -planning/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp -planning/autoware_behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/autoware_costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/autoware_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/autoware_freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/autoware_freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/autoware_mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/autoware_objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/autoware_path_optimizer/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/autoware_path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/autoware_planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/autoware_planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai +planning/autoware_route_handler/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp planning/autoware_rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp planning/autoware_scenario_selector/** fumiya.watanabe@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 planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/autoware_surround_obstacle_checker/** go.sakayori@tier4.jp satoshi.ota@tier4.jp planning/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp -planning/behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/autoware_behavior_path_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp -planning/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp -planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp -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/autoware_behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_lane_change_module/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_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_planner/autoware_behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_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 +planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp -planning/autoware_objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp -planning/autoware_obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai bnk@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp -planning/planning_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp -planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp -planning/autoware_route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp -planning/rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp -planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp @@ -254,9 +240,9 @@ system/system_monitor/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp tools/reaction_analyzer/** berkay@leodrive.ai -vehicle/autoware_accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp +vehicle/autoware_accel_brake_map_calibrator/** eiki.nagata.2@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp vehicle/autoware_external_cmd_converter/** eiki.nagata.2@tier4.jp takamasa.horibe@tier4.jp -vehicle/autoware_raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp +vehicle/autoware_raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp sho.iwasawa.2@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp vehicle/autoware_vehicle_info_utils/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 5b60a371abd93..263df86438787 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -18,6 +18,14 @@ - source: .github/workflows/pre-commit-optional.yaml - source: .github/workflows/semantic-pull-request.yaml - source: .github/workflows/spell-check-differential.yaml + pre-commands: | + sd " with:\n" " with:\n local-cspell-json: .cspell.json\n" {source} + - source: .github/workflows/spell-check-differential.yaml + dest: .github/workflows/spell-check-daily.yaml + pre-commands: | + sd "spell-check-differential" "spell-check-daily" {source} + sd " with:\n" " with:\n local-cspell-json: .cspell.json\n incremental-files-only: false\n" {source} + sd "on:\n pull_request:\n" "on:\n schedule:\n - cron: 0 0 * * *\n workflow_dispatch:\n" {source} - source: .github/workflows/sync-files.yaml - source: .clang-format - source: .markdown-link-check.json @@ -31,39 +39,9 @@ - repository: autowarefoundation/autoware_common files: - - source: .github/workflows/build-and-test.yaml - pre-commands: | - sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} - - sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source} - sd -- \ - " include:" \ - " container-suffix: - - \"\" - - -cuda - include:" {source} - - source: .github/workflows/build-and-test-differential-self-hosted.yaml + - source: .github/workflows/clang-tidy-differential.yaml pre-commands: | - sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} - - sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source} - sd -- \ - " include:" \ - " container-suffix: - - \"\" - - -cuda - include:" {source} - - source: .github/workflows/build-and-test-self-hosted.yaml - pre-commands: | - sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} - - sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source} - sd -- \ - " include:" \ - " container-suffix: - - \"\" - - -cuda - include:" {source} + sd 'container: ros:(\w+)' 'container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda' {source} - source: .github/workflows/check-build-depends.yaml - source: .github/workflows/clang-tidy-pr-comments.yaml - source: .github/workflows/clang-tidy-pr-comments-manually.yaml diff --git a/.github/workflows/build-and-test-arm64.yaml b/.github/workflows/build-and-test-daily-arm64.yaml similarity index 79% rename from .github/workflows/build-and-test-arm64.yaml rename to .github/workflows/build-and-test-daily-arm64.yaml index c9a4b46874e18..ee9caae26e5d6 100644 --- a/.github/workflows/build-and-test-arm64.yaml +++ b/.github/workflows/build-and-test-daily-arm64.yaml @@ -1,4 +1,4 @@ -name: build-and-test-arm64 +name: build-and-test-daily-arm64 on: schedule: @@ -6,7 +6,7 @@ on: workflow_dispatch: jobs: - build-and-test-arm64: + build-and-test-daily-arm64: runs-on: [self-hosted, linux, ARM64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: @@ -24,6 +24,8 @@ jobs: steps: - name: Check out repository uses: actions/checkout@v4 + with: + fetch-depth: 1 - name: Show disk space before the tasks run: df -h @@ -45,11 +47,21 @@ jobs: - name: Test if: ${{ steps.get-self-packages.outputs.self-packages != '' }} + id: test uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: rosdistro: ${{ matrix.rosdistro }} target-packages: ${{ steps.get-self-packages.outputs.self-packages }} build-depends-repos: ${{ matrix.build-depends-repos }} + - name: Upload coverage to CodeCov + if: ${{ steps.test.outputs.coverage-report-files != '' }} + uses: codecov/codecov-action@v4 + with: + files: ${{ steps.test.outputs.coverage-report-files }} + fail_ci_if_error: false + verbose: true + flags: total-arm64 + - name: Show disk space after the tasks run: df -h diff --git a/.github/workflows/build-and-test-daily.yaml b/.github/workflows/build-and-test-daily.yaml new file mode 100644 index 0000000000000..50e259053de46 --- /dev/null +++ b/.github/workflows/build-and-test-daily.yaml @@ -0,0 +1,67 @@ +name: build-and-test-daily + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + build-and-test-daily: + runs-on: [self-hosted, linux, X64] + container: ${{ matrix.container }}${{ matrix.container-suffix }} + strategy: + fail-fast: false + matrix: + rosdistro: + - humble + container-suffix: + - "" + - -cuda + include: + - rosdistro: humble + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt + build-depends-repos: build_depends.repos + steps: + - name: Check out repository + uses: actions/checkout@v4 + with: + fetch-depth: 1 + + - name: Show disk space before the tasks + run: df -h + + - name: Remove exec_depend + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 + + - name: Get self packages + id: get-self-packages + uses: autowarefoundation/autoware-github-actions/get-self-packages@v1 + + - name: Build + if: ${{ steps.get-self-packages.outputs.self-packages != '' }} + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 + with: + rosdistro: ${{ matrix.rosdistro }} + target-packages: ${{ steps.get-self-packages.outputs.self-packages }} + build-depends-repos: ${{ matrix.build-depends-repos }} + + - name: Test + if: ${{ steps.get-self-packages.outputs.self-packages != '' }} + id: test + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 + with: + rosdistro: ${{ matrix.rosdistro }} + target-packages: ${{ steps.get-self-packages.outputs.self-packages }} + build-depends-repos: ${{ matrix.build-depends-repos }} + + - name: Upload coverage to CodeCov + if: ${{ steps.test.outputs.coverage-report-files != '' }} + uses: codecov/codecov-action@v4 + with: + files: ${{ steps.test.outputs.coverage-report-files }} + fail_ci_if_error: false + verbose: true + flags: total + + - name: Show disk space after the tasks + run: df -h diff --git a/.github/workflows/build-and-test-differential-arm64.yaml b/.github/workflows/build-and-test-differential-arm64.yaml index d6e49eaabc5bd..8f333a3ab22d1 100644 --- a/.github/workflows/build-and-test-differential-arm64.yaml +++ b/.github/workflows/build-and-test-differential-arm64.yaml @@ -5,18 +5,18 @@ on: types: - opened - synchronize + - reopened - labeled - workflow_dispatch: jobs: - prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + make-sure-label-is-present: + uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 with: label: type:arm64 build-and-test-differential-arm64: - needs: prevent-no-label-execution - if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} + needs: make-sure-label-is-present + if: ${{ needs.make-sure-label-is-present.outputs.result == 'true' }} runs-on: [self-hosted, linux, ARM64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: @@ -32,10 +32,14 @@ jobs: container: ghcr.io/autowarefoundation/autoware:latest-prebuilt build-depends-repos: build_depends.repos steps: - - name: Check out repository + - name: Set PR fetch depth + run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + + - name: Checkout PR branch and all PR commits uses: actions/checkout@v4 with: - fetch-depth: 0 + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} - name: Show disk space before the tasks run: df -h @@ -56,6 +60,7 @@ jobs: build-depends-repos: ${{ matrix.build-depends-repos }} - name: Test + id: test if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: @@ -63,5 +68,14 @@ jobs: target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} build-depends-repos: ${{ matrix.build-depends-repos }} + - name: Upload coverage to CodeCov + if: ${{ steps.test.outputs.coverage-report-files != '' }} + uses: codecov/codecov-action@v4 + with: + files: ${{ steps.test.outputs.coverage-report-files }} + fail_ci_if_error: false + verbose: true + flags: differential-arm64 + - name: Show disk space after the tasks run: df -h diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 6745976aa543a..e789b8744f03d 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -5,17 +5,22 @@ on: types: - opened - synchronize + - reopened - labeled +env: + CC: /usr/lib/ccache/gcc + CXX: /usr/lib/ccache/g++ + jobs: - prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + make-sure-label-is-present: + uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 with: label: tag:run-build-and-test-differential build-and-test-differential: - needs: prevent-no-label-execution - if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} + needs: make-sure-label-is-present + if: ${{ needs.make-sure-label-is-present.outputs.result == 'true' }} runs-on: ubuntu-latest container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: @@ -31,10 +36,14 @@ jobs: container: ghcr.io/autowarefoundation/autoware:latest-prebuilt build-depends-repos: build_depends.repos steps: - - name: Check out repository + - name: Set PR fetch depth + run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + + - name: Checkout PR branch and all PR commits uses: actions/checkout@v4 with: - fetch-depth: 0 + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} - name: Show disk space before the tasks run: df -h @@ -46,6 +55,25 @@ jobs: id: get-modified-packages uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + - name: Create ccache directory + run: | + mkdir -p ${CCACHE_DIR} + du -sh ${CCACHE_DIR} && ccache -s + shell: bash + + - name: Attempt to restore ccache + uses: actions/cache/restore@v4 + with: + path: | + /root/.ccache + key: ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}-${{ github.event.pull_request.base.sha }} + restore-keys: | + ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}- + + - name: Show ccache stats before build + run: du -sh ${CCACHE_DIR} && ccache -s + shell: bash + - name: Build if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-build@v1 @@ -54,6 +82,10 @@ jobs: target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} build-depends-repos: ${{ matrix.build-depends-repos }} + - name: Show ccache stats after build + run: du -sh ${CCACHE_DIR} && ccache -s + shell: bash + - name: Test id: test if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} @@ -65,7 +97,7 @@ jobs: - name: Upload coverage to CodeCov if: ${{ steps.test.outputs.coverage-report-files != '' }} - uses: codecov/codecov-action@v3 + uses: codecov/codecov-action@v4 with: files: ${{ steps.test.outputs.coverage-report-files }} fail_ci_if_error: false diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 85c531a02fca9..0c56d12800888 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -2,13 +2,16 @@ name: build-and-test on: push: - schedule: - - cron: 0 0 * * * + branches: + - main workflow_dispatch: +env: + CC: /usr/lib/ccache/gcc + CXX: /usr/lib/ccache/g++ + jobs: build-and-test: - if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }} runs-on: [self-hosted, linux, X64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: @@ -17,7 +20,6 @@ jobs: rosdistro: - humble container-suffix: - - "" - -cuda include: - rosdistro: humble @@ -26,6 +28,8 @@ jobs: steps: - name: Check out repository uses: actions/checkout@v4 + with: + fetch-depth: 1 - name: Show disk space before the tasks run: df -h @@ -37,6 +41,31 @@ jobs: id: get-self-packages uses: autowarefoundation/autoware-github-actions/get-self-packages@v1 + - name: Create ccache directory + run: | + mkdir -p ${CCACHE_DIR} + du -sh ${CCACHE_DIR} && ccache -s + shell: bash + + - name: Attempt to restore ccache + uses: actions/cache/restore@v4 + with: + path: | + /root/.ccache + key: ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}-${{ github.sha }} + restore-keys: | + ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}- + + - name: Limit ccache size + run: | + rm -f "${CCACHE_DIR}/ccache.conf" + echo -e "# Set maximum cache size\nmax_size = 600MB" >> "${CCACHE_DIR}/ccache.conf" + shell: bash + + - name: Show ccache stats before build + run: du -sh ${CCACHE_DIR} && ccache -s + shell: bash + - name: Build if: ${{ steps.get-self-packages.outputs.self-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-build@v1 @@ -45,6 +74,19 @@ jobs: target-packages: ${{ steps.get-self-packages.outputs.self-packages }} build-depends-repos: ${{ matrix.build-depends-repos }} + - name: Show ccache stats after build + run: du -sh ${CCACHE_DIR} && ccache -s + shell: bash + + # Only keep save the -cuda version because cuda packages covers non-cuda packages too + - name: Push the ccache cache + if: matrix.container-suffix == '-cuda' + uses: actions/cache/save@v4 + with: + path: | + /root/.ccache + key: ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}-${{ github.sha }} + - name: Test if: ${{ steps.get-self-packages.outputs.self-packages != '' }} id: test @@ -56,7 +98,7 @@ jobs: - name: Upload coverage to CodeCov if: ${{ steps.test.outputs.coverage-report-files != '' }} - uses: codecov/codecov-action@v3 + uses: codecov/codecov-action@v4 with: files: ${{ steps.test.outputs.coverage-report-files }} fail_ci_if_error: false diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml index 1da4d24966de9..44983f7deadcb 100644 --- a/.github/workflows/cancel-previous-workflows.yaml +++ b/.github/workflows/cancel-previous-workflows.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Cancel previous runs - uses: styfle/cancel-workflow-action@0.12.0 + uses: styfle/cancel-workflow-action@0.12.1 with: workflow_id: all all_but_latest: true diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index c790c2132d71e..81618a1db0eea 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -20,7 +20,7 @@ jobs: build-depends-repos: build_depends.repos steps: - name: Check out repository - uses: actions/checkout@v4 + uses: actions/checkout@v3 - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml index 26c5314d4b119..a59287a6d9b37 100644 --- a/.github/workflows/clang-tidy-differential.yaml +++ b/.github/workflows/clang-tidy-differential.yaml @@ -5,24 +5,29 @@ on: types: - opened - synchronize + - reopened - labeled jobs: - prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + make-sure-label-is-present: + uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 with: label: tag:run-clang-tidy-differential clang-tidy-differential: - needs: prevent-no-label-execution - if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} + needs: make-sure-label-is-present + if: ${{ needs.make-sure-label-is-present.outputs.result == 'true' }} runs-on: ubuntu-latest container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda steps: - - name: Check out repository + - name: Set PR fetch depth + run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + + - name: Checkout PR branch and all PR commits uses: actions/checkout@v4 with: - fetch-depth: 0 + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} - name: Show disk space before the tasks run: df -h @@ -36,7 +41,7 @@ jobs: - name: Get modified files id: get-modified-files - uses: tj-actions/changed-files@v35 + uses: tj-actions/changed-files@v42 with: files: | **/*.cpp @@ -51,3 +56,6 @@ jobs: target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy build-depends-repos: build_depends.repos + + - name: Show disk space after the tasks + run: df -h diff --git a/.github/workflows/clang-tidy-pr-comments-manually.yaml b/.github/workflows/clang-tidy-pr-comments-manually.yaml index 87f939fe8b72f..9bccd972becde 100644 --- a/.github/workflows/clang-tidy-pr-comments-manually.yaml +++ b/.github/workflows/clang-tidy-pr-comments-manually.yaml @@ -11,7 +11,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v4 + uses: actions/checkout@v3 - name: Download analysis results run: | @@ -36,7 +36,7 @@ jobs: - name: Check out PR head if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} - uses: actions/checkout@v4 + uses: actions/checkout@v3 with: repository: ${{ steps.set-variables.outputs.pr-head-repo }} ref: ${{ steps.set-variables.outputs.pr-head-ref }} diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml index 0f6db69dfedd4..baaa0fb8e7744 100644 --- a/.github/workflows/clang-tidy-pr-comments.yaml +++ b/.github/workflows/clang-tidy-pr-comments.yaml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v4 + uses: actions/checkout@v3 - name: Download analysis results run: | @@ -37,7 +37,7 @@ jobs: - name: Check out PR head if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} - uses: actions/checkout@v4 + uses: actions/checkout@v3 with: repository: ${{ steps.set-variables.outputs.pr-head-repo }} ref: ${{ steps.set-variables.outputs.pr-head-ref }} diff --git a/.github/workflows/delete-closed-pr-docs.yaml b/.github/workflows/delete-closed-pr-docs.yaml index eb211766c49dd..b7b009fb00263 100644 --- a/.github/workflows/delete-closed-pr-docs.yaml +++ b/.github/workflows/delete-closed-pr-docs.yaml @@ -10,7 +10,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v4 + uses: actions/checkout@v3 with: fetch-depth: 0 diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index e5967b0d50c67..b48d70dbacb0c 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -30,7 +30,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v4 + uses: actions/checkout@v3 with: fetch-depth: 0 ref: ${{ github.event.pull_request.head.sha }} diff --git a/.github/workflows/pre-commit-autoupdate.yaml b/.github/workflows/pre-commit-autoupdate.yaml new file mode 100644 index 0000000000000..23b403f2a52af --- /dev/null +++ b/.github/workflows/pre-commit-autoupdate.yaml @@ -0,0 +1,37 @@ +name: pre-commit-autoupdate + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + check-secret: + uses: autowarefoundation/autoware-github-actions/.github/workflows/check-secret.yaml@v1 + secrets: + secret: ${{ secrets.APP_ID }} + + pre-commit-autoupdate: + needs: check-secret + if: ${{ needs.check-secret.outputs.set == 'true' }} + runs-on: ubuntu-latest + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v2 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - name: Run pre-commit-autoupdate + uses: autowarefoundation/autoware-github-actions/pre-commit-autoupdate@v1 + with: + token: ${{ steps.generate-token.outputs.token }} + pre-commit-config: .pre-commit-config.yaml + pr-labels: | + tag:bot + tag:pre-commit-autoupdate + pr-branch: pre-commit-autoupdate + pr-title: "ci(pre-commit): autoupdate" + pr-commit-message: "ci(pre-commit): autoupdate" + auto-merge-method: squash diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index 33c00ee1066ae..c724885fcb3e4 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -10,7 +10,7 @@ jobs: steps: - name: Generate token id: generate-token - uses: tibdex/github-app-token@v1 + uses: tibdex/github-app-token@v2 with: app_id: ${{ secrets.APP_ID }} private_key: ${{ secrets.PRIVATE_KEY }} diff --git a/.github/workflows/spell-check-partial.yaml b/.github/workflows/spell-check-daily.yaml similarity index 88% rename from .github/workflows/spell-check-partial.yaml rename to .github/workflows/spell-check-daily.yaml index cf5eaa71e5741..dcee449abc728 100644 --- a/.github/workflows/spell-check-partial.yaml +++ b/.github/workflows/spell-check-daily.yaml @@ -1,13 +1,12 @@ -name: spell-check-partial +name: spell-check-daily on: - pull_request: schedule: - cron: 0 0 * * * workflow_dispatch: jobs: - spell-check-partial: + spell-check-daily: runs-on: ubuntu-latest steps: - name: Check out repository @@ -16,6 +15,6 @@ jobs: - name: Run spell-check uses: autowarefoundation/autoware-github-actions/spell-check@v1 with: - cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json local-cspell-json: .cspell.json incremental-files-only: false + cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml new file mode 100644 index 0000000000000..ee9d451ba9b9e --- /dev/null +++ b/.github/workflows/spell-check-differential.yaml @@ -0,0 +1,17 @@ +name: spell-check-differential + +on: + pull_request: + +jobs: + spell-check-differential: + runs-on: ubuntu-latest + steps: + - name: Check out repository + uses: actions/checkout@v4 + + - name: Run spell-check + uses: autowarefoundation/autoware-github-actions/spell-check@v1 + with: + local-cspell-json: .cspell.json + cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json diff --git a/.github/workflows/sync-files.yaml b/.github/workflows/sync-files.yaml index 5025e6c8bd7a7..51e523b8031bf 100644 --- a/.github/workflows/sync-files.yaml +++ b/.github/workflows/sync-files.yaml @@ -18,7 +18,7 @@ jobs: steps: - name: Generate token id: generate-token - uses: tibdex/github-app-token@v1 + uses: tibdex/github-app-token@v2 with: app_id: ${{ secrets.APP_ID }} private_key: ${{ secrets.PRIVATE_KEY }} diff --git a/.github/workflows/update-codeowners-from-packages.yaml b/.github/workflows/update-codeowners-from-packages.yaml index 7898dfe09177a..8b3d2407fbc75 100644 --- a/.github/workflows/update-codeowners-from-packages.yaml +++ b/.github/workflows/update-codeowners-from-packages.yaml @@ -18,7 +18,7 @@ jobs: steps: - name: Generate token id: generate-token - uses: tibdex/github-app-token@v1 + uses: tibdex/github-app-token@v2 with: app_id: ${{ secrets.APP_ID }} private_key: ${{ secrets.PRIVATE_KEY }} diff --git a/.gitignore b/.gitignore index 5ce2c268169c8..757d2d8a09bed 100644 --- a/.gitignore +++ b/.gitignore @@ -13,3 +13,6 @@ log/ # Python *.pyc + +#prettier +node_modules/ diff --git a/.markdownlint.yaml b/.markdownlint.yaml index babaaa1f1586d..7b7359fe0cdc4 100644 --- a/.markdownlint.yaml +++ b/.markdownlint.yaml @@ -7,5 +7,6 @@ MD029: style: ordered MD033: false MD041: false +MD045: false MD046: false MD049: false diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml index 3b43f9ae1139d..8c9345e15f064 100644 --- a/.pre-commit-config-optional.yaml +++ b/.pre-commit-config-optional.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/tcort/markdown-link-check - rev: v3.11.2 + rev: v3.12.2 hooks: - id: markdown-link-check args: [--quiet, --config=.markdown-link-check.json] diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a5ca7b6cf37fa..823ff516c1dc7 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -3,7 +3,7 @@ ci: repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v4.6.0 hooks: - id: check-json - id: check-merge-conflict @@ -18,18 +18,18 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.33.0 + rev: v0.41.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] - repo: https://github.com/pre-commit/mirrors-prettier - rev: v3.0.0-alpha.6 + rev: v4.0.0-alpha.8 hooks: - id: prettier - repo: https://github.com/adrienverge/yamllint - rev: v1.30.0 + rev: v1.35.1 hooks: - id: yamllint @@ -44,29 +44,29 @@ repos: - id: sort-package-xml - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.9.0.2 + rev: v0.10.0.1 hooks: - id: shellcheck - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.6.0-2 + rev: v3.8.0-1 hooks: - id: shfmt args: [-w, -s, -i=4] - repo: https://github.com/pycqa/isort - rev: 5.12.0 + rev: 5.13.2 hooks: - id: isort - repo: https://github.com/psf/black - rev: 23.3.0 + rev: 24.4.2 hooks: - id: black args: [--line-length=100] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v16.0.0 + rev: v18.1.6 hooks: - id: clang-format types_or: [c++, c, cuda] @@ -79,7 +79,7 @@ repos: exclude: .cu - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.23.2 + rev: 0.28.5 hooks: - id: check-metaschema files: ^.+/schema/.*schema\.json$ diff --git a/common/.pages b/common/.pages index 7d17018e2762b..7a87ae2711e10 100644 --- a/common/.pages +++ b/common/.pages @@ -7,16 +7,16 @@ nav: - 'Common Libraries': - 'autoware_auto_common': - 'comparisons': common/autoware_auto_common/design/comparisons + - 'autoware_grid_map_utils': common/autoware_grid_map_utils - 'autoware_point_types': common/autoware_point_types - 'Cuda Utils': common/cuda_utils - 'Geography Utils': common/geography_utils - 'Global Parameter Loader': common/global_parameter_loader/Readme - 'Glog Component': common/glog_component - - 'grid_map_utils': common/grid_map_utils - 'interpolation': common/interpolation - 'Kalman Filter': common/kalman_filter - - 'Motion Utils': common/motion_utils - - 'Vehicle Utils': common/motion_utils/docs/vehicle/vehicle + - 'Motion Utils': common/autoware_motion_utils + - 'Vehicle Utils': common/autoware_motion_utils/docs/vehicle/vehicle - 'Object Recognition Utils': common/object_recognition_utils - 'OSQP Interface': common/osqp_interface/design/osqp_interface-design - 'Perception Utils': common/perception_utils @@ -25,7 +25,7 @@ nav: - 'Introduction': common/signal_processing - 'Butterworth Filter': common/signal_processing/documentation/ButterworthFilter - 'TensorRT Common': common/tensorrt_common - - 'tier4_autoware_utils': common/tier4_autoware_utils + - 'autoware_universe_utils': common/autoware_universe_utils - 'traffic_light_utils': common/traffic_light_utils - 'TVM Utility': - 'Introduction': common/tvm_utility diff --git a/common/README.md b/common/README.md index 95b8973a66b2b..05f85de61286e 100644 --- a/common/README.md +++ b/common/README.md @@ -12,5 +12,5 @@ The Autoware.Universe Common folder consists of common and testing libraries tha Some of the commonly used libraries are: -1. `tier4_autoware_utils` -2. `motion_utils` +1. `autoware_universe_utils` +2. `autoware_motion_utils` diff --git a/common/grid_map_utils/CMakeLists.txt b/common/autoware_grid_map_utils/CMakeLists.txt similarity index 96% rename from common/grid_map_utils/CMakeLists.txt rename to common/autoware_grid_map_utils/CMakeLists.txt index 0fbb34e94e688..677b59a93a514 100644 --- a/common/grid_map_utils/CMakeLists.txt +++ b/common/autoware_grid_map_utils/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(grid_map_utils) +project(autoware_grid_map_utils) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) diff --git a/common/grid_map_utils/README.md b/common/autoware_grid_map_utils/README.md similarity index 76% rename from common/grid_map_utils/README.md rename to common/autoware_grid_map_utils/README.md index f3c6ae5d879be..34adfe230cf78 100644 --- a/common/grid_map_utils/README.md +++ b/common/autoware_grid_map_utils/README.md @@ -19,18 +19,18 @@ More details on the scan line algorithm can be found in the References. ## API -The `grid_map_utils::PolygonIterator` follows the same API as the original [`grid_map::PolygonIterator`](https://docs.ros.org/en/kinetic/api/grid_map_core/html/classgrid__map_1_1PolygonIterator.html). +The `autoware::grid_map_utils::PolygonIterator` follows the same API as the original [`grid_map::PolygonIterator`](https://docs.ros.org/en/kinetic/api/grid_map_core/html/classgrid__map_1_1PolygonIterator.html). ## Assumptions -The behavior of the `grid_map_utils::PolygonIterator` is only guaranteed to match the `grid_map::PolygonIterator` if edges of the polygon do not _exactly_ cross any cell center. +The behavior of the `autoware::grid_map_utils::PolygonIterator` is only guaranteed to match the `grid_map::PolygonIterator` if edges of the polygon do not _exactly_ cross any cell center. In such a case, whether the crossed cell is considered inside or outside of the polygon can vary due to floating precision error. ## Performances -Benchmarking code is implemented in `test/benchmarking.cpp` and is also used to validate that the `grid_map_utils::PolygonIterator` behaves exactly like the `grid_map::PolygonIterator`. +Benchmarking code is implemented in `test/benchmarking.cpp` and is also used to validate that the `autoware::grid_map_utils::PolygonIterator` behaves exactly like the `grid_map::PolygonIterator`. -The following figure shows a comparison of the runtime between the implementation of this package (`grid_map_utils`) and the original implementation (`grid_map`). +The following figure shows a comparison of the runtime between the implementation of this package (`autoware_grid_map_utils`) and the original implementation (`grid_map`). The time measured includes the construction of the iterator and the iteration over all indexes and is shown using a logarithmic scale. Results were obtained varying the side size of a square grid map with `100 <= n <= 1000` (size=`n` means a grid of `n x n` cells), random polygons with a number of vertices `3 <= m <= 100` and with each parameter `(n,m)` repeated 10 times. diff --git a/common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp b/common/autoware_grid_map_utils/include/autoware_grid_map_utils/polygon_iterator.hpp similarity index 95% rename from common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp rename to common/autoware_grid_map_utils/include/autoware_grid_map_utils/polygon_iterator.hpp index 4f2e149a50f72..9bb82e7be50ea 100644 --- a/common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp +++ b/common/autoware_grid_map_utils/include/autoware_grid_map_utils/polygon_iterator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ -#define GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ +#ifndef AUTOWARE_GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ +#define AUTOWARE_GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ #include "grid_map_core/TypeDefs.hpp" @@ -24,7 +24,7 @@ #include #include -namespace grid_map_utils +namespace autoware::grid_map_utils { /// @brief Representation of a polygon edge made of 2 vertices @@ -124,6 +124,6 @@ class PolygonIterator int current_col_; int current_to_col_; }; -} // namespace grid_map_utils +} // namespace autoware::grid_map_utils -#endif // GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ +#endif // AUTOWARE_GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ diff --git a/common/grid_map_utils/media/runtime_comparison.png b/common/autoware_grid_map_utils/media/runtime_comparison.png similarity index 100% rename from common/grid_map_utils/media/runtime_comparison.png rename to common/autoware_grid_map_utils/media/runtime_comparison.png diff --git a/common/grid_map_utils/package.xml b/common/autoware_grid_map_utils/package.xml similarity index 90% rename from common/grid_map_utils/package.xml rename to common/autoware_grid_map_utils/package.xml index 6e63271d56750..d8344b08f2341 100644 --- a/common/grid_map_utils/package.xml +++ b/common/autoware_grid_map_utils/package.xml @@ -1,7 +1,7 @@ - grid_map_utils + autoware_grid_map_utils 0.0.0 Utilities for the grid_map library Maxime CLEMENT @@ -10,10 +10,10 @@ autoware_cmake eigen3_cmake_module + autoware_universe_utils grid_map_core grid_map_cv libopencv-dev - tier4_autoware_utils ament_cmake_gtest ament_lint_auto diff --git a/common/grid_map_utils/src/polygon_iterator.cpp b/common/autoware_grid_map_utils/src/polygon_iterator.cpp similarity index 98% rename from common/grid_map_utils/src/polygon_iterator.cpp rename to common/autoware_grid_map_utils/src/polygon_iterator.cpp index d2a738a971263..1360ad6e6c255 100644 --- a/common/grid_map_utils/src/polygon_iterator.cpp +++ b/common/autoware_grid_map_utils/src/polygon_iterator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "grid_map_utils/polygon_iterator.hpp" +#include "autoware_grid_map_utils/polygon_iterator.hpp" #include "grid_map_core/GridMap.hpp" #include "grid_map_core/Polygon.hpp" @@ -22,7 +22,7 @@ #include #include -namespace grid_map_utils +namespace autoware::grid_map_utils { std::vector PolygonIterator::calculateSortedEdges(const grid_map::Polygon & polygon) @@ -214,4 +214,4 @@ PolygonIterator & PolygonIterator::operator++() { return current_line_ >= intersections_per_line_.size(); } -} // namespace grid_map_utils +} // namespace autoware::grid_map_utils diff --git a/common/grid_map_utils/test/benchmark.cpp b/common/autoware_grid_map_utils/test/benchmark.cpp similarity index 94% rename from common/grid_map_utils/test/benchmark.cpp rename to common/autoware_grid_map_utils/test/benchmark.cpp index c8e352ce5d185..a63ed9985fef1 100644 --- a/common/grid_map_utils/test/benchmark.cpp +++ b/common/autoware_grid_map_utils/test/benchmark.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_grid_map_utils/polygon_iterator.hpp" #include "grid_map_core/TypeDefs.hpp" #include "grid_map_cv/GridMapCvConverter.hpp" #include "grid_map_cv/GridMapCvProcessing.hpp" -#include "grid_map_utils/polygon_iterator.hpp" +#include #include #include #include #include #include #include -#include #include #include @@ -46,7 +46,7 @@ int main(int argc, char * argv[]) result_file << "#Size PolygonVertices PolygonIndexes grid_map_utils_constructor grid_map_utils_iteration " "grid_map_constructor grid_map_iteration\n"; - tier4_autoware_utils::StopWatch stopwatch; + autoware::universe_utils::StopWatch stopwatch; constexpr auto nb_iterations = 10; constexpr auto polygon_side_vertices = @@ -116,7 +116,7 @@ int main(int argc, char * argv[]) polygon.addVertex(base_polygon.getVertex(idx) + offset); } stopwatch.tic("gmu_ctor"); - grid_map_utils::PolygonIterator grid_map_utils_iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator grid_map_utils_iterator(map, polygon); grid_map_utils_constructor_duration += stopwatch.toc("gmu_ctor"); stopwatch.tic("gm_ctor"); grid_map::PolygonIterator grid_map_iterator(map, polygon); @@ -143,8 +143,8 @@ int main(int argc, char * argv[]) if (diff || visualize) { // Prepare images of the cells selected by the two PolygonIterators auto gridmap = map; - for (grid_map_utils::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); - ++iterator) + for (autoware::grid_map_utils::PolygonIterator iterator(map, polygon); + !iterator.isPastEnd(); ++iterator) map.at("layer", *iterator) = 100; for (grid_map::PolygonIterator iterator(gridmap, polygon); !iterator.isPastEnd(); ++iterator) diff --git a/common/grid_map_utils/test/test_polygon_iterator.cpp b/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp similarity index 92% rename from common/grid_map_utils/test/test_polygon_iterator.cpp rename to common/autoware_grid_map_utils/test/test_polygon_iterator.cpp index f39d080d7cad5..1646586853800 100644 --- a/common/grid_map_utils/test/test_polygon_iterator.cpp +++ b/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "grid_map_utils/polygon_iterator.hpp" +#include "autoware_grid_map_utils/polygon_iterator.hpp" +#include #include -#include // gtest #include @@ -45,7 +45,7 @@ TEST(PolygonIterator, FullCover) polygon.addVertex(Position(100.0, -100.0)); polygon.addVertex(Position(-100.0, -100.0)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(0, (*iterator)(0)); @@ -83,7 +83,7 @@ TEST(PolygonIterator, Outside) polygon.addVertex(Position(101.0, 99.0)); polygon.addVertex(Position(99.0, 99.0)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); EXPECT_TRUE(iterator.isPastEnd()); } @@ -100,7 +100,7 @@ TEST(PolygonIterator, Square) polygon.addVertex(Position(1.0, -1.5)); polygon.addVertex(Position(-1.0, -1.5)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(3, (*iterator)(0)); @@ -146,7 +146,7 @@ TEST(PolygonIterator, TopLeftTriangle) polygon.addVertex(Position(40.1, 20.4)); polygon.addVertex(Position(-40.1, -20.6)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(0, (*iterator)(0)); @@ -170,7 +170,7 @@ TEST(PolygonIterator, MoveMap) polygon.addVertex(Position(0.9, 1.6)); polygon.addVertex(Position(0.9, -1.6)); polygon.addVertex(Position(6.1, -1.6)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(6, (*iterator)(0)); @@ -213,7 +213,7 @@ TEST(PolygonIterator, Difference) polygon.addVertex(Position(2.5, 2.5)); polygon.addVertex(Position(-2.5, 2.5)); polygon.addVertex(Position(-2.5, -2.5)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); grid_map::PolygonIterator gm_iterator(map, polygon); bool diff = false; while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) { @@ -231,7 +231,7 @@ TEST(PolygonIterator, Difference) polygon.addVertex(Position(2.5, 2.1)); polygon.addVertex(Position(-2.5, 2.5)); polygon.addVertex(Position(-2.5, -2.9)); - iterator = grid_map_utils::PolygonIterator(map, polygon); + iterator = autoware::grid_map_utils::PolygonIterator(map, polygon); gm_iterator = grid_map::PolygonIterator(map, polygon); diff = false; while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) { @@ -256,7 +256,7 @@ TEST(PolygonIterator, SelfCrossingPolygon) polygon.addVertex(Position(2.5, -2.9)); polygon.addVertex(Position(-2.5, 2.5)); polygon.addVertex(Position(-2.5, -2.5)); - grid_map_utils::PolygonIterator iterator(map, polygon); + autoware::grid_map_utils::PolygonIterator iterator(map, polygon); grid_map::PolygonIterator gm_iterator(map, polygon); const std::vector expected_indexes = { diff --git a/common/motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt similarity index 55% rename from common/motion_utils/CMakeLists.txt rename to common/autoware_motion_utils/CMakeLists.txt index cd81f16685d72..4c36ef2f4e70d 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/autoware_motion_utils/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.14) -project(motion_utils) +project(autoware_motion_utils) find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) -ament_auto_add_library(motion_utils SHARED +ament_auto_add_library(autoware_motion_utils SHARED DIRECTORY src ) @@ -15,10 +15,10 @@ if(BUILD_TESTING) file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(test_motion_utils ${test_files}) + ament_add_ros_isolated_gtest(test_autoware_motion_utils ${test_files}) - target_link_libraries(test_motion_utils - motion_utils + target_link_libraries(test_autoware_motion_utils + autoware_motion_utils ) endif() diff --git a/common/motion_utils/README.md b/common/autoware_motion_utils/README.md similarity index 97% rename from common/motion_utils/README.md rename to common/autoware_motion_utils/README.md index fd18540a9d0cf..df4808a6f68d7 100644 --- a/common/motion_utils/README.md +++ b/common/autoware_motion_utils/README.md @@ -114,4 +114,4 @@ const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_ Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. -`motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. +`autoware_motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/motion_utils/docs/vehicle/vehicle.md b/common/autoware_motion_utils/docs/vehicle/vehicle.md similarity index 96% rename from common/motion_utils/docs/vehicle/vehicle.md rename to common/autoware_motion_utils/docs/vehicle/vehicle.md index c768e6115731c..32cbf6be28dcc 100644 --- a/common/motion_utils/docs/vehicle/vehicle.md +++ b/common/autoware_motion_utils/docs/vehicle/vehicle.md @@ -36,7 +36,7 @@ bool isVehicleStopped(const double stop_duration) Necessary includes: ```c++ -#include +#include ``` 1.Create a checker instance. @@ -116,7 +116,7 @@ bool isVehicleStoppedAtStopPoint(const double stop_duration) Necessary includes: ```c++ -#include +#include ``` 1.Create a checker instance. diff --git a/common/motion_utils/include/motion_utils/constants.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp similarity index 74% rename from common/motion_utils/include/motion_utils/constants.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp index 63a3439a34b8a..9c1f7c876e73e 100644 --- a/common/motion_utils/include/motion_utils/constants.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__CONSTANTS_HPP_ -#define MOTION_UTILS__CONSTANTS_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ +#define AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ -namespace motion_utils +namespace autoware::motion_utils { constexpr double overlap_threshold = 0.1; -} // namespace motion_utils +} // namespace autoware::motion_utils -#endif // MOTION_UTILS__CONSTANTS_HPP_ +#endif // AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ diff --git a/common/motion_utils/include/motion_utils/distance/distance.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp similarity index 79% rename from common/motion_utils/include/motion_utils/distance/distance.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp index 0a0d00fc9f260..308528954eccc 100644 --- a/common/motion_utils/include/motion_utils/distance/distance.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__DISTANCE__DISTANCE_HPP_ -#define MOTION_UTILS__DISTANCE__DISTANCE_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ +#define AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ #include #include @@ -22,12 +22,12 @@ #include #include -namespace motion_utils +namespace autoware::motion_utils { std::optional calcDecelDistWithJerkAndAccConstraints( const double current_vel, const double target_vel, const double current_acc, const double acc_min, const double jerk_acc, const double jerk_dec); -} // namespace motion_utils +} // namespace autoware::motion_utils -#endif // MOTION_UTILS__DISTANCE__DISTANCE_HPP_ +#endif // AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ diff --git a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp similarity index 85% rename from common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp index 077c66f9dd6bc..30eca6927db34 100644 --- a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ -#define MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ +#define AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ #include #include @@ -24,7 +24,7 @@ #include #include -namespace motion_utils +namespace autoware::motion_utils { using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; @@ -49,6 +49,6 @@ class VelocityFactorInterface VelocityFactor velocity_factor_{}; }; -} // namespace motion_utils +} // namespace autoware::motion_utils -#endif // MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ +#endif // AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp similarity index 88% rename from common/motion_utils/include/motion_utils/marker/marker_helper.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp index 0604eee65372a..4679e3d9aba91 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ -#define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ +#define AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ #include @@ -21,7 +21,7 @@ #include -namespace motion_utils +namespace autoware::motion_utils { using geometry_msgs::msg::Pose; @@ -48,6 +48,6 @@ visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker( visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( const rclcpp::Time & now, const int32_t id); -} // namespace motion_utils +} // namespace autoware::motion_utils -#endif // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ +#endif // AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ diff --git a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp similarity index 89% rename from common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp index be5a70ed7ffc9..07fcbd9840c88 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ -#define MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ +#define AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ #include @@ -25,7 +25,7 @@ #include #include -namespace motion_utils +namespace autoware::motion_utils { /// @brief type of virtual wall associated with different marker styles and namespace @@ -76,6 +76,6 @@ class VirtualWallMarkerCreator /// @param now current time to be used for displaying the markers visualization_msgs::msg::MarkerArray create_markers(const rclcpp::Time & now = rclcpp::Time()); }; -} // namespace motion_utils +} // namespace autoware::motion_utils -#endif // MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ +#endif // AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp similarity index 98% rename from common/motion_utils/include/motion_utils/resample/resample.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp index 91322360ce5b8..19328179932c4 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ +#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -21,7 +21,7 @@ #include -namespace motion_utils +namespace autoware::motion_utils { /** * @brief A resampling function for a path(points). Note that in a default setting, position xy are @@ -234,6 +234,6 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true, const bool resample_input_trajectory_stop_point = true); -} // namespace motion_utils +} // namespace autoware::motion_utils -#endif // MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ +#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp similarity index 69% rename from common/motion_utils/include/motion_utils/resample/resample_utils.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp index fb151ec56b085..5d622c54da452 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ -#define MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ -#include "tier4_autoware_utils/system/backtrace.hpp" +#include "autoware/universe_utils/system/backtrace.hpp" -#include -#include -#include +#include +#include +#include #include @@ -29,7 +29,7 @@ constexpr double close_s_threshold = 1e-6; static inline rclcpp::Logger get_logger() { - constexpr const char * logger{"motion_utils.resample_utils"}; + constexpr const char * logger{"autoware_motion_utils.resample_utils"}; return rclcpp::get_logger(logger); } @@ -42,7 +42,7 @@ bool validate_size(const T & points) template bool validate_resampling_range(const T & points, const std::vector & resampling_intervals) { - const double points_length = motion_utils::calcArcLength(points); + const double points_length = autoware::motion_utils::calcArcLength(points); return points_length >= resampling_intervals.back(); } @@ -50,9 +50,9 @@ template bool validate_points_duplication(const T & points) { for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & curr_pt = tier4_autoware_utils::getPoint(points.at(i)); - const auto & next_pt = tier4_autoware_utils::getPoint(points.at(i + 1)); - const double ds = tier4_autoware_utils::calcDistance2d(curr_pt, next_pt); + const auto & curr_pt = autoware::universe_utils::getPoint(points.at(i)); + const auto & next_pt = autoware::universe_utils::getPoint(points.at(i + 1)); + const double ds = autoware::universe_utils::calcDistance2d(curr_pt, next_pt); if (ds < close_s_threshold) { return false; } @@ -67,27 +67,27 @@ bool validate_arguments(const T & input_points, const std::vector & resa // Check size of the arguments if (!validate_size(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } if (!validate_size(resampling_intervals)) { RCLCPP_DEBUG( get_logger(), "invalid argument: The number of resampling intervals is less than 2"); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } // Check resampling range if (!validate_resampling_range(input_points, resampling_intervals)) { RCLCPP_DEBUG(get_logger(), "invalid argument: resampling interval is longer than input points"); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } @@ -100,23 +100,23 @@ bool validate_arguments(const T & input_points, const double resampling_interval // Check size of the arguments if (!validate_size(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } // check resampling interval - if (resampling_interval < motion_utils::overlap_threshold) { + if (resampling_interval < autoware::motion_utils::overlap_threshold) { RCLCPP_DEBUG( get_logger(), "invalid argument: resampling interval is less than %f", - motion_utils::overlap_threshold); - tier4_autoware_utils::print_backtrace(); + autoware::motion_utils::overlap_threshold); + autoware::universe_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); return false; } @@ -124,4 +124,4 @@ bool validate_arguments(const T & input_points, const double resampling_interval } } // namespace resample_utils -#endif // MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp similarity index 94% rename from common/motion_utils/include/motion_utils/trajectory/conversion.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp index 8d009730a925d..d4f88c17c4d70 100644 --- a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ -#define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ #include "autoware_planning_msgs/msg/detail/path__struct.hpp" #include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp" @@ -23,7 +23,7 @@ #include -namespace motion_utils +namespace autoware::motion_utils { using TrajectoryPoints = std::vector; @@ -115,6 +115,6 @@ inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( return output; } -} // namespace motion_utils +} // namespace autoware::motion_utils -#endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp similarity index 79% rename from common/motion_utils/include/motion_utils/trajectory/interpolation.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp index 0a0b14cb7488d..5272478cccd78 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ -#define MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "tier4_planning_msgs/msg/path_with_lane_id.hpp" @@ -25,7 +25,7 @@ #include #include -namespace motion_utils +namespace autoware::motion_utils { /** * @brief An interpolation function that finds the closest interpolated point on the trajectory from @@ -73,24 +73,24 @@ geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double tar } if (points.size() < 2 || target_length < 0.0) { - return tier4_autoware_utils::getPose(points.front()); + return autoware::universe_utils::getPose(points.front()); } double accumulated_length = 0; for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & curr_pose = tier4_autoware_utils::getPose(points.at(i)); - const auto & next_pose = tier4_autoware_utils::getPose(points.at(i + 1)); - const double length = tier4_autoware_utils::calcDistance3d(curr_pose, next_pose); + const auto & curr_pose = autoware::universe_utils::getPose(points.at(i)); + const auto & next_pose = autoware::universe_utils::getPose(points.at(i + 1)); + const double length = autoware::universe_utils::calcDistance3d(curr_pose, next_pose); if (accumulated_length + length > target_length) { const double ratio = (target_length - accumulated_length) / std::max(length, 1e-6); - return tier4_autoware_utils::calcInterpolatedPose(curr_pose, next_pose, ratio); + return autoware::universe_utils::calcInterpolatedPose(curr_pose, next_pose, ratio); } accumulated_length += length; } - return tier4_autoware_utils::getPose(points.back()); + return autoware::universe_utils::getPose(points.back()); } -} // namespace motion_utils +} // namespace autoware::motion_utils -#endif // MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp similarity index 86% rename from common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp index d6d4fbf559323..0d875e636bad5 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include #include #include -namespace motion_utils +namespace autoware::motion_utils { std::optional> getPathIndexRangeWithLaneId( const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); @@ -41,6 +41,6 @@ size_t findNearestSegmentIndexFromLaneId( tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation = true); -} // namespace motion_utils +} // namespace autoware::motion_utils -#endif // MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp similarity index 90% rename from common/motion_utils/include/motion_utils/trajectory/trajectory.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index 3715f466e7684..da37d04550f5e 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#define MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/system/backtrace.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/system/backtrace.hpp" #include #include @@ -35,11 +35,11 @@ #include #include -namespace motion_utils +namespace autoware::motion_utils { static inline rclcpp::Logger get_logger() { - constexpr const char * logger{"motion_utils.trajectory"}; + constexpr const char * logger{"autoware_motion_utils.trajectory"}; return rclcpp::get_logger(logger); } @@ -51,8 +51,8 @@ template void validateNonEmpty(const T & points) { if (points.empty()) { - tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("[motion_utils] validateNonEmpty(): Points is empty."); + autoware::universe_utils::print_backtrace(); + throw std::invalid_argument("[autoware_motion_utils] validateNonEmpty(): Points is empty."); } } @@ -72,23 +72,24 @@ extern template void validateNonEmpty void validateNonSharpAngle( const T & point1, const T & point2, const T & point3, - const double angle_threshold = tier4_autoware_utils::pi / 4) + const double angle_threshold = autoware::universe_utils::pi / 4) { - const auto p1 = tier4_autoware_utils::getPoint(point1); - const auto p2 = tier4_autoware_utils::getPoint(point2); - const auto p3 = tier4_autoware_utils::getPoint(point3); + const auto p1 = autoware::universe_utils::getPoint(point1); + const auto p2 = autoware::universe_utils::getPoint(point2); + const auto p3 = autoware::universe_utils::getPoint(point3); const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; const auto product = std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2); - const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); + const auto dist_1to2 = autoware::universe_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = autoware::universe_utils::calcDistance3d(p3, p2); constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { - tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("[motion_utils] validateNonSharpAngle(): Too sharp angle."); + autoware::universe_utils::print_backtrace(); + throw std::invalid_argument( + "[autoware_motion_utils] validateNonSharpAngle(): Too sharp angle."); } } @@ -105,10 +106,10 @@ std::optional isDrivingForward(const T & points) } // check the first point direction - const auto & first_pose = tier4_autoware_utils::getPose(points.at(0)); - const auto & second_pose = tier4_autoware_utils::getPose(points.at(1)); + const auto & first_pose = autoware::universe_utils::getPose(points.at(0)); + const auto & second_pose = autoware::universe_utils::getPose(points.at(1)); - return tier4_autoware_utils::isDrivingForward(first_pose, second_pose); + return autoware::universe_utils::isDrivingForward(first_pose, second_pose); } extern template std::optional @@ -134,10 +135,10 @@ std::optional isDrivingForwardWithTwist(const T & points_with_twist) return std::nullopt; } if (points_with_twist.size() == 1) { - if (0.0 < tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { + if (0.0 < autoware::universe_utils::getLongitudinalVelocity(points_with_twist.front())) { return true; } - if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { + if (0.0 > autoware::universe_utils::getLongitudinalVelocity(points_with_twist.front())) { return false; } return std::nullopt; @@ -181,8 +182,8 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) constexpr double eps = 1.0E-08; for (size_t i = start_idx + 1; i < points.size(); ++i) { - const auto prev_p = tier4_autoware_utils::getPoint(dst.back()); - const auto curr_p = tier4_autoware_utils::getPoint(points.at(i)); + const auto prev_p = autoware::universe_utils::getPoint(dst.back()); + const auto curr_p = autoware::universe_utils::getPoint(points.at(i)); if (std::abs(prev_p.x - curr_p.x) < eps && std::abs(prev_p.y - curr_p.y) < eps) { continue; } @@ -298,7 +299,7 @@ size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & poin size_t min_idx = 0; for (size_t i = 0; i < points.size(); ++i) { - const auto dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), point); + const auto dist = autoware::universe_utils::calcSquaredDistance2d(points.at(i), point); if (dist < min_dist) { min_dist = dist; min_idx = i; @@ -350,13 +351,13 @@ std::optional findNearestIndex( size_t min_idx = 0; for (size_t i = 0; i < points.size(); ++i) { - const auto squared_dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose); + const auto squared_dist = autoware::universe_utils::calcSquaredDistance2d(points.at(i), pose); if (squared_dist > max_squared_dist || squared_dist >= min_squared_dist) { continue; } - const auto yaw = - tier4_autoware_utils::calcYawDeviation(tier4_autoware_utils::getPose(points.at(i)), pose); + const auto yaw = autoware::universe_utils::calcYawDeviation( + autoware::universe_utils::getPose(points.at(i)), pose); if (std::fabs(yaw) > max_yaw) { continue; } @@ -405,10 +406,10 @@ double calcLongitudinalOffsetToSegment( { if (seg_idx >= points.size() - 1) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + ": Failed to calculate longitudinal offset because the given segment index is out of the " "points size."); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -434,9 +435,9 @@ double calcLongitudinalOffsetToSegment( if (seg_idx >= overlap_removed_points.size() - 1) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + ": Longitudinal offset calculation is not supported for the same points."); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -447,8 +448,8 @@ double calcLongitudinalOffsetToSegment( return std::nan(""); } - const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)); - const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); + const auto p_front = autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx)); + const auto p_back = autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0}; @@ -596,9 +597,9 @@ double calcLateralOffset( if (overlap_removed_points.size() == 1) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + ": Lateral offset calculation is not supported for the same points."); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -613,8 +614,8 @@ double calcLateralOffset( const auto p_front_idx = (p_indices > seg_idx) ? seg_idx : p_indices; const auto p_back_idx = p_front_idx + 1; - const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_front_idx)); - const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_back_idx)); + const auto p_front = autoware::universe_utils::getPoint(overlap_removed_points.at(p_front_idx)); + const auto p_back = autoware::universe_utils::getPoint(overlap_removed_points.at(p_back_idx)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; @@ -669,9 +670,9 @@ double calcLateralOffset( if (overlap_removed_points.size() == 1) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + ": Lateral offset calculation is not supported for the same points."); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -723,7 +724,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t double dist_sum = 0.0; for (size_t i = src_idx; i < dst_idx; ++i) { - dist_sum += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + dist_sum += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); } return dist_sum; } @@ -771,7 +772,7 @@ std::vector calcSignedArcLengthPartialSum( double dist_sum = 0.0; partial_dist.push_back(dist_sum); for (size_t i = src_idx; i < dst_idx - 1; ++i) { - dist_sum += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + dist_sum += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); partial_dist.push_back(dist_sum); } return partial_dist; @@ -958,10 +959,10 @@ std::vector calcCurvature(const T & points) } for (size_t i = 1; i < points.size() - 1; ++i) { - const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); - const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); - const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1)); - curvature_vec.at(i) = (tier4_autoware_utils::calcCurvature(p1, p2, p3)); + const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1)); + const auto p2 = autoware::universe_utils::getPoint(points.at(i)); + const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1)); + curvature_vec.at(i) = (autoware::universe_utils::calcCurvature(p1, p2, p3)); } curvature_vec.at(0) = curvature_vec.at(1); curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2); @@ -996,12 +997,13 @@ std::vector> calcCurvatureAndArcLength(const T & point std::vector> curvature_arc_length_vec; curvature_arc_length_vec.emplace_back(0.0, 0.0); for (size_t i = 1; i < points.size() - 1; ++i) { - const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); - const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); - const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1)); - const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3); - const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) + - tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1)); + const auto p2 = autoware::universe_utils::getPoint(points.at(i)); + const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1)); + const double curvature = autoware::universe_utils::calcCurvature(p1, p2, p3); + const double arc_length = + autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)) + + autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); curvature_arc_length_vec.emplace_back(curvature, arc_length); } curvature_arc_length_vec.emplace_back(0.0, 0.0); @@ -1073,10 +1075,10 @@ std::optional calcLongitudinalOffsetPoint( if (points.size() - 1 < src_idx) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + " error: The given source index is out of the points size. Failed to calculate longitudinal " "offset."); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -1092,7 +1094,7 @@ std::optional calcLongitudinalOffsetPoint( } if (src_idx + 1 == points.size() && offset == 0.0) { - return tier4_autoware_utils::getPoint(points.at(src_idx)); + return autoware::universe_utils::getPoint(points.at(src_idx)); } if (offset < 0.0) { @@ -1108,12 +1110,12 @@ std::optional calcLongitudinalOffsetPoint( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware::universe_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPoint( + return autoware::universe_utils::calcInterpolatedPoint( p_back, p_front, std::abs(dist_res / dist_segment)); } } @@ -1204,10 +1206,10 @@ std::optional calcLongitudinalOffsetPose( if (points.size() - 1 < src_idx) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + " error: The given source index is out of the points size. Failed to calculate longitudinal " "offset."); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -1221,7 +1223,7 @@ std::optional calcLongitudinalOffsetPose( } if (src_idx + 1 == points.size() && offset == 0.0) { - return tier4_autoware_utils::getPose(points.at(src_idx)); + return autoware::universe_utils::getPose(points.at(src_idx)); } if (offset < 0.0) { @@ -1234,12 +1236,12 @@ std::optional calcLongitudinalOffsetPose( const auto & p_front = reverse_points.at(i); const auto & p_back = reverse_points.at(i + 1); - const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware::universe_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = -offset - dist_sum; if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPose( + return autoware::universe_utils::calcInterpolatedPose( p_back, p_front, std::abs(dist_res / dist_segment), set_orientation_from_position_direction); } @@ -1251,12 +1253,12 @@ std::optional calcLongitudinalOffsetPose( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware::universe_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPose( + return autoware::universe_utils::calcInterpolatedPose( p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), set_orientation_from_position_direction); } @@ -1356,8 +1358,8 @@ std::optional insertTargetPoint( return {}; } - const auto p_front = tier4_autoware_utils::getPoint(points.at(seg_idx)); - const auto p_back = tier4_autoware_utils::getPoint(points.at(seg_idx + 1)); + const auto p_front = autoware::universe_utils::getPoint(points.at(seg_idx)); + const auto p_back = autoware::universe_utils::getPoint(points.at(seg_idx + 1)); try { validateNonSharpAngle(p_front, p_target, p_back); @@ -1367,9 +1369,9 @@ std::optional insertTargetPoint( } const auto overlap_with_front = - tier4_autoware_utils::calcDistance2d(p_target, p_front) < overlap_threshold; + autoware::universe_utils::calcDistance2d(p_target, p_front) < overlap_threshold; const auto overlap_with_back = - tier4_autoware_utils::calcDistance2d(p_target, p_back) < overlap_threshold; + autoware::universe_utils::calcDistance2d(p_target, p_back) < overlap_threshold; const auto is_driving_forward = isDrivingForward(points); if (!is_driving_forward) { @@ -1379,31 +1381,31 @@ std::optional insertTargetPoint( geometry_msgs::msg::Pose target_pose; { const auto p_base = is_driving_forward.value() ? p_back : p_front; - const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_base); - const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_base); + const auto pitch = autoware::universe_utils::calcElevationAngle(p_target, p_base); + const auto yaw = autoware::universe_utils::calcAzimuthAngle(p_target, p_base); target_pose.position = p_target; - target_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + target_pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); } auto p_insert = points.at(seg_idx); - tier4_autoware_utils::setPose(target_pose, p_insert); + autoware::universe_utils::setPose(target_pose, p_insert); geometry_msgs::msg::Pose base_pose; { const auto p_base = is_driving_forward.value() ? p_front : p_back; - const auto pitch = tier4_autoware_utils::calcElevationAngle(p_base, p_target); - const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_base, p_target); + const auto pitch = autoware::universe_utils::calcElevationAngle(p_base, p_target); + const auto yaw = autoware::universe_utils::calcAzimuthAngle(p_base, p_target); - base_pose.position = tier4_autoware_utils::getPoint(p_base); - base_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + base_pose.position = autoware::universe_utils::getPoint(p_base); + base_pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); } if (!overlap_with_front && !overlap_with_back) { if (is_driving_forward.value()) { - tier4_autoware_utils::setPose(base_pose, points.at(seg_idx)); + autoware::universe_utils::setPose(base_pose, points.at(seg_idx)); } else { - tier4_autoware_utils::setPose(base_pose, points.at(seg_idx + 1)); + autoware::universe_utils::setPose(base_pose, points.at(seg_idx + 1)); } points.insert(points.begin() + seg_idx + 1, p_insert); return seg_idx + 1; @@ -1537,9 +1539,9 @@ std::optional insertTargetPoint( const double target_length = insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx); const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0); - const auto p_target = tier4_autoware_utils::calcInterpolatedPoint( - tier4_autoware_utils::getPoint(points.at(*segment_idx)), - tier4_autoware_utils::getPoint(points.at(*segment_idx + 1)), ratio); + const auto p_target = autoware::universe_utils::calcInterpolatedPoint( + autoware::universe_utils::getPoint(points.at(*segment_idx)), + autoware::universe_utils::getPoint(points.at(*segment_idx + 1)), ratio); return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } @@ -1643,7 +1645,7 @@ std::optional insertStopPoint( } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return stop_idx; @@ -1685,9 +1687,9 @@ std::optional insertStopPoint( double accumulated_length = 0; for (size_t i = 0; i < points_with_twist.size() - 1; ++i) { - const auto curr_pose = tier4_autoware_utils::getPose(points_with_twist.at(i)); - const auto next_pose = tier4_autoware_utils::getPose(points_with_twist.at(i + 1)); - const double length = tier4_autoware_utils::calcDistance2d(curr_pose, next_pose); + const auto curr_pose = autoware::universe_utils::getPose(points_with_twist.at(i)); + const auto next_pose = autoware::universe_utils::getPose(points_with_twist.at(i + 1)); + const double length = autoware::universe_utils::calcDistance2d(curr_pose, next_pose); if (accumulated_length + length + overlap_threshold > distance_to_stop_point) { const double insert_length = distance_to_stop_point - accumulated_length; return insertStopPoint(i, insert_length, points_with_twist, overlap_threshold); @@ -1747,7 +1749,7 @@ std::optional insertStopPoint( } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return stop_idx; @@ -1786,15 +1788,15 @@ std::optional insertStopPoint( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) { - const auto insert_idx = - motion_utils::insertTargetPoint(stop_seg_idx, stop_point, points_with_twist, overlap_threshold); + const auto insert_idx = autoware::motion_utils::insertTargetPoint( + stop_seg_idx, stop_point, points_with_twist, overlap_threshold); if (!insert_idx) { return std::nullopt; } for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { - tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return insert_idx; @@ -1834,8 +1836,8 @@ std::optional insertDecelPoint( for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { const auto & original_velocity = - tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.at(i)); - tier4_autoware_utils::setLongitudinalVelocity( + autoware::universe_utils::getLongitudinalVelocity(points_with_twist.at(i)); + autoware::universe_utils::setLongitudinalVelocity( std::min(original_velocity, velocity), points_with_twist.at(i)); } @@ -1858,30 +1860,30 @@ void insertOrientation(T & points, const bool is_driving_forward) { if (is_driving_forward) { for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & src_point = tier4_autoware_utils::getPoint(points.at(i)); - const auto & dst_point = tier4_autoware_utils::getPoint(points.at(i + 1)); - const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); - tier4_autoware_utils::setOrientation( - tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); + const auto & src_point = autoware::universe_utils::getPoint(points.at(i)); + const auto & dst_point = autoware::universe_utils::getPoint(points.at(i + 1)); + const double pitch = autoware::universe_utils::calcElevationAngle(src_point, dst_point); + const double yaw = autoware::universe_utils::calcAzimuthAngle(src_point, dst_point); + autoware::universe_utils::setOrientation( + autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); if (i == points.size() - 2) { // Terminal orientation is same as the point before it - tier4_autoware_utils::setOrientation( - tier4_autoware_utils::getPose(points.at(i)).orientation, points.at(i + 1)); + autoware::universe_utils::setOrientation( + autoware::universe_utils::getPose(points.at(i)).orientation, points.at(i + 1)); } } } else { for (size_t i = points.size() - 1; i >= 1; --i) { - const auto & src_point = tier4_autoware_utils::getPoint(points.at(i)); - const auto & dst_point = tier4_autoware_utils::getPoint(points.at(i - 1)); - const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); - tier4_autoware_utils::setOrientation( - tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); + const auto & src_point = autoware::universe_utils::getPoint(points.at(i)); + const auto & dst_point = autoware::universe_utils::getPoint(points.at(i - 1)); + const double pitch = autoware::universe_utils::calcElevationAngle(src_point, dst_point); + const double yaw = autoware::universe_utils::calcAzimuthAngle(src_point, dst_point); + autoware::universe_utils::setOrientation( + autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); } // Initial orientation is same as the point after it - tier4_autoware_utils::setOrientation( - tier4_autoware_utils::getPose(points.at(1)).orientation, points.at(0)); + autoware::universe_utils::setOrientation( + autoware::universe_utils::getPose(points.at(1)).orientation, points.at(0)); } } @@ -1906,14 +1908,14 @@ template void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) { for (auto itr = points.begin(); std::next(itr) != points.end();) { - const auto p1 = tier4_autoware_utils::getPose(*itr); - const auto p2 = tier4_autoware_utils::getPose(*std::next(itr)); + const auto p1 = autoware::universe_utils::getPose(*itr); + const auto p2 = autoware::universe_utils::getPose(*std::next(itr)); const double yaw1 = tf2::getYaw(p1.orientation); const double yaw2 = tf2::getYaw(p2.orientation); if ( - max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) || - !tier4_autoware_utils::isDrivingForward(p1, p2)) { + max_yaw_diff < std::abs(autoware::universe_utils::normalizeRadian(yaw1 - yaw2)) || + !autoware::universe_utils::isDrivingForward(p1, p2)) { points.erase(std::next(itr)); return; } else { @@ -2066,9 +2068,9 @@ size_t findFirstNearestIndexWithSoftConstraints( bool is_within_constraints = false; for (size_t i = 0; i < points.size(); ++i) { const auto squared_dist = - tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose.position); - const auto yaw = - tier4_autoware_utils::calcYawDeviation(tier4_autoware_utils::getPose(points.at(i)), pose); + autoware::universe_utils::calcSquaredDistance2d(points.at(i), pose.position); + const auto yaw = autoware::universe_utils::calcYawDeviation( + autoware::universe_utils::getPose(points.at(i)), pose); if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) { if (is_within_constraints) { @@ -2099,7 +2101,7 @@ size_t findFirstNearestIndexWithSoftConstraints( bool is_within_constraints = false; for (size_t i = 0; i < points.size(); ++i) { const auto squared_dist = - tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose.position); + autoware::universe_utils::calcSquaredDistance2d(points.at(i), pose.position); if (squared_dist_threshold < squared_dist) { if (is_within_constraints) { @@ -2229,13 +2231,13 @@ std::optional calcDistanceToForwardStopPoint( } const auto nearest_segment_idx = - motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); + autoware::motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); if (!nearest_segment_idx) { return std::nullopt; } - const auto stop_idx = motion_utils::searchZeroVelocityIndex( + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex( points_with_twist, *nearest_segment_idx + 1, points_with_twist.size()); if (!stop_idx) { @@ -2276,9 +2278,9 @@ T cropForwardPoints( } double sum_length = - -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + -autoware::motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (forward_length < sum_length) { const size_t end_idx = i; return T{points.begin(), points.begin() + end_idx}; @@ -2316,9 +2318,9 @@ T cropBackwardPoints( } double sum_length = - -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + -autoware::motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); for (int i = target_seg_idx; 0 < i; --i) { - sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < -backward_length) { const size_t begin_idx = i; return T{points.begin() + begin_idx, points.end()}; @@ -2415,9 +2417,9 @@ double calcYawDeviation( if (overlap_removed_points.size() <= 1) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + " Given points size is less than 2. Failed to calculate yaw deviation."); - tier4_autoware_utils::print_backtrace(); + autoware::universe_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -2430,12 +2432,12 @@ double calcYawDeviation( const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, pose.position); - const double path_yaw = tier4_autoware_utils::calcAzimuthAngle( - tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)), - tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1))); + const double path_yaw = autoware::universe_utils::calcAzimuthAngle( + autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx)), + autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx + 1))); const double pose_yaw = tf2::getYaw(pose.orientation); - return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw); + return autoware::universe_utils::normalizeRadian(pose_yaw - path_yaw); } extern template double calcYawDeviation>( @@ -2485,6 +2487,6 @@ extern template bool isTargetPointFront @@ -23,7 +23,7 @@ #include -namespace motion_utils +namespace autoware::motion_utils { using autoware_planning_msgs::msg::Trajectory; @@ -77,6 +77,6 @@ class VehicleArrivalChecker : public VehicleStopChecker void onTrajectory(const Trajectory::ConstSharedPtr msg); }; -} // namespace motion_utils +} // namespace autoware::motion_utils -#endif // MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ +#endif // AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ diff --git a/common/motion_utils/media/ego_nearest_search.svg b/common/autoware_motion_utils/media/ego_nearest_search.svg similarity index 100% rename from common/motion_utils/media/ego_nearest_search.svg rename to common/autoware_motion_utils/media/ego_nearest_search.svg diff --git a/common/motion_utils/media/segment.svg b/common/autoware_motion_utils/media/segment.svg similarity index 100% rename from common/motion_utils/media/segment.svg rename to common/autoware_motion_utils/media/segment.svg diff --git a/common/motion_utils/package.xml b/common/autoware_motion_utils/package.xml similarity index 92% rename from common/motion_utils/package.xml rename to common/autoware_motion_utils/package.xml index 21471a4a6a75e..81fec78b04812 100644 --- a/common/motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -1,9 +1,9 @@ - motion_utils + autoware_motion_utils 0.1.0 - The motion_utils package + The autoware_motion_utils package Satoshi Ota Takayuki Murooka @@ -23,6 +23,7 @@ autoware_adapi_v1_msgs autoware_planning_msgs + autoware_universe_utils autoware_vehicle_msgs builtin_interfaces geometry_msgs @@ -31,7 +32,6 @@ rclcpp tf2 tf2_geometry_msgs - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/common/motion_utils/src/distance/distance.cpp b/common/autoware_motion_utils/src/distance/distance.cpp similarity index 98% rename from common/motion_utils/src/distance/distance.cpp rename to common/autoware_motion_utils/src/distance/distance.cpp index 3ac37bbc7733b..d31e7ec709810 100644 --- a/common/motion_utils/src/distance/distance.cpp +++ b/common/autoware_motion_utils/src/distance/distance.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/distance/distance.hpp" +#include "autoware/motion_utils/distance/distance.hpp" -namespace motion_utils +namespace autoware::motion_utils { namespace { @@ -269,4 +269,4 @@ std::optional calcDecelDistWithJerkAndAccConstraints( return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); } -} // namespace motion_utils +} // namespace autoware::motion_utils diff --git a/common/motion_utils/src/factor/velocity_factor_interface.cpp b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp similarity index 86% rename from common/motion_utils/src/factor/velocity_factor_interface.cpp rename to common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp index 6878c056b6f7a..e405cdb655c02 100644 --- a/common/motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include #include #include -namespace motion_utils +namespace autoware::motion_utils { template void VelocityFactorInterface::set( @@ -31,7 +31,7 @@ void VelocityFactorInterface::set( velocity_factor_.behavior = behavior_; velocity_factor_.pose = stop_pose; velocity_factor_.distance = - static_cast(motion_utils::calcSignedArcLength(points, curr_point, stop_point)); + static_cast(autoware::motion_utils::calcSignedArcLength(points, curr_point, stop_point)); velocity_factor_.status = status; velocity_factor_.detail = detail; } @@ -46,4 +46,4 @@ template void VelocityFactorInterface::set &, const Pose &, const Pose &, const VelocityFactorStatus, const std::string &); -} // namespace motion_utils +} // namespace autoware::motion_utils diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp similarity index 86% rename from common/motion_utils/src/marker/marker_helper.cpp rename to common/autoware_motion_utils/src/marker/marker_helper.cpp index 6e9c45adc241b..388c7102b825c 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" -#include +#include #include -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createDeletedDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createDeletedDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; using visualization_msgs::msg::MarkerArray; namespace @@ -85,14 +85,14 @@ inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( } } // namespace -namespace motion_utils +namespace autoware::motion_utils { visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "stop_", now, id, @@ -104,7 +104,7 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, @@ -116,7 +116,7 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, @@ -140,4 +140,4 @@ visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( { return createDeletedVirtualWallMarkerArray("dead_line_", now, id); } -} // namespace motion_utils +} // namespace autoware::motion_utils diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp similarity index 86% rename from common/motion_utils/src/marker/virtual_wall_marker_creator.cpp rename to common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp index 34aae096ffa9e..4fecaea1bb838 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/marker/virtual_wall_marker_creator.hpp" +#include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" -#include "motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" -namespace motion_utils +namespace autoware::motion_utils { void VirtualWallMarkerCreator::cleanup() @@ -55,13 +55,13 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( for (const auto & virtual_wall : virtual_walls_) { switch (virtual_wall.style) { case stop: - create_fn = motion_utils::createStopVirtualWallMarker; + create_fn = autoware::motion_utils::createStopVirtualWallMarker; break; case slowdown: - create_fn = motion_utils::createSlowDownVirtualWallMarker; + create_fn = autoware::motion_utils::createSlowDownVirtualWallMarker; break; case deadline: - create_fn = motion_utils::createDeadLineVirtualWallMarker; + create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker; break; } auto markers = create_fn( @@ -85,4 +85,4 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( cleanup(); return marker_array; } -} // namespace motion_utils +} // namespace autoware::motion_utils diff --git a/common/motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp similarity index 87% rename from common/motion_utils/src/resample/resample.cpp rename to common/autoware_motion_utils/src/resample/resample.cpp index 4a4e8dff1e4ad..baf1c534a8a00 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/resample/resample_utils.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" -#include "motion_utils/resample/resample_utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -namespace motion_utils +namespace autoware::motion_utils { std::vector resamplePointVector( const std::vector & points, @@ -50,7 +50,7 @@ std::vector resamplePointVector( for (size_t i = 1; i < points.size(); ++i) { const auto & prev_pt = points.at(i - 1); const auto & curr_pt = points.at(i); - const double ds = tier4_autoware_utils::calcDistance2d(prev_pt, curr_pt); + const double ds = autoware::universe_utils::calcDistance2d(prev_pt, curr_pt); input_arclength.push_back(ds + input_arclength.back()); x.push_back(curr_pt.x); y.push_back(curr_pt.y); @@ -91,19 +91,19 @@ std::vector resamplePointVector( const std::vector & points, const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { - const double input_length = motion_utils::calcArcLength(points); + const double input_length = autoware::motion_utils::calcArcLength(points); std::vector resampling_arclength; for (double s = 0.0; s < input_length; s += resample_interval) { resampling_arclength.push_back(s); } if (resampling_arclength.empty()) { - std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl; + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; return points; } // Insert terminal point - if (input_length - resampling_arclength.back() < motion_utils::overlap_threshold) { + if (input_length - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { resampling_arclength.back() = input_length; } else { resampling_arclength.push_back(input_length); @@ -118,7 +118,7 @@ std::vector resamplePoseVector( const bool use_lerp_for_z) { // Remove overlap points for resampling - const auto points = motion_utils::removeOverlapPoints(points_raw); + const auto points = autoware::motion_utils::removeOverlapPoints(points_raw); // validate arguments if (!resample_utils::validate_arguments(points, resampled_arclength)) { @@ -144,8 +144,8 @@ std::vector resamplePoseVector( } const bool is_driving_forward = - tier4_autoware_utils::isDrivingForward(points.at(0), points.at(1)); - motion_utils::insertOrientation(resampled_points, is_driving_forward); + autoware::universe_utils::isDrivingForward(points.at(0), points.at(1)); + autoware::motion_utils::insertOrientation(resampled_points, is_driving_forward); // Initial orientation is depend on the initial value of the resampled_arclength // when backward driving @@ -160,19 +160,19 @@ std::vector resamplePoseVector( const std::vector & points, const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { - const double input_length = motion_utils::calcArcLength(points); + const double input_length = autoware::motion_utils::calcArcLength(points); std::vector resampling_arclength; for (double s = 0.0; s < input_length; s += resample_interval) { resampling_arclength.push_back(s); } if (resampling_arclength.empty()) { - std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl; + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; return points; } // Insert terminal point - if (input_length - resampling_arclength.back() < motion_utils::overlap_threshold) { + if (input_length - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { resampling_arclength.back() = input_length; } else { resampling_arclength.push_back(input_length); @@ -203,9 +203,9 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( std::fabs(distance_to_resampling_point - resampling_arclength.at(j - 1)); const double dist_to_following_point = std::fabs(resampling_arclength.at(j) - distance_to_resampling_point); - if (dist_to_prev_point < motion_utils::overlap_threshold) { + if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { resampling_arclength.at(j - 1) = distance_to_resampling_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { + } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { resampling_arclength.at(j) = distance_to_resampling_point; } else { resampling_arclength.insert( @@ -260,7 +260,7 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( const auto & prev_pt = input_path.points.at(i - 1).point; const auto & curr_pt = input_path.points.at(i).point; const double ds = - tier4_autoware_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -271,7 +271,7 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( } if (input_arclength.back() < resampling_arclength.back()) { - std::cerr << "[motion_utils]: resampled path length is longer than input path length" + std::cerr << "[autoware_motion_utils]: resampled path length is longer than input path length" << std::endl; return input_path; } @@ -328,8 +328,9 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( } if (interpolated_pose.size() != resampling_arclength.size()) { - std::cerr << "[motion_utils]: Resampled pose size is different from resampled arclength" - << std::endl; + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; return input_path; } @@ -369,19 +370,19 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( transformed_input_path.at(i) = input_path.points.at(i).point; } // compute path length - const double input_path_len = motion_utils::calcArcLength(transformed_input_path); + const double input_path_len = autoware::motion_utils::calcArcLength(transformed_input_path); std::vector resampling_arclength; for (double s = 0.0; s < input_path_len; s += resample_interval) { resampling_arclength.push_back(s); } if (resampling_arclength.empty()) { - std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl; + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; return input_path; } // Insert terminal point - if (input_path_len - resampling_arclength.back() < motion_utils::overlap_threshold) { + if (input_path_len - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { resampling_arclength.back() = input_path_len; } else { resampling_arclength.push_back(input_path_len); @@ -390,7 +391,7 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( // Insert stop point if (resample_input_path_stop_point) { const auto distance_to_stop_point = - motion_utils::calcDistanceToForwardStopPoint(transformed_input_path, 0); + autoware::motion_utils::calcDistanceToForwardStopPoint(transformed_input_path, 0); if (distance_to_stop_point && !resampling_arclength.empty()) { for (size_t i = 1; i < resampling_arclength.size(); ++i) { if ( @@ -400,9 +401,9 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); const double dist_to_following_point = std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); - if (dist_to_prev_point < motion_utils::overlap_threshold) { + if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { + } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { resampling_arclength.at(i) = *distance_to_stop_point; } else { resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); @@ -449,7 +450,7 @@ autoware_planning_msgs::msg::Path resamplePath( const auto & prev_pt = input_path.points.at(i - 1); const auto & curr_pt = input_path.points.at(i); const double ds = - tier4_autoware_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -478,8 +479,9 @@ autoware_planning_msgs::msg::Path resamplePath( const auto interpolated_heading_rate = lerp(heading_rate); if (interpolated_pose.size() != resampled_arclength.size()) { - std::cerr << "[motion_utils]: Resampled pose size is different from resampled arclength" - << std::endl; + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; return input_path; } @@ -510,19 +512,19 @@ autoware_planning_msgs::msg::Path resamplePath( return input_path; } - const double input_path_len = motion_utils::calcArcLength(input_path.points); + const double input_path_len = autoware::motion_utils::calcArcLength(input_path.points); std::vector resampling_arclength; for (double s = 0.0; s < input_path_len; s += resample_interval) { resampling_arclength.push_back(s); } if (resampling_arclength.empty()) { - std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl; + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; return input_path; } // Insert terminal point - if (input_path_len - resampling_arclength.back() < motion_utils::overlap_threshold) { + if (input_path_len - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { resampling_arclength.back() = input_path_len; } else { resampling_arclength.push_back(input_path_len); @@ -531,7 +533,7 @@ autoware_planning_msgs::msg::Path resamplePath( // Insert stop point if (resample_input_path_stop_point) { const auto distance_to_stop_point = - motion_utils::calcDistanceToForwardStopPoint(input_path.points, 0); + autoware::motion_utils::calcDistanceToForwardStopPoint(input_path.points, 0); if (distance_to_stop_point && !resampling_arclength.empty()) { for (size_t i = 1; i < resampling_arclength.size(); ++i) { if ( @@ -541,9 +543,9 @@ autoware_planning_msgs::msg::Path resamplePath( std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); const double dist_to_following_point = std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); - if (dist_to_prev_point < motion_utils::overlap_threshold) { + if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { + } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { resampling_arclength.at(i) = *distance_to_stop_point; } else { resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); @@ -603,7 +605,7 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( const auto & prev_pt = input_trajectory.points.at(i - 1); const auto & curr_pt = input_trajectory.points.at(i); const double ds = - tier4_autoware_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -641,8 +643,9 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( const auto interpolated_time_from_start = lerp(time_from_start); if (interpolated_pose.size() != resampled_arclength.size()) { - std::cerr << "[motion_utils]: Resampled pose size is different from resampled arclength" - << std::endl; + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; return input_trajectory; } @@ -675,19 +678,22 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( return input_trajectory; } - const double input_trajectory_len = motion_utils::calcArcLength(input_trajectory.points); + const double input_trajectory_len = + autoware::motion_utils::calcArcLength(input_trajectory.points); std::vector resampling_arclength; for (double s = 0.0; s < input_trajectory_len; s += resample_interval) { resampling_arclength.push_back(s); } if (resampling_arclength.empty()) { - std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl; + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; return input_trajectory; } // Insert terminal point - if (input_trajectory_len - resampling_arclength.back() < motion_utils::overlap_threshold) { + if ( + input_trajectory_len - resampling_arclength.back() < + autoware::motion_utils::overlap_threshold) { resampling_arclength.back() = input_trajectory_len; } else { resampling_arclength.push_back(input_trajectory_len); @@ -696,7 +702,7 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( // Insert stop point if (resample_input_trajectory_stop_point) { const auto distance_to_stop_point = - motion_utils::calcDistanceToForwardStopPoint(input_trajectory.points, 0); + autoware::motion_utils::calcDistanceToForwardStopPoint(input_trajectory.points, 0); if (distance_to_stop_point && !resampling_arclength.empty()) { for (size_t i = 1; i < resampling_arclength.size(); ++i) { if ( @@ -706,9 +712,9 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); const double dist_to_following_point = std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); - if (dist_to_prev_point < motion_utils::overlap_threshold) { + if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { + } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { resampling_arclength.at(i) = *distance_to_stop_point; } else { resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); @@ -724,4 +730,4 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( use_zero_order_hold_for_twist); } -} // namespace motion_utils +} // namespace autoware::motion_utils diff --git a/common/motion_utils/src/trajectory/conversion.cpp b/common/autoware_motion_utils/src/trajectory/conversion.cpp similarity index 94% rename from common/motion_utils/src/trajectory/conversion.cpp rename to common/autoware_motion_utils/src/trajectory/conversion.cpp index e3a221c61a2a7..368d3e0fbbb75 100644 --- a/common/motion_utils/src/trajectory/conversion.cpp +++ b/common/autoware_motion_utils/src/trajectory/conversion.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include -namespace motion_utils +namespace autoware::motion_utils { /** * @brief Convert std::vector to @@ -51,4 +51,4 @@ std::vector convertToTrajectoryPoi return output; } -} // namespace motion_utils +} // namespace autoware::motion_utils diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp similarity index 82% rename from common/motion_utils/src/trajectory/interpolation.cpp rename to common/autoware_motion_utils/src/trajectory/interpolation.cpp index 13df39daab59d..4a9eaeca58d30 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "interpolation/linear_interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; -namespace motion_utils +namespace autoware::motion_utils { TrajectoryPoint calcInterpolatedPoint( const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose, @@ -37,14 +37,15 @@ TrajectoryPoint calcInterpolatedPoint( return trajectory.points.front(); } - const size_t segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory.points, target_pose, dist_threshold, yaw_threshold); + const size_t segment_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory.points, target_pose, dist_threshold, yaw_threshold); // Calculate interpolation ratio const auto & curr_pt = trajectory.points.at(segment_idx); const auto & next_pt = trajectory.points.at(segment_idx + 1); - const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt, next_pt); - const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt, target_pose); + const auto v1 = autoware::universe_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = autoware::universe_utils::point2tfVector(curr_pt, target_pose); if (v1.length2() < 1e-3) { return curr_pt; } @@ -57,7 +58,7 @@ TrajectoryPoint calcInterpolatedPoint( // pose interpolation interpolated_point.pose = - tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + autoware::universe_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { @@ -105,14 +106,15 @@ PathPointWithLaneId calcInterpolatedPoint( return path.points.front(); } - const size_t segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, target_pose, dist_threshold, yaw_threshold); + const size_t segment_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, target_pose, dist_threshold, yaw_threshold); // Calculate interpolation ratio const auto & curr_pt = path.points.at(segment_idx); const auto & next_pt = path.points.at(segment_idx + 1); - const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt.point, next_pt.point); - const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt.point, target_pose); + const auto v1 = autoware::universe_utils::point2tfVector(curr_pt.point, next_pt.point); + const auto v2 = autoware::universe_utils::point2tfVector(curr_pt.point, target_pose); if (v1.length2() < 1e-3) { return curr_pt; } @@ -125,7 +127,7 @@ PathPointWithLaneId calcInterpolatedPoint( // pose interpolation interpolated_point.point.pose = - tier4_autoware_utils::calcInterpolatedPose(curr_pt.point, next_pt.point, clamped_ratio); + autoware::universe_utils::calcInterpolatedPose(curr_pt.point, next_pt.point, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { @@ -145,4 +147,4 @@ PathPointWithLaneId calcInterpolatedPoint( return interpolated_point; } -} // namespace motion_utils +} // namespace autoware::motion_utils diff --git a/common/motion_utils/src/trajectory/path_with_lane_id.cpp b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp similarity index 88% rename from common/motion_utils/src/trajectory/path_with_lane_id.cpp rename to common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp index 4c86135811003..cd8de63f56c1d 100644 --- a/common/motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include #include #include -namespace motion_utils +namespace autoware::motion_utils { std::optional> getPathIndexRangeWithLaneId( @@ -116,15 +116,15 @@ tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( // apply beta to CoG pose geometry_msgs::msg::Pose cog_pose_with_beta; - cog_pose_with_beta.position = tier4_autoware_utils::getPoint(path.points.at(i)); + cog_pose_with_beta.position = autoware::universe_utils::getPoint(path.points.at(i)); cog_pose_with_beta.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw_vec.at(i) - beta); + autoware::universe_utils::createQuaternionFromYaw(yaw_vec.at(i) - beta); const auto rear_pose = - tier4_autoware_utils::calcOffsetPose(cog_pose_with_beta, -rear_to_cog, 0.0, 0.0); + autoware::universe_utils::calcOffsetPose(cog_pose_with_beta, -rear_to_cog, 0.0, 0.0); // update pose - tier4_autoware_utils::setPose(rear_pose, cog_path.points.at(i)); + autoware::universe_utils::setPose(rear_pose, cog_path.points.at(i)); } // compensate for the last pose @@ -136,4 +136,4 @@ tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( return cog_path; } -} // namespace motion_utils +} // namespace autoware::motion_utils diff --git a/common/motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp similarity index 99% rename from common/motion_utils/src/trajectory/trajectory.cpp rename to common/autoware_motion_utils/src/trajectory/trajectory.cpp index 42c750e3c109f..5d536c0772fea 100644 --- a/common/motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" -namespace motion_utils +namespace autoware::motion_utils { // @@ -599,4 +599,4 @@ template bool isTargetPointFront -namespace motion_utils +namespace autoware::motion_utils { VehicleStopCheckerBase::VehicleStopCheckerBase(rclcpp::Node * node, double buffer_duration) : clock_(node->get_clock()), logger_(node->get_logger()) @@ -118,18 +118,18 @@ bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_durati } const auto & p = odometry_ptr_->pose.pose.position; - const auto idx = motion_utils::searchZeroVelocityIndex(trajectory_ptr_->points); + const auto idx = autoware::motion_utils::searchZeroVelocityIndex(trajectory_ptr_->points); if (!idx) { return false; } - return std::abs(motion_utils::calcSignedArcLength(trajectory_ptr_->points, p, idx.value())) < - th_arrived_distance_m; + return std::abs(autoware::motion_utils::calcSignedArcLength( + trajectory_ptr_->points, p, idx.value())) < th_arrived_distance_m; } void VehicleArrivalChecker::onTrajectory(const Trajectory::ConstSharedPtr msg) { trajectory_ptr_ = msg; } -} // namespace motion_utils +} // namespace autoware::motion_utils diff --git a/common/motion_utils/test/src/distance/test_distance.cpp b/common/autoware_motion_utils/test/src/distance/test_distance.cpp similarity index 96% rename from common/motion_utils/test/src/distance/test_distance.cpp rename to common/autoware_motion_utils/test/src/distance/test_distance.cpp index 5bcb2c26fe1bd..f6d6a9cc4dafd 100644 --- a/common/motion_utils/test/src/distance/test_distance.cpp +++ b/common/autoware_motion_utils/test/src/distance/test_distance.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/distance/distance.hpp" #include "gtest/gtest.h" -#include "motion_utils/distance/distance.hpp" namespace { -using motion_utils::calcDecelDistWithJerkAndAccConstraints; +using autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints; constexpr double epsilon = 1e-3; diff --git a/common/motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp similarity index 89% rename from common/motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp rename to common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp index 7019a5425ac2e..5e2e0cc4bdf02 100644 --- a/common/motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp +++ b/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" #include "gtest/gtest.h" -#include "motion_utils/marker/virtual_wall_marker_creator.hpp" namespace { constexpr auto wall_ns_suffix = "_virtual_wall"; @@ -38,9 +38,9 @@ bool has_ns_id( TEST(VirtualWallMarkerCreator, oneWall) { - motion_utils::VirtualWall wall; - motion_utils::VirtualWallMarkerCreator creator; - wall.style = motion_utils::VirtualWallType::stop; + autoware::motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWallMarkerCreator creator; + wall.style = autoware::motion_utils::VirtualWallType::stop; wall.pose.position.x = 1.0; wall.pose.position.y = 2.0; creator.add_virtual_wall(wall); @@ -63,16 +63,16 @@ TEST(VirtualWallMarkerCreator, oneWall) TEST(VirtualWallMarkerCreator, manyWalls) { - motion_utils::VirtualWall wall; - motion_utils::VirtualWallMarkerCreator creator; - wall.style = motion_utils::VirtualWallType::stop; + autoware::motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWallMarkerCreator creator; + wall.style = autoware::motion_utils::VirtualWallType::stop; wall.ns = "ns1_"; creator.add_virtual_wall(wall); creator.add_virtual_wall(wall); creator.add_virtual_wall(wall); wall.ns = "ns2_"; creator.add_virtual_wall(wall); - wall.style = motion_utils::VirtualWallType::slowdown; + wall.style = autoware::motion_utils::VirtualWallType::slowdown; wall.ns = "ns2_"; creator.add_virtual_wall(wall); creator.add_virtual_wall(wall); @@ -80,7 +80,7 @@ TEST(VirtualWallMarkerCreator, manyWalls) creator.add_virtual_wall(wall); creator.add_virtual_wall(wall); creator.add_virtual_wall(wall); - wall.style = motion_utils::VirtualWallType::deadline; + wall.style = autoware::motion_utils::VirtualWallType::deadline; wall.ns = "ns1_"; creator.add_virtual_wall(wall); wall.ns = "ns2_"; diff --git a/common/motion_utils/test/src/resample/test_resample.cpp b/common/autoware_motion_utils/test/src/resample/test_resample.cpp similarity index 97% rename from common/motion_utils/test/src/resample/test_resample.cpp rename to common/autoware_motion_utils/test/src/resample/test_resample.cpp index ef374f0b484db..62db1b665d07a 100644 --- a/common/motion_utils/test/src/resample/test_resample.cpp +++ b/common/autoware_motion_utils/test/src/resample/test_resample.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/constants.hpp" -#include "motion_utils/resample/resample.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/motion_utils/constants.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include #include @@ -27,13 +27,13 @@ namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::transformPoint; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::transformPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; @@ -177,8 +177,8 @@ std::vector generateArclength(const size_t num_points, const double inte TEST(resample_vector_pose, resample_by_same_interval) { + using autoware::motion_utils::resamplePoseVector; using geometry_msgs::msg::Pose; - using motion_utils::resamplePoseVector; std::vector path(10); for (size_t i = 0; i < 10; ++i) { @@ -220,7 +220,7 @@ TEST(resample_vector_pose, resample_by_same_interval) TEST(resample_path_with_lane_id, resample_path_by_vector) { - using motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; // Output is same as input { auto path = generateTestPathWithLaneId(10, 1.0, 3.0, 1.0, 0.01); @@ -251,7 +251,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Change the last point orientation path.points.back() = generateTestPathPointWithLaneId( - 9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); + 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); { const auto resampled_path = resamplePath(path, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -276,7 +276,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); @@ -540,14 +540,14 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) TEST(resample_path_with_lane_id, resample_path_by_vector_backward) { - using motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; { tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, tier4_autoware_utils::pi, i * 1.0, i * 0.5, i * 0.1, false, + i * 1.0, 0.0, 0.0, autoware::universe_utils::pi, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); } path.points.back().point.is_final = true; @@ -639,7 +639,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) } } - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -659,7 +659,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) } path.points.back().point.is_final = true; path.points.at(0).point.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; const auto resampled_path = resamplePath(path, resampled_arclength); @@ -750,7 +750,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) // Initial Orientation { - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); const auto p = resampled_path.points.at(0).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); @@ -758,7 +758,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); } - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 1; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -771,7 +771,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) { - using motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; // Lerp x, y { @@ -919,7 +919,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -1009,7 +1009,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) TEST(resample_path_with_lane_id, resample_path_by_same_interval) { - using motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; // Same point resampling { @@ -1045,7 +1045,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) } // Change the last point orientation path.points.back() = generateTestPathPointWithLaneId( - 9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); + 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); { const auto resampled_path = resamplePath(path, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -1070,7 +1070,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(0.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(0.0); EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); @@ -1194,7 +1194,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) path.points.at(0).point.longitudinal_velocity_mps = 5.0; path.points.back().point.is_final = true; - const double ds = 1.0 - motion_utils::overlap_threshold; + const double ds = 1.0 - autoware::motion_utils::overlap_threshold; const auto resampled_path = resamplePath(path, ds); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { const auto p = resampled_path.points.at(i); @@ -1564,7 +1564,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) TEST(resample_path, resample_path_by_vector) { - using motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; // Output is same as input { auto path = generateTestPath(10, 1.0, 3.0, 1.0, 0.01); @@ -1590,7 +1590,7 @@ TEST(resample_path, resample_path_by_vector) // Change the last point orientation path.points.back() = - generateTestPathPoint(9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01); + generateTestPathPoint(9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01); { const auto resampled_path = resamplePath(path, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -1610,7 +1610,7 @@ TEST(resample_path, resample_path_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -1812,7 +1812,7 @@ TEST(resample_path, resample_path_by_vector) TEST(resample_path, resample_path_by_vector_backward) { - using motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; { autoware_planning_msgs::msg::Path path; @@ -1884,7 +1884,7 @@ TEST(resample_path, resample_path_by_vector_backward) EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); } - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -1902,7 +1902,7 @@ TEST(resample_path, resample_path_by_vector_backward) path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); } path.points.at(0).pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; const auto resampled_path = resamplePath(path, resampled_arclength); @@ -1969,7 +1969,7 @@ TEST(resample_path, resample_path_by_vector_backward) // Initial Orientation { - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); const auto p = resampled_path.points.at(0); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); @@ -1977,7 +1977,7 @@ TEST(resample_path, resample_path_by_vector_backward) EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); } - const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); for (size_t i = 1; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -1990,7 +1990,7 @@ TEST(resample_path, resample_path_by_vector_backward) TEST(resample_path, resample_path_by_vector_non_default) { - using motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; // Lerp x, y { @@ -2103,7 +2103,7 @@ TEST(resample_path, resample_path_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -2175,7 +2175,7 @@ TEST(resample_path, resample_path_by_vector_non_default) TEST(resample_path, resample_path_by_same_interval) { - using motion_utils::resamplePath; + using autoware::motion_utils::resamplePath; // Same point resampling { @@ -2205,7 +2205,7 @@ TEST(resample_path, resample_path_by_same_interval) // Change the last point orientation path.points.back() = - generateTestPathPoint(9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01); + generateTestPathPoint(9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01); { const auto resampled_path = resamplePath(path, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -2225,7 +2225,7 @@ TEST(resample_path, resample_path_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -2321,7 +2321,7 @@ TEST(resample_path, resample_path_by_same_interval) } path.points.at(0).longitudinal_velocity_mps = 5.0; - const double ds = 1.0 - motion_utils::overlap_threshold; + const double ds = 1.0 - autoware::motion_utils::overlap_threshold; const auto resampled_path = resamplePath(path, ds); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { const auto p = resampled_path.points.at(i); @@ -2633,7 +2633,7 @@ TEST(resample_path, resample_path_by_same_interval) TEST(resample_trajectory, resample_trajectory_by_vector) { - using motion_utils::resampleTrajectory; + using autoware::motion_utils::resampleTrajectory; // Output is same as input { auto traj = generateTestTrajectory(10, 1.0, 3.0, 1.0, 0.01, 0.5); @@ -2660,7 +2660,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Change the last point orientation traj.points.back() = generateTestTrajectoryPoint( - 9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); + 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); { const auto resampled_path = resampleTrajectory(traj, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -2681,7 +2681,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = traj.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -2891,7 +2891,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) TEST(resample_trajectory, resample_trajectory_by_vector_non_default) { - using motion_utils::resampleTrajectory; + using autoware::motion_utils::resampleTrajectory; // Lerp x, y { @@ -3013,7 +3013,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_traj.points.size(); ++i) { const auto p = resampled_traj.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -3090,7 +3090,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) TEST(resample_trajectory, resample_trajectory_by_same_interval) { - using motion_utils::resampleTrajectory; + using autoware::motion_utils::resampleTrajectory; // Same point resampling { @@ -3122,7 +3122,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Change the last point orientation traj.points.back() = generateTestTrajectoryPoint( - 9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); + 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); { const auto resampled_path = resampleTrajectory(traj, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -3143,7 +3143,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = traj.points.back(); - const auto ans_quat = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -3246,7 +3246,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) } traj.points.at(0).longitudinal_velocity_mps = 5.0; - const double ds = 1.0 - motion_utils::overlap_threshold; + const double ds = 1.0 - autoware::motion_utils::overlap_threshold; const auto resampled_traj = resampleTrajectory(traj, ds); for (size_t i = 0; i < resampled_traj.points.size() - 1; ++i) { const auto p = resampled_traj.points.at(i); diff --git a/common/motion_utils/test/src/test_motion_utils.cpp b/common/autoware_motion_utils/test/src/test_motion_utils.cpp similarity index 100% rename from common/motion_utils/test/src/test_motion_utils.cpp rename to common/autoware_motion_utils/test/src/test_motion_utils.cpp diff --git a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp similarity index 91% rename from common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp rename to common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp index 7aae860b76533..dc828e885af64 100644 --- a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include #include @@ -22,9 +22,9 @@ namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; using autoware_planning_msgs::msg::Trajectory; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; constexpr double epsilon = 1e-6; @@ -66,7 +66,7 @@ TEST(trajectory_benchmark, DISABLED_calcLateralOffset) std::default_random_engine e1(r()); std::uniform_real_distribution uniform_dist(0.0, 1000.0); - using motion_utils::calcLateralOffset; + using autoware::motion_utils::calcLateralOffset; const auto traj = generateTestTrajectory(1000, 1.0, 0.0, 0.0, 0.1); constexpr auto nb_iteration = 10000; diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp similarity index 97% rename from common/motion_utils/test/src/trajectory/test_interpolation.cpp rename to common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp index 90b11e535c486..b4b60ff403048 100644 --- a/common/motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include #include @@ -27,11 +27,11 @@ namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::transformPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::transformPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; @@ -124,7 +124,7 @@ T generateTestPath( TEST(Interpolation, interpolate_path_for_trajectory) { - using motion_utils::calcInterpolatedPoint; + using autoware::motion_utils::calcInterpolatedPoint; { autoware_planning_msgs::msg::Trajectory traj; @@ -348,7 +348,7 @@ TEST(Interpolation, interpolate_path_for_trajectory) TEST(Interpolation, interpolate_path_for_path) { - using motion_utils::calcInterpolatedPoint; + using autoware::motion_utils::calcInterpolatedPoint; { tier4_planning_msgs::msg::PathWithLaneId path; @@ -540,7 +540,7 @@ TEST(Interpolation, interpolate_path_for_path) TEST(Interpolation, interpolate_points_with_length) { - using motion_utils::calcInterpolatedPose; + using autoware::motion_utils::calcInterpolatedPose; { autoware_planning_msgs::msg::Trajectory traj; diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp similarity index 91% rename from common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp rename to common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 6dd60c499d181..62e4ac74cb639 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/path_with_lane_id.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -22,7 +22,7 @@ namespace { -using tier4_autoware_utils::createPoint; +using autoware::universe_utils::createPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; @@ -31,7 +31,7 @@ geometry_msgs::msg::Pose createPose( { geometry_msgs::msg::Pose p; p.position = createPoint(x, y, z); - p.orientation = tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); + p.orientation = autoware::universe_utils::createQuaternionFromRPY(roll, pitch, yaw); return p; } @@ -54,7 +54,7 @@ PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { - using motion_utils::getPathIndexRangeWithLaneId; + using autoware::motion_utils::getPathIndexRangeWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; // Usual cases @@ -99,8 +99,8 @@ TEST(path_with_lane_id, getPathIndexRangeWithLaneId) TEST(path_with_lane_id, findNearestIndexFromLaneId) { - using motion_utils::findNearestIndexFromLaneId; - using motion_utils::findNearestSegmentIndexFromLaneId; + using autoware::motion_utils::findNearestIndexFromLaneId; + using autoware::motion_utils::findNearestSegmentIndexFromLaneId; const auto path = generateTestPathWithLaneId(10, 1.0); @@ -164,7 +164,7 @@ TEST(path_with_lane_id, findNearestIndexFromLaneId) // NOTE: This test is temporary for the current implementation. TEST(path_with_lane_id, convertToRearWheelCenter) { - using motion_utils::convertToRearWheelCenter; + using autoware::motion_utils::convertToRearWheelCenter; PathWithLaneId path; diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp similarity index 95% rename from common/motion_utils/test/src/trajectory/test_trajectory.cpp rename to common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp index 9ebc712b3bf1a..4ff9b2a33ca13 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include #include @@ -28,9 +28,9 @@ namespace { using autoware_planning_msgs::msg::Trajectory; using TrajectoryPointArray = std::vector; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::transformPoint; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::transformPoint; constexpr double epsilon = 1e-6; @@ -94,7 +94,7 @@ void updateTrajectoryVelocityAt(T & points, const size_t idx, const double vel) TEST(trajectory, validateNonEmpty) { - using motion_utils::validateNonEmpty; + using autoware::motion_utils::validateNonEmpty; // Empty EXPECT_THROW(validateNonEmpty(Trajectory{}.points), std::invalid_argument); @@ -106,8 +106,8 @@ TEST(trajectory, validateNonEmpty) TEST(trajectory, validateNonSharpAngle_DefaultThreshold) { + using autoware::motion_utils::validateNonSharpAngle; using autoware_planning_msgs::msg::TrajectoryPoint; - using motion_utils::validateNonSharpAngle; TrajectoryPoint p1; p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); @@ -135,9 +135,9 @@ TEST(trajectory, validateNonSharpAngle_DefaultThreshold) TEST(trajectory, validateNonSharpAngle_SetThreshold) { + using autoware::motion_utils::validateNonSharpAngle; + using autoware::universe_utils::pi; using autoware_planning_msgs::msg::TrajectoryPoint; - using motion_utils::validateNonSharpAngle; - using tier4_autoware_utils::pi; TrajectoryPoint p1; p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); @@ -165,7 +165,7 @@ TEST(trajectory, validateNonSharpAngle_SetThreshold) TEST(trajectory, searchZeroVelocityIndex) { - using motion_utils::searchZeroVelocityIndex; + using autoware::motion_utils::searchZeroVelocityIndex; // Empty EXPECT_FALSE(searchZeroVelocityIndex(Trajectory{}.points)); @@ -244,7 +244,7 @@ TEST(trajectory, searchZeroVelocityIndex) TEST(trajectory, searchZeroVelocityIndex_from_pose) { - using motion_utils::searchZeroVelocityIndex; + using autoware::motion_utils::searchZeroVelocityIndex; // No zero velocity point { @@ -307,7 +307,7 @@ TEST(trajectory, searchZeroVelocityIndex_from_pose) TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) { - using motion_utils::findNearestIndex; + using autoware::motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -338,7 +338,7 @@ TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) { - using motion_utils::findNearestIndex; + using autoware::motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -348,7 +348,7 @@ TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) TEST(trajectory, findNearestIndex_Pose_NoThreshold) { - using motion_utils::findNearestIndex; + using autoware::motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -374,7 +374,7 @@ TEST(trajectory, findNearestIndex_Pose_NoThreshold) TEST(trajectory, findNearestIndex_Pose_DistThreshold) { - using motion_utils::findNearestIndex; + using autoware::motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -390,7 +390,7 @@ TEST(trajectory, findNearestIndex_Pose_DistThreshold) TEST(trajectory, findNearestIndex_Pose_YawThreshold) { - using motion_utils::findNearestIndex; + using autoware::motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); const auto max_d = std::numeric_limits::max(); @@ -409,7 +409,7 @@ TEST(trajectory, findNearestIndex_Pose_YawThreshold) TEST(trajectory, findNearestIndex_Pose_DistAndYawThreshold) { - using motion_utils::findNearestIndex; + using autoware::motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -423,7 +423,7 @@ TEST(trajectory, findNearestIndex_Pose_DistAndYawThreshold) TEST(trajectory, findNearestSegmentIndex) { - using motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::findNearestSegmentIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -463,7 +463,7 @@ TEST(trajectory, findNearestSegmentIndex) TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) { - using motion_utils::calcLongitudinalOffsetToSegment; + using autoware::motion_utils::calcLongitudinalOffsetToSegment; const auto traj = generateTestTrajectory(10, 1.0); const bool throw_exception = true; @@ -513,7 +513,7 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) { - using motion_utils::calcLongitudinalOffsetToSegment; + using autoware::motion_utils::calcLongitudinalOffsetToSegment; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -525,7 +525,7 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) TEST(trajectory, calcLateralOffset) { - using motion_utils::calcLateralOffset; + using autoware::motion_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0); const bool throw_exception = true; @@ -566,7 +566,7 @@ TEST(trajectory, calcLateralOffset) TEST(trajectory, calcLateralOffset_without_segment_idx) { - using motion_utils::calcLateralOffset; + using autoware::motion_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0); const bool throw_exception = true; @@ -627,7 +627,7 @@ TEST(trajectory, calcLateralOffset_without_segment_idx) TEST(trajectory, calcLateralOffset_CurveTrajectory) { - using motion_utils::calcLateralOffset; + using autoware::motion_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -638,7 +638,7 @@ TEST(trajectory, calcLateralOffset_CurveTrajectory) TEST(trajectory, calcSignedArcLengthFromIndexToIndex) { - using motion_utils::calcSignedArcLength; + using autoware::motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -661,7 +661,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToIndex) TEST(trajectory, calcSignedArcLengthFromPointToIndex) { - using motion_utils::calcSignedArcLength; + using autoware::motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -690,7 +690,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToIndex) TEST(trajectory, calcSignedArcLengthFromIndexToPoint) { - using motion_utils::calcSignedArcLength; + using autoware::motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -719,7 +719,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToPoint) TEST(trajectory, calcSignedArcLengthFromPointToPoint) { - using motion_utils::calcSignedArcLength; + using autoware::motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -782,7 +782,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToPoint) TEST(trajectory, calcArcLength) { - using motion_utils::calcArcLength; + using autoware::motion_utils::calcArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -795,7 +795,7 @@ TEST(trajectory, calcArcLength) TEST(trajectory, convertToTrajectory) { - using motion_utils::convertToTrajectory; + using autoware::motion_utils::convertToTrajectory; // Size check { @@ -807,7 +807,7 @@ TEST(trajectory, convertToTrajectory) TEST(trajectory, convertToTrajectoryPointArray) { - using motion_utils::convertToTrajectoryPointArray; + using autoware::motion_utils::convertToTrajectoryPointArray; const auto traj_input = generateTestTrajectory(100, 1.0); const auto traj = convertToTrajectoryPointArray(traj_input); @@ -823,7 +823,7 @@ TEST(trajectory, convertToTrajectoryPointArray) TEST(trajectory, calcDistanceToForwardStopPointFromIndex) { - using motion_utils::calcDistanceToForwardStopPoint; + using autoware::motion_utils::calcDistanceToForwardStopPoint; auto traj_input = generateTestTrajectory(100, 1.0, 3.0); traj_input.points.at(50).longitudinal_velocity_mps = 0.0; @@ -881,7 +881,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromIndex) TEST(trajectory, calcDistanceToForwardStopPointFromPose) { - using motion_utils::calcDistanceToForwardStopPoint; + using autoware::motion_utils::calcDistanceToForwardStopPoint; auto traj_input = generateTestTrajectory(100, 1.0, 3.0); traj_input.points.at(50).longitudinal_velocity_mps = 0.0; @@ -965,7 +965,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) { - using motion_utils::calcDistanceToForwardStopPoint; + using autoware::motion_utils::calcDistanceToForwardStopPoint; auto traj_input = generateTestTrajectory(100, 1.0, 3.0); traj_input.points.at(50).longitudinal_velocity_mps = 0.0; @@ -1009,8 +1009,8 @@ TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) { - using motion_utils::calcDistanceToForwardStopPoint; - using tier4_autoware_utils::deg2rad; + using autoware::motion_utils::calcDistanceToForwardStopPoint; + using autoware::universe_utils::deg2rad; const auto max_d = std::numeric_limits::max(); auto traj_input = generateTestTrajectory(100, 1.0, 3.0); @@ -1061,10 +1061,10 @@ TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) TEST(trajectory, calcLongitudinalOffsetPointFromIndex) { - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPoint; - using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::getPoint; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPoint; + using autoware::motion_utils::calcSignedArcLength; + using autoware::universe_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1137,11 +1137,11 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) TEST(trajectory, calcLongitudinalOffsetPointFromPoint) { - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPoint; - using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::getPoint; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPoint; + using autoware::motion_utils::calcSignedArcLength; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1215,10 +1215,10 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) { - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::getPoint; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcSignedArcLength; + using autoware::universe_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1299,10 +1299,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) { + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::universe_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using tier4_autoware_utils::deg2rad; Trajectory traj{}; @@ -1385,10 +1385,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) { + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::universe_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using tier4_autoware_utils::deg2rad; Trajectory traj{}; @@ -1477,11 +1477,11 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) { - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::getPoint; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcSignedArcLength; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1563,12 +1563,12 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) { + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcLongitudinalOffsetToSegment; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using motion_utils::calcLongitudinalOffsetToSegment; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; Trajectory traj{}; @@ -1637,12 +1637,12 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) { + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcLongitudinalOffsetToSegment; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using motion_utils::calcLongitudinalOffsetToSegment; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; Trajectory traj{}; @@ -1715,13 +1715,13 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) TEST(trajectory, insertTargetPoint) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1928,19 +1928,19 @@ TEST(trajectory, insertTargetPoint) TEST(trajectory, insertTargetPoint_Reverse) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternionFromYaw; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; constexpr double overlap_threshold = 1e-4; auto traj = generateTestTrajectory(10, 1.0); for (size_t i = 0; i < traj.points.size(); ++i) { - traj.points.at(i).pose.orientation = createQuaternionFromYaw(tier4_autoware_utils::pi); + traj.points.at(i).pose.orientation = createQuaternionFromYaw(autoware::universe_utils::pi); } const auto total_length = calcArcLength(traj.points); @@ -1986,13 +1986,13 @@ TEST(trajectory, insertTargetPoint_Reverse) TEST(trajectory, insertTargetPoint_OverlapThreshold) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; constexpr double overlap_threshold = 1e-4; const auto traj = generateTestTrajectory(10, 1.0); @@ -2079,13 +2079,13 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) TEST(trajectory, insertTargetPoint_Length) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -2317,13 +2317,13 @@ TEST(trajectory, insertTargetPoint_Length) TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -2503,13 +2503,13 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Idx) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); @@ -2733,13 +2733,13 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero_Start_Idx) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); @@ -2920,13 +2920,13 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero TEST(trajectory, insertTargetPoint_Length_from_a_pose) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -3272,13 +3272,13 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) TEST(trajectory, insertStopPoint_from_a_source_index) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertStopPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertStopPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0, 10.0); @@ -3527,13 +3527,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) TEST(trajectory, insertStopPoint_from_front_point) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertStopPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertStopPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0, 10.0); @@ -3724,13 +3724,13 @@ TEST(trajectory, insertStopPoint_from_front_point) TEST(trajectory, insertStopPoint_from_a_pose) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertStopPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertStopPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0, 10.0); const auto total_length = calcArcLength(traj.points); @@ -4110,13 +4110,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) TEST(trajectory, insertStopPoint_with_pose_and_segment_index) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertStopPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertStopPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getPose; const auto traj = generateTestTrajectory(10, 1.0, 3.0); const auto total_length = calcArcLength(traj.points); @@ -4354,12 +4354,12 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) TEST(trajectory, insertDecelPoint_from_a_point) { - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertDecelPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::getLongitudinalVelocity; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertDecelPoint; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::getLongitudinalVelocity; const auto traj = generateTestTrajectory(10, 1.0, 10.0); const auto total_length = calcArcLength(traj.points); @@ -4444,9 +4444,9 @@ TEST(trajectory, insertDecelPoint_from_a_point) TEST(trajectory, findFirstNearestIndexWithSoftConstraints) { - using motion_utils::findFirstNearestIndexWithSoftConstraints; - using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; - using tier4_autoware_utils::pi; + using autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; + using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; + using autoware::universe_utils::pi; const auto traj = generateTestTrajectory(10, 1.0); @@ -4970,7 +4970,7 @@ TEST(trajectory, findFirstNearestIndexWithSoftConstraints) TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointAndSegmentIndex) { - using motion_utils::calcSignedArcLength; + using autoware::motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -5040,7 +5040,7 @@ TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointAndSegmentInd TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex) { - using motion_utils::calcSignedArcLength; + using autoware::motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -5119,7 +5119,7 @@ TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex) TEST(trajectory, removeOverlapPoints) { - using motion_utils::removeOverlapPoints; + using autoware::motion_utils::removeOverlapPoints; const auto traj = generateTestTrajectory(10, 1.0, 1.0); const auto removed_traj = removeOverlapPoints(traj.points, 0); @@ -5246,101 +5246,101 @@ TEST(trajectory, removeOverlapPoints) TEST(trajectory, cropForwardPoints) { - using motion_utils::cropForwardPoints; + using autoware::motion_utils::cropForwardPoints; const auto traj = generateTestTrajectory(10, 1.0, 1.0); { // Normal case const auto cropped_traj_points = - cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.5, 1.5, 0.0), 1, 4.8); + cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.5, 1.5, 0.0), 1, 4.8); EXPECT_EQ(cropped_traj_points.size(), static_cast(7)); } { // Forward length is longer than points arc length. const auto cropped_traj_points = - cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.5, 1.5, 0.0), 1, 10.0); + cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.5, 1.5, 0.0), 1, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.0, 1.0, 0.0), 0, 2.5); + cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.0, 1.0, 0.0), 0, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropForwardPoints(traj.points, tier4_autoware_utils::createPoint(1.0, 1.0, 0.0), 1, 2.5); + cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.0, 1.0, 0.0), 1, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } TEST(trajectory, cropBackwardPoints) { - using motion_utils::cropBackwardPoints; + using autoware::motion_utils::cropBackwardPoints; const auto traj = generateTestTrajectory(10, 1.0, 1.0); { // Normal case const auto cropped_traj_points = - cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.5, 8.5, 0.0), 8, 4.8); + cropBackwardPoints(traj.points, autoware::universe_utils::createPoint(8.5, 8.5, 0.0), 8, 4.8); EXPECT_EQ(cropped_traj_points.size(), static_cast(6)); } { // Backward length is longer than points arc length. - const auto cropped_traj_points = - cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.5, 8.5, 0.0), 8, 10.0); + const auto cropped_traj_points = cropBackwardPoints( + traj.points, autoware::universe_utils::createPoint(8.5, 8.5, 0.0), 8, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.0, 8.0, 0.0), 7, 2.5); + cropBackwardPoints(traj.points, autoware::universe_utils::createPoint(8.0, 8.0, 0.0), 7, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropBackwardPoints(traj.points, tier4_autoware_utils::createPoint(8.0, 8.0, 0.0), 8, 2.5); + cropBackwardPoints(traj.points, autoware::universe_utils::createPoint(8.0, 8.0, 0.0), 8, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } TEST(trajectory, cropPoints) { - using motion_utils::cropPoints; + using autoware::motion_utils::cropPoints; const auto traj = generateTestTrajectory(10, 1.0, 1.0); { // Normal case const auto cropped_traj_points = - cropPoints(traj.points, tier4_autoware_utils::createPoint(3.5, 3.5, 0.0), 3, 2.3, 1.2); + cropPoints(traj.points, autoware::universe_utils::createPoint(3.5, 3.5, 0.0), 3, 2.3, 1.2); EXPECT_EQ(cropped_traj_points.size(), static_cast(3)); } { // Length is longer than points arc length. const auto cropped_traj_points = - cropPoints(traj.points, tier4_autoware_utils::createPoint(3.5, 3.5, 0.0), 3, 10.0, 10.0); + cropPoints(traj.points, autoware::universe_utils::createPoint(3.5, 3.5, 0.0), 3, 10.0, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropPoints(traj.points, tier4_autoware_utils::createPoint(3.0, 3.0, 0.0), 2, 2.2, 1.9); + cropPoints(traj.points, autoware::universe_utils::createPoint(3.0, 3.0, 0.0), 2, 2.2, 1.9); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropPoints(traj.points, tier4_autoware_utils::createPoint(3.0, 3.0, 0.0), 3, 2.2, 1.9); + cropPoints(traj.points, autoware::universe_utils::createPoint(3.0, 3.0, 0.0), 3, 2.2, 1.9); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } TEST(Trajectory, removeFirstInvalidOrientationPoints) { - using motion_utils::insertOrientation; - using motion_utils::removeFirstInvalidOrientationPoints; + using autoware::motion_utils::insertOrientation; + using autoware::motion_utils::removeFirstInvalidOrientationPoints; const double max_yaw_diff = M_PI_2; @@ -5357,7 +5357,7 @@ TEST(Trajectory, removeFirstInvalidOrientationPoints) EXPECT_EQ(traj.points.at(i), modified_traj.points.at(i)); const double yaw1 = tf2::getYaw(modified_traj.points.at(i).pose.orientation); const double yaw2 = tf2::getYaw(modified_traj.points.at(i + 1).pose.orientation); - const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)); + const double yaw_diff = std::abs(autoware::universe_utils::normalizeRadian(yaw1 - yaw2)); EXPECT_LE(yaw_diff, max_yaw_diff); } }; @@ -5365,8 +5365,7 @@ TEST(Trajectory, removeFirstInvalidOrientationPoints) auto traj = generateTestTrajectory(10, 1.0, 1.0); // no invalid points - testRemoveInvalidOrientationPoints( - traj, [](Trajectory &) {}, traj.points.size()); + testRemoveInvalidOrientationPoints(traj, [](Trajectory &) {}, traj.points.size()); // invalid point at the end testRemoveInvalidOrientationPoints( @@ -5393,8 +5392,8 @@ TEST(Trajectory, removeFirstInvalidOrientationPoints) TEST(trajectory, calcYawDeviation) { + using autoware::motion_utils::calcYawDeviation; using autoware_planning_msgs::msg::TrajectoryPoint; - using motion_utils::calcYawDeviation; constexpr double tolerance = 1e-3; @@ -5420,9 +5419,9 @@ TEST(trajectory, calcYawDeviation) TEST(trajectory, isTargetPointFront) { + using autoware::motion_utils::isTargetPointFront; + using autoware::universe_utils::createPoint; using autoware_planning_msgs::msg::TrajectoryPoint; - using motion_utils::isTargetPointFront; - using tier4_autoware_utils::createPoint; // Generate test trajectory const auto trajectory = generateTestTrajectory(10, 1.0); diff --git a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp similarity index 98% rename from common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp rename to common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp index 540aef70cc1f4..b2d2f3a2e8003 100644 --- a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp +++ b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/vehicle/vehicle_state_checker.hpp" +#include "autoware/motion_utils/vehicle/vehicle_state_checker.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "test_vehicle_state_checker_helper.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -32,12 +32,12 @@ constexpr double STOP_DURATION_THRESHOLD_600_MS = 0.6; constexpr double STOP_DURATION_THRESHOLD_800_MS = 0.8; constexpr double STOP_DURATION_THRESHOLD_1000_MS = 1.0; -using motion_utils::VehicleArrivalChecker; -using motion_utils::VehicleStopChecker; +using autoware::motion_utils::VehicleArrivalChecker; +using autoware::motion_utils::VehicleStopChecker; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternion; +using autoware::universe_utils::createTranslation; using nav_msgs::msg::Odometry; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternion; -using tier4_autoware_utils::createTranslation; class CheckerNode : public rclcpp::Node { diff --git a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp similarity index 100% rename from common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp rename to common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp diff --git a/common/autoware_test_utils/CMakeLists.txt b/common/autoware_test_utils/CMakeLists.txt index 91adf935445b4..6a12539d06cbf 100644 --- a/common/autoware_test_utils/CMakeLists.txt +++ b/common/autoware_test_utils/CMakeLists.txt @@ -15,11 +15,10 @@ target_link_libraries(mock_data_parser ) if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_mock_data_parser - test/test_mock_data_parser.cpp) - - target_link_libraries(test_mock_data_parser - mock_data_parser) + ament_auto_add_gtest(test_autoware_test_utils + test/test_mock_data_parser.cpp + test/test_autoware_test_manager.cpp + ) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp index 6b77bb4c24931..d7de1397bfaac 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp @@ -15,15 +15,12 @@ #ifndef AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_ #define AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_ -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "rclcpp/rclcpp.hpp" - +#include #include #include #include #include #include -#include #include #include @@ -51,7 +48,7 @@ #include #include #include -#include +#include #include namespace autoware::test_utils @@ -66,6 +63,8 @@ using autoware_planning_msgs::msg::Trajectory; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using RouteSections = std::vector; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; @@ -74,8 +73,6 @@ using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using tf2_msgs::msg::TFMessage; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; using tier4_planning_msgs::msg::Scenario; using unique_identifier_msgs::msg::UUID; @@ -174,6 +171,45 @@ LaneletRoute makeNormalRoute(); */ OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2); +/** + * @brief Get the absolute path to the lanelet map file. + * + * This function retrieves the absolute path to a lanelet map file located in the + * package's share directory under the "test_map" folder. + * + * @param package_name The name of the package containing the map file. + * @param map_filename The name of the map file. + * @return A string representing the absolute path to the lanelet map file. + */ +std::string get_absolute_path_to_lanelet_map( + const std::string & package_name, const std::string & map_filename); + +/** + * @brief Get the absolute path to the route file. + * + * This function retrieves the absolute path to a route file located in the + * package's share directory under the "test_route" folder. + * + * @param package_name The name of the package containing the route file. + * @param route_filename The name of the route file. + * @return A string representing the absolute path to the route file. + */ +std::string get_absolute_path_to_route( + const std::string & package_name, const std::string & route_filename); + +/** + * @brief Get the absolute path to the config file. + * + * This function retrieves the absolute path to a route file located in the + * package's share directory under the "config" folder. + * + * @param package_name The name of the package containing the route file. + * @param route_filename The name of the config file. + * @return A string representing the absolute path to the config file. + */ +std::string get_absolute_path_to_config( + const std::string & package_name, const std::string & config_filename); + /** * @brief Creates a LaneletMapBin message from a Lanelet map file. * @@ -477,6 +513,62 @@ void publishToTargetNode( autoware::test_utils::spinSomeNodes(test_node, target_node, repeat_count); } +/** + * @brief Manages publishing and subscribing to ROS topics for testing Autoware. + * + * The AutowareTestManager class provides utility functions to facilitate + * the publishing of messages to specified topics and the setting up of + * subscribers to listen for messages on specified topics. This class + * simplifies the setup of test environments in Autoware. + */ +class AutowareTestManager +{ +public: + AutowareTestManager() + { + test_node_ = std::make_shared("autoware_test_manager_node"); + } + + template + void test_pub_msg( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name, MessageType & msg) + { + if (publishers_.find(topic_name) == publishers_.end()) { + auto publisher = test_node_->create_publisher(topic_name, 10); + publishers_[topic_name] = std::static_pointer_cast(publisher); + } + + auto publisher = + std::dynamic_pointer_cast>(publishers_[topic_name]); + + autoware::test_utils::publishToTargetNode(test_node_, target_node, topic_name, publisher, msg); + RCLCPP_INFO(test_node_->get_logger(), "Published message on topic '%s'", topic_name.c_str()); + } + + template + void set_subscriber( + const std::string & topic_name, + std::function callback) + { + if (subscribers_.find(topic_name) == subscribers_.end()) { + std::shared_ptr> subscriber; + autoware::test_utils::createSubscription( + test_node_, topic_name, callback, subscriber); + subscribers_[topic_name] = std::static_pointer_cast(subscriber); + } else { + RCLCPP_WARN(test_node_->get_logger(), "Subscriber %s already set.", topic_name.c_str()); + } + } + +protected: + // Publisher + std::unordered_map> publishers_; + std::unordered_map> subscribers_; + + // Node + rclcpp::Node::SharedPtr test_node_; +}; // class AutowareTestManager + } // namespace autoware::test_utils #endif // AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_ diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml index 555345c1d9105..778b9a42ca207 100644 --- a/common/autoware_test_utils/package.xml +++ b/common/autoware_test_utils/package.xml @@ -19,6 +19,7 @@ autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_universe_utils autoware_vehicle_msgs component_interface_specs component_interface_utils @@ -29,7 +30,6 @@ tf2_msgs tf2_ros tier4_api_msgs - tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs unique_identifier_msgs diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp index c7435be5afcec..563350dbe53cc 100644 --- a/common/autoware_test_utils/src/autoware_test_utils.cpp +++ b/common/autoware_test_utils/src/autoware_test_utils.cpp @@ -12,7 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_test_utils/autoware_test_utils.hpp" +#include +#include +#include +#include +#include +#include + +#include + namespace autoware::test_utils { @@ -103,6 +111,27 @@ OccupancyGrid makeCostMapMsg(size_t width, size_t height, double resolution) return costmap_msg; } +std::string get_absolute_path_to_lanelet_map( + const std::string & package_name, const std::string & map_filename) +{ + const auto dir = ament_index_cpp::get_package_share_directory(package_name); + return dir + "/test_map/" + map_filename; +} + +std::string get_absolute_path_to_route( + const std::string & package_name, const std::string & route_filename) +{ + const auto dir = ament_index_cpp::get_package_share_directory(package_name); + return dir + "/test_route/" + route_filename; +} + +std::string get_absolute_path_to_config( + const std::string & package_name, const std::string & config_filename) +{ + const auto dir = ament_index_cpp::get_package_share_directory(package_name); + return dir + "/config/" + config_filename; +} + LaneletMapBin make_map_bin_msg( const std::string & absolute_path, const double center_line_resolution) { @@ -122,12 +151,8 @@ LaneletMapBin make_map_bin_msg( LaneletMapBin makeMapBinMsg() { - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto lanelet2_path = autoware_test_utils_dir + "/test_map/lanelet2_map.osm"; - double center_line_resolution = 5.0; - - return make_map_bin_msg(lanelet2_path, center_line_resolution); + return make_map_bin_msg( + get_absolute_path_to_lanelet_map("autoware_test_utils", "lanelet2_map.osm")); } Odometry makeOdometry(const double shift) @@ -251,9 +276,8 @@ void updateNodeOptions( PathWithLaneId loadPathWithLaneIdInYaml() { - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto yaml_path = autoware_test_utils_dir + "/config/path_with_lane_id_data.yaml"; + const auto yaml_path = + get_absolute_path_to_config("autoware_test_utils", "path_with_lane_id_data.yaml"); YAML::Node yaml_node = YAML::LoadFile(yaml_path); PathWithLaneId path_msg; diff --git a/common/autoware_test_utils/test/test_autoware_test_manager.cpp b/common/autoware_test_utils/test/test_autoware_test_manager.cpp new file mode 100644 index 0000000000000..688ec9df9ad42 --- /dev/null +++ b/common/autoware_test_utils/test/test_autoware_test_manager.cpp @@ -0,0 +1,71 @@ +// Copyright 2024 TIER IV +// +// 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 "autoware_test_utils/autoware_test_utils.hpp" + +#include + +#include + +#include + +class RelayNode : public rclcpp::Node +{ +public: + RelayNode() : Node("relay_node") + { + subscription_ = this->create_subscription( + "input_topic", 10, [this](const std_msgs::msg::String::ConstSharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received message: %s", msg->data.c_str()); + auto new_msg = std::make_shared(); + new_msg->data = msg->data; + publisher_->publish(*new_msg); + }); + + publisher_ = this->create_publisher("output_topic", 10); + } + +private: + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Publisher::SharedPtr publisher_; +}; + +TEST(AutowareTestManagerTest, TestRelayNode) +{ + rclcpp::init(0, nullptr); + + const std::string input_topic_name = "input_topic"; + const std::string output_topic_name = "output_topic"; + + // Setup target node and its test manager + auto target_node = std::make_shared(); + auto manager = std::make_shared(); + + // Setup subscriber for test manager + std::string received_msg; + manager->set_subscriber( + "output_topic", + [&received_msg](const std_msgs::msg::String::ConstSharedPtr msg) { received_msg = msg->data; }); + + // Publish a message to the relay node + std_msgs::msg::String msg; + msg.data = "Hello, Relay!"; + manager->test_pub_msg(target_node, input_topic_name, msg); + + // Spin to process callbacks + rclcpp::spin_some(target_node); + + // Check that the message was relayed and received by the test node + EXPECT_EQ(received_msg, "Hello, Relay!"); +} diff --git a/common/autoware_test_utils/test_map/overlap_map.osm b/common/autoware_test_utils/test_map/overlap_map.osm new file mode 100644 index 0000000000000..d4fec4f72aeaa --- /dev/null +++ b/common/autoware_test_utils/test_map/overlap_map.osm @@ -0,0 +1,974 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt similarity index 69% rename from common/tier4_autoware_utils/CMakeLists.txt rename to common/autoware_universe_utils/CMakeLists.txt index 9cb54e52362a5..9e86ddeb60692 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.14) -project(tier4_autoware_utils) +project(autoware_universe_utils) find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) -ament_auto_add_library(tier4_autoware_utils SHARED +ament_auto_add_library(autoware_universe_utils SHARED src/geometry/geometry.cpp src/geometry/pose_deviation.cpp src/geometry/boost_polygon_utils.cpp @@ -23,10 +23,10 @@ if(BUILD_TESTING) file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(test_tier4_autoware_utils ${test_files}) + ament_add_ros_isolated_gtest(test_autoware_universe_utils ${test_files}) - target_link_libraries(test_tier4_autoware_utils - tier4_autoware_utils + target_link_libraries(test_autoware_universe_utils + autoware_universe_utils ) endif() diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md new file mode 100644 index 0000000000000..22b9355b09635 --- /dev/null +++ b/common/autoware_universe_utils/README.md @@ -0,0 +1,9 @@ +# autoware_universe_utils + +## Purpose + +This package contains many common functions used by other packages, so please refer to them as needed. + +## For developers + +`autoware_universe_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_geometry.hpp similarity index 84% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_geometry.hpp index d986d0f23fc85..a0e24c68b7918 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_geometry.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ #include #include @@ -25,7 +25,7 @@ #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { // 2D struct Point2d; @@ -93,12 +93,12 @@ inline Point3d fromMsg(const geometry_msgs::msg::Point & msg) point.z() = msg.z; return point; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT - tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT -BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT - tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT -BOOST_GEOMETRY_REGISTER_RING(tier4_autoware_utils::LinearRing2d) // NOLINT +BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLINT + autoware::universe_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT +BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT + autoware::universe_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT +BOOST_GEOMETRY_REGISTER_RING(autoware::universe_utils::LinearRing2d) // NOLINT -#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp similarity index 85% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp index 665e959ed2460..1313ec558fe01 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include #include @@ -24,7 +24,7 @@ #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { bool isClockwise(const Polygon2d & polygon); Polygon2d inverseClockwise(const Polygon2d & polygon); @@ -47,6 +47,6 @@ Polygon2d toFootprint( const double base_to_rear, const double width); double getArea(const autoware_perception_msgs::msg::Shape & shape); Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset); -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp similarity index 97% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp index 334df8f32af91..87c06dfd9cf08 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GEOMETRY_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GEOMETRY_HPP_ -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -96,7 +96,7 @@ inline void doTransform( #endif } // namespace tf2 -namespace tier4_autoware_utils +namespace autoware::universe_utils { template geometry_msgs::msg::Point getPoint(const T & p) @@ -577,6 +577,6 @@ std::optional intersect( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GEOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/pose_deviation.hpp similarity index 82% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/geometry/pose_deviation.hpp index dfa678eaa07d9..c3a602caf4232 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/pose_deviation.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { struct PoseDeviation { @@ -39,6 +39,6 @@ double calcYawDeviation( PoseDeviation calcPoseDeviation( const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose); -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp similarity index 74% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp index 984fa04016b57..f06770c2925c7 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ -namespace tier4_autoware_utils +namespace autoware::universe_utils { constexpr double pi = 3.14159265358979323846; // To be replaced by std::numbers::pi in C++20 constexpr double gravity = 9.80665; -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/normalization.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/normalization.hpp similarity index 79% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/normalization.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/normalization.hpp index 1eb32f33886cf..16af7f44415cc 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/normalization.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/normalization.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__NORMALIZATION_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__NORMALIZATION_HPP_ -#include "tier4_autoware_utils/math/constants.hpp" +#include "autoware/universe_utils/math/constants.hpp" #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { inline double normalizeDegree(const double deg, const double min_deg = -180) { @@ -45,6 +45,6 @@ inline double normalizeRadian(const double rad, const double min_rad = -pi) return value - std::copysign(2 * pi, value); } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__NORMALIZATION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/range.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/range.hpp similarity index 89% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/range.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/range.hpp index 957a6f4e729e5..ed0ef455bfcfe 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/range.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/range.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__RANGE_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__RANGE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__RANGE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__RANGE_HPP_ #include #include #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { template std::vector arange(const T start, const T stop, const T step = 1) @@ -73,6 +73,6 @@ std::vector linspace(const T start, const T stop, const size_t num) return out; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__RANGE_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__RANGE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/sin_table.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/sin_table.hpp similarity index 76% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/sin_table.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/sin_table.hpp index bea3b1e0ecb46..e87d8a2e5fc40 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/sin_table.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/sin_table.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__SIN_TABLE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__SIN_TABLE_HPP_ #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { constexpr size_t sin_table_size = 32769; @@ -25,6 +25,6 @@ constexpr size_t discrete_arcs_num_90 = 32768; constexpr size_t discrete_arcs_num_360 = 131072; extern const float g_sin_table[sin_table_size]; -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__SIN_TABLE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp similarity index 73% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp index 4901e28472acb..19a59523c7f08 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_ #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { float sin(float radian); @@ -26,6 +26,6 @@ float cos(float radian); std::pair sin_and_cos(float radian); -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp similarity index 74% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp index 38a0a7696775a..36ce8e9f39514 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__UNIT_CONVERSION_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__UNIT_CONVERSION_HPP_ -#include "tier4_autoware_utils/math/constants.hpp" +#include "autoware/universe_utils/math/constants.hpp" -namespace tier4_autoware_utils +namespace autoware::universe_utils { constexpr double deg2rad(const double deg) { @@ -36,6 +36,6 @@ constexpr double mps2kmph(const double mps) { return mps * 3600.0 / 1000.0; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__UNIT_CONVERSION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_publisher.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_publisher.hpp similarity index 82% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_publisher.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_publisher.hpp index 0750b6894debe..acd82995b1d84 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_publisher.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_publisher.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ -#include "tier4_autoware_utils/ros/debug_traits.hpp" +#include "autoware/universe_utils/ros/debug_traits.hpp" #include #include @@ -25,14 +25,15 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { namespace debug_publisher { template < class T_msg, class T, std::enable_if_t< - tier4_autoware_utils::debug_traits::is_debug_message::value, std::nullptr_t> = nullptr> + autoware::universe_utils::debug_traits::is_debug_message::value, std::nullptr_t> = + nullptr> T_msg toDebugMsg(const T & data, const rclcpp::Time & stamp) { T_msg msg; @@ -72,6 +73,6 @@ class DebugPublisher const char * ns_; std::unordered_map> pub_map_; }; -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_traits.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp similarity index 89% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_traits.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp index 345e49079eede..8420f930e0ce9 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/debug_traits.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ #include #include @@ -28,7 +28,7 @@ #include -namespace tier4_autoware_utils::debug_traits +namespace autoware::universe_utils::debug_traits { template struct is_debug_message : std::false_type @@ -84,6 +84,6 @@ template <> struct is_debug_message : std::true_type { }; -} // namespace tier4_autoware_utils::debug_traits +} // namespace autoware::universe_utils::debug_traits -#endif // TIER4_AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/logger_level_configure.hpp similarity index 80% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/logger_level_configure.hpp index 5aee3a251dad2..b1b11d33ab448 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/logger_level_configure.hpp @@ -22,12 +22,12 @@ // =============== How to use =============== // ___In your_node.hpp___ -// #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +// #include "autoware/universe_utils/ros/logger_level_configure.hpp" // class YourNode : public rclcpp::Node { // ... // // // Define logger_configure as a node class member variable -// std::unique_ptr logger_configure_; +// std::unique_ptr logger_configure_; // } // // ___In your_node.cpp___ @@ -38,14 +38,14 @@ // logger_configure_ = std::make_unique(this); // } -#ifndef TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ #include "logging_demo/srv/config_logger.hpp" #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { class LoggerLevelConfigure { @@ -64,5 +64,5 @@ class LoggerLevelConfigure const ConfigLogger::Response::SharedPtr response); }; -} // namespace tier4_autoware_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ +} // namespace autoware::universe_utils +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp similarity index 89% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp index c00c3c1c364df..6991962c420d1 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__MARKER_HELPER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__MARKER_HELPER_HPP_ #include @@ -22,7 +22,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) { @@ -76,6 +76,6 @@ void appendMarkerArray( visualization_msgs::msg::MarkerArray * marker_array, const std::optional & current_time = {}); -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__MARKER_HELPER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_covariance.hpp similarity index 90% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_covariance.hpp index 4ebf81d4bfda5..ff957a878440e 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_covariance.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__MSG_COVARIANCE_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__MSG_COVARIANCE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__MSG_COVARIANCE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__MSG_COVARIANCE_HPP_ -namespace tier4_autoware_utils +namespace autoware::universe_utils { namespace xyz_covariance_index { @@ -115,6 +115,6 @@ enum XYZ_UPPER_COV_IDX { Z_Z = 5, }; } // namespace xyz_upper_covariance_index -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__MSG_COVARIANCE_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__MSG_COVARIANCE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_operation.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_operation.hpp similarity index 79% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_operation.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_operation.hpp index 4ac5dc05204f9..5ac3441dd2fc4 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_operation.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/msg_operation.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__MSG_OPERATION_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__MSG_OPERATION_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__MSG_OPERATION_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__MSG_OPERATION_HPP_ #include "geometry_msgs/msg/quaternion.hpp" -// NOTE: Do not use tier4_autoware_utils namespace +// NOTE: Do not use autoware_universe_utils namespace namespace geometry_msgs { namespace msg @@ -28,4 +28,4 @@ Quaternion operator-(Quaternion a, Quaternion b) noexcept; } // namespace msg } // namespace geometry_msgs -#endif // TIER4_AUTOWARE_UTILS__ROS__MSG_OPERATION_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__MSG_OPERATION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/parameter.hpp similarity index 78% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/parameter.hpp index ac9b124a02cdb..89846688d365d 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/parameter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__PARAMETER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__PARAMETER_HPP_ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { template T getOrDeclareParameter(rclcpp::Node & node, const std::string & name) @@ -30,6 +30,6 @@ T getOrDeclareParameter(rclcpp::Node & node, const std::string & name) return node.declare_parameter(name); } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__PARAMETER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/pcl_conversion.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/pcl_conversion.hpp similarity index 90% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/pcl_conversion.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/pcl_conversion.hpp index c66a63eb1ac51..7fab38790921a 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/pcl_conversion.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/pcl_conversion.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__PCL_CONVERSION_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__PCL_CONVERSION_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__PCL_CONVERSION_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__PCL_CONVERSION_HPP_ #include #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { /** * @brief a faster implementation of converting sensor_msgs::msg::PointCloud2 to @@ -67,6 +67,6 @@ void transformPointCloudFromROSMsg( } } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__PCL_CONVERSION_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__PCL_CONVERSION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp similarity index 92% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp index d0b6dfac2dcff..1b7ea0bd69951 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ #include @@ -23,9 +23,16 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { +inline rclcpp::SensorDataQoS SingleDepthSensorQoS() +{ + rclcpp::SensorDataQoS qos; + qos.get_rmw_qos_profile().depth = 1; + return qos; +} + template class InterProcessPollingSubscriber; @@ -152,6 +159,6 @@ class InterProcessPollingSubscriber= 2)>::typ }; }; -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/processing_time_publisher.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/processing_time_publisher.hpp similarity index 86% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/processing_time_publisher.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/processing_time_publisher.hpp index 80d394a1c200e..96cddc595a5b9 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/processing_time_publisher.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/processing_time_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { class ProcessingTimePublisher { @@ -62,6 +62,6 @@ class ProcessingTimePublisher return oss.str(); } }; -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/published_time_publisher.hpp similarity index 92% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/published_time_publisher.hpp index 21705cab9a962..60112d9e83cf3 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/published_time_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ #include @@ -26,7 +26,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { class PublishedTimePublisher @@ -109,6 +109,6 @@ class PublishedTimePublisher // store them for each different publisher of the node std::map::SharedPtr, GidCompare> publishers_; }; -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/self_pose_listener.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/self_pose_listener.hpp similarity index 77% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/self_pose_listener.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/self_pose_listener.hpp index 8148392ecda3b..cdcf78a9c5edc 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/self_pose_listener.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/self_pose_listener.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/transform_listener.hpp" #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { class SelfPoseListener { @@ -53,6 +53,6 @@ class SelfPoseListener private: TransformListener transform_listener_; }; -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__SELF_POSE_LISTENER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/transform_listener.hpp similarity index 91% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/transform_listener.hpp index 96b1cc6ea6ccc..e053ef3668034 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/transform_listener.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ #include @@ -26,7 +26,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { class TransformListener { @@ -82,6 +82,6 @@ class TransformListener std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; }; -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__TRANSFORM_LISTENER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/update_param.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/update_param.hpp similarity index 80% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/update_param.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/update_param.hpp index 36abcc11175e1..bebb38af0b261 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/update_param.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/update_param.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__UPDATE_PARAM_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__UPDATE_PARAM_HPP_ #include #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { template bool updateParam(const std::vector & params, const std::string & name, T & value) @@ -37,6 +37,6 @@ bool updateParam(const std::vector & params, const std::strin value = itr->template get_value(); return true; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__UPDATE_PARAM_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/uuid_helper.hpp similarity index 87% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/uuid_helper.hpp index efb5564bdaa71..62c9f75f3f233 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/uuid_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__UUID_HELPER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__UUID_HELPER_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { inline unique_identifier_msgs::msg::UUID generateUUID() { @@ -58,6 +58,6 @@ inline unique_identifier_msgs::msg::UUID toUUIDMsg(const boost::uuids::uuid & id return ros_uuid; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__UUID_HELPER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/wait_for_param.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/wait_for_param.hpp similarity index 84% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/wait_for_param.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/wait_for_param.hpp index 67a1249c5b042..1faaaa861b1c4 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/wait_for_param.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/wait_for_param.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ #include @@ -21,7 +21,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { template T waitForParam( @@ -47,6 +47,6 @@ T waitForParam( return {}; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp similarity index 71% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp index c784d71ad5060..0b025160bacf8 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ -#define TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__SYSTEM__BACKTRACE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__BACKTRACE_HPP_ -namespace tier4_autoware_utils +namespace autoware::universe_utils { void print_backtrace(); -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__BACKTRACE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/stop_watch.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/stop_watch.hpp similarity index 87% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/system/stop_watch.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/system/stop_watch.hpp index 0d804e797936c..eb5cb546b8c7f 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/stop_watch.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/stop_watch.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ -#define TIER4_AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__SYSTEM__STOP_WATCH_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__STOP_WATCH_HPP_ #include #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { template < class OutputUnit = std::chrono::seconds, class InternalUnit = std::chrono::microseconds, @@ -58,6 +58,6 @@ class StopWatch std::unordered_map t_start_; }; -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__SYSTEM__STOP_WATCH_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__STOP_WATCH_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/transform/transforms.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/transform/transforms.hpp similarity index 85% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/transform/transforms.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/transform/transforms.hpp index ea88ea7624216..8a92116b426ec 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/transform/transforms.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/transform/transforms.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_ -#define TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__TRANSFORM__TRANSFORMS_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__TRANSFORM__TRANSFORMS_HPP_ #include #include @@ -21,7 +21,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { template void transformPointCloud( @@ -46,6 +46,6 @@ void transformPointCloud( pcl::transformPointCloud(cloud_in, cloud_out, transform); } } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils -#endif // TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__TRANSFORM__TRANSFORMS_HPP_ diff --git a/common/tier4_autoware_utils/package.xml b/common/autoware_universe_utils/package.xml similarity index 93% rename from common/tier4_autoware_utils/package.xml rename to common/autoware_universe_utils/package.xml index 334ee226a5f22..de0461b5a841d 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/autoware_universe_utils/package.xml @@ -1,9 +1,9 @@ - tier4_autoware_utils + autoware_universe_utils 0.1.0 - The tier4_autoware_utils package + The autoware_universe_utils package Takamasa Horibe Takayuki Murooka Mamoru Sobue diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/autoware_universe_utils/src/geometry/boost_polygon_utils.cpp similarity index 85% rename from common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp rename to common/autoware_universe_utils/src/geometry/boost_polygon_utils.cpp index 01e02d1cf0038..da3da42cbc41b 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/autoware_universe_utils/src/geometry/boost_polygon_utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -23,8 +23,8 @@ namespace { namespace bg = boost::geometry; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { @@ -68,7 +68,7 @@ double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions) } } // namespace -namespace tier4_autoware_utils +namespace autoware::universe_utils { bool isClockwise(const Polygon2d & polygon) { @@ -123,16 +123,16 @@ Polygon2d toPolygon2d( Polygon2d polygon; if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - const auto point0 = tier4_autoware_utils::calcOffsetPose( + const auto point0 = autoware::universe_utils::calcOffsetPose( pose, shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) .position; - const auto point1 = tier4_autoware_utils::calcOffsetPose( + const auto point1 = autoware::universe_utils::calcOffsetPose( pose, -shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) .position; - const auto point2 = tier4_autoware_utils::calcOffsetPose( + const auto point2 = autoware::universe_utils::calcOffsetPose( pose, -shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) .position; - const auto point3 = tier4_autoware_utils::calcOffsetPose( + const auto point3 = autoware::universe_utils::calcOffsetPose( pose, shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) .position; @@ -168,7 +168,7 @@ Polygon2d toPolygon2d( appendPointToPolygon(polygon, abs_point); } } else { - throw std::logic_error("The shape type is not supported in tier4_autoware_utils."); + throw std::logic_error("The shape type is not supported in autoware_universe_utils."); } // NOTE: push back the first point in order to close polygon @@ -179,24 +179,24 @@ Polygon2d toPolygon2d( return isClockwise(polygon) ? polygon : inverseClockwise(polygon); } -tier4_autoware_utils::Polygon2d toPolygon2d( +autoware::universe_utils::Polygon2d toPolygon2d( const autoware_perception_msgs::msg::DetectedObject & object) { - return tier4_autoware_utils::toPolygon2d( + return autoware::universe_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } -tier4_autoware_utils::Polygon2d toPolygon2d( +autoware::universe_utils::Polygon2d toPolygon2d( const autoware_perception_msgs::msg::TrackedObject & object) { - return tier4_autoware_utils::toPolygon2d( + return autoware::universe_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } -tier4_autoware_utils::Polygon2d toPolygon2d( +autoware::universe_utils::Polygon2d toPolygon2d( const autoware_perception_msgs::msg::PredictedObject & object) { - return tier4_autoware_utils::toPolygon2d( + return autoware::universe_utils::toPolygon2d( object.kinematics.initial_pose_with_covariance.pose, object.shape); } @@ -206,13 +206,17 @@ Polygon2d toFootprint( { Polygon2d polygon; const auto point0 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, base_to_front, width / 2.0, 0.0).position; + autoware::universe_utils::calcOffsetPose(base_link_pose, base_to_front, width / 2.0, 0.0) + .position; const auto point1 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, base_to_front, -width / 2.0, 0.0).position; + autoware::universe_utils::calcOffsetPose(base_link_pose, base_to_front, -width / 2.0, 0.0) + .position; const auto point2 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, -width / 2.0, 0.0).position; + autoware::universe_utils::calcOffsetPose(base_link_pose, -base_to_rear, -width / 2.0, 0.0) + .position; const auto point3 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, width / 2.0, 0.0).position; + autoware::universe_utils::calcOffsetPose(base_link_pose, -base_to_rear, width / 2.0, 0.0) + .position; appendPointToPolygon(polygon, point0); appendPointToPolygon(polygon, point1); @@ -233,7 +237,7 @@ double getArea(const autoware_perception_msgs::msg::Shape & shape) return getPolygonArea(shape.footprint); } - throw std::logic_error("The shape type is not supported in tier4_autoware_utils."); + throw std::logic_error("The shape type is not supported in autoware_universe_utils."); } // NOTE: The number of vertices on the expanded polygon by boost::geometry::buffer @@ -268,4 +272,4 @@ Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset) boost::geometry::correct(expanded_polygon); return expanded_polygon; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils diff --git a/common/tier4_autoware_utils/src/geometry/geometry.cpp b/common/autoware_universe_utils/src/geometry/geometry.cpp similarity index 98% rename from common/tier4_autoware_utils/src/geometry/geometry.cpp rename to common/autoware_universe_utils/src/geometry/geometry.cpp index b88646b73dd7c..1f4fd318e227b 100644 --- a/common/tier4_autoware_utils/src/geometry/geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/geometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -32,7 +32,7 @@ void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped().x(point32.x).y(point32.y).z(point32.z); - const auto transformed_point = tier4_autoware_utils::transformPoint(point, pose); + const auto transformed_point = autoware::universe_utils::transformPoint(point, pose); return geometry_msgs::build() .x(transformed_point.x) .y(transformed_point.y) @@ -383,4 +383,4 @@ std::optional intersect( return intersect_point; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils diff --git a/common/tier4_autoware_utils/src/geometry/pose_deviation.cpp b/common/autoware_universe_utils/src/geometry/pose_deviation.cpp similarity index 92% rename from common/tier4_autoware_utils/src/geometry/pose_deviation.cpp rename to common/autoware_universe_utils/src/geometry/pose_deviation.cpp index dda8afb974d26..77849e73f5dfd 100644 --- a/common/tier4_autoware_utils/src/geometry/pose_deviation.cpp +++ b/common/autoware_universe_utils/src/geometry/pose_deviation.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include @@ -28,7 +28,7 @@ #include #endif -namespace tier4_autoware_utils +namespace autoware::universe_utils { double calcLateralDeviation( @@ -82,4 +82,4 @@ PoseDeviation calcPoseDeviation( return deviation; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils diff --git a/common/tier4_autoware_utils/src/math/sin_table.cpp b/common/autoware_universe_utils/src/math/sin_table.cpp similarity index 99% rename from common/tier4_autoware_utils/src/math/sin_table.cpp rename to common/autoware_universe_utils/src/math/sin_table.cpp index 0657c2303226f..ce021869176ee 100644 --- a/common/tier4_autoware_utils/src/math/sin_table.cpp +++ b/common/autoware_universe_utils/src/math/sin_table.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/sin_table.hpp" +#include "autoware/universe_utils/math/sin_table.hpp" -namespace tier4_autoware_utils +namespace autoware::universe_utils { const float g_sin_table[sin_table_size] = { @@ -8212,4 +8212,4 @@ const float g_sin_table[sin_table_size] = { 0.9999999816164293f, 0.9999999896592414f, 0.9999999954041073f, 0.9999999988510269f, 1.0000000000000000f}; -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp similarity index 81% rename from common/tier4_autoware_utils/src/math/trigonometry.cpp rename to common/autoware_universe_utils/src/math/trigonometry.cpp index 0ce65c7aa5bc8..586b9075ba6d5 100644 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ b/common/autoware_universe_utils/src/math/trigonometry.cpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/trigonometry.hpp" +#include "autoware/universe_utils/math/trigonometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/sin_table.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/sin_table.hpp" #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { float sin(float radian) { - float degree = radian * (180.f / static_cast(tier4_autoware_utils::pi)) * + float degree = radian * (180.f / static_cast(autoware::universe_utils::pi)) * (discrete_arcs_num_360 / 360.f); size_t idx = (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % @@ -46,13 +46,13 @@ float sin(float radian) float cos(float radian) { - return sin(radian + static_cast(tier4_autoware_utils::pi) / 2.f); + return sin(radian + static_cast(autoware::universe_utils::pi) / 2.f); } std::pair sin_and_cos(float radian) { constexpr float tmp = - (180.f / static_cast(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f); + (180.f / static_cast(autoware::universe_utils::pi)) * (discrete_arcs_num_360 / 360.f); const float degree = radian * tmp; size_t idx = (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % @@ -72,4 +72,4 @@ std::pair sin_and_cos(float radian) } } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils diff --git a/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp b/common/autoware_universe_utils/src/ros/logger_level_configure.cpp similarity index 93% rename from common/tier4_autoware_utils/src/ros/logger_level_configure.cpp rename to common/autoware_universe_utils/src/ros/logger_level_configure.cpp index d764299290d05..d517b81b93224 100644 --- a/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp +++ b/common/autoware_universe_utils/src/ros/logger_level_configure.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { LoggerLevelConfigure::LoggerLevelConfigure(rclcpp::Node * node) : ros_logger_(node->get_logger()) { @@ -58,4 +58,4 @@ void LoggerLevelConfigure::onLoggerConfigService( return; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils diff --git a/common/tier4_autoware_utils/src/ros/marker_helper.cpp b/common/autoware_universe_utils/src/ros/marker_helper.cpp similarity index 94% rename from common/tier4_autoware_utils/src/ros/marker_helper.cpp rename to common/autoware_universe_utils/src/ros/marker_helper.cpp index fda997edc83db..507be41dea7bb 100644 --- a/common/tier4_autoware_utils/src/ros/marker_helper.cpp +++ b/common/autoware_universe_utils/src/ros/marker_helper.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" -namespace tier4_autoware_utils +namespace autoware::universe_utils { visualization_msgs::msg::Marker createDefaultMarker( @@ -68,4 +68,4 @@ void appendMarkerArray( } } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils diff --git a/common/tier4_autoware_utils/src/ros/msg_operation.cpp b/common/autoware_universe_utils/src/ros/msg_operation.cpp similarity index 92% rename from common/tier4_autoware_utils/src/ros/msg_operation.cpp rename to common/autoware_universe_utils/src/ros/msg_operation.cpp index cc1a59317a8e0..02c24b2a6cb33 100644 --- a/common/tier4_autoware_utils/src/ros/msg_operation.cpp +++ b/common/autoware_universe_utils/src/ros/msg_operation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/ros/msg_operation.hpp" +#include "autoware/universe_utils/ros/msg_operation.hpp" #include @@ -22,7 +22,7 @@ #include #endif -// NOTE: Do not use tier4_autoware_utils namespace +// NOTE: Do not use autoware_universe_utils namespace namespace geometry_msgs { namespace msg diff --git a/common/tier4_autoware_utils/src/system/backtrace.cpp b/common/autoware_universe_utils/src/system/backtrace.cpp similarity index 85% rename from common/tier4_autoware_utils/src/system/backtrace.cpp rename to common/autoware_universe_utils/src/system/backtrace.cpp index 343f200296cad..7af91bdcc60dc 100644 --- a/common/tier4_autoware_utils/src/system/backtrace.cpp +++ b/common/autoware_universe_utils/src/system/backtrace.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/system/backtrace.hpp" +#include "autoware/universe_utils/system/backtrace.hpp" #include "rclcpp/rclcpp.hpp" @@ -23,7 +23,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { void print_backtrace() @@ -44,9 +44,9 @@ void print_backtrace() for (int i = 1; i < addrlen; i++) { ss << " @ " << symbol_list[i] << std::endl; } - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("tier4_autoware_utils"), ss.str()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("autoware_universe_utils"), ss.str()); free(symbol_list); } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils diff --git a/common/tier4_autoware_utils/test/src/geometry/test_boost_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_boost_geometry.cpp similarity index 88% rename from common/tier4_autoware_utils/test/src/geometry/test_boost_geometry.cpp rename to common/autoware_universe_utils/test/src/geometry/test_boost_geometry.cpp index 4e5483935da00..7c688e1982838 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_boost_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_boost_geometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include @@ -20,8 +20,8 @@ namespace bg = boost::geometry; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Point3d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Point3d; TEST(boost_geometry, boost_geometry_distance) { @@ -54,7 +54,7 @@ TEST(boost_geometry, to_2d) TEST(boost_geometry, toMsg) { - using tier4_autoware_utils::toMsg; + using autoware::universe_utils::toMsg; { const Point3d p(1.0, 2.0, 3.0); @@ -68,7 +68,7 @@ TEST(boost_geometry, toMsg) TEST(boost_geometry, fromMsg) { - using tier4_autoware_utils::fromMsg; + using autoware::universe_utils::fromMsg; geometry_msgs::msg::Point p_msg; p_msg.x = 1.0; diff --git a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp b/common/autoware_universe_utils/test/src/geometry/test_boost_polygon_utils.cpp similarity index 95% rename from common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp rename to common/autoware_universe_utils/test/src/geometry/test_boost_polygon_utils.cpp index 7689544bd79cc..4b75e8130a253 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_boost_polygon_utils.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include -using tier4_autoware_utils::Polygon2d; +using autoware::universe_utils::Polygon2d; namespace { @@ -39,7 +39,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double p.position.x = x; p.position.y = y; p.position.z = 0.0; - p.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + p.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); return p; } @@ -47,7 +47,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double TEST(boost_geometry, boost_isClockwise) { - using tier4_autoware_utils::isClockwise; + using autoware::universe_utils::isClockwise; // empty Polygon2d empty_polygon; @@ -72,8 +72,8 @@ TEST(boost_geometry, boost_isClockwise) TEST(boost_geometry, boost_inverseClockwise) { - using tier4_autoware_utils::inverseClockwise; - using tier4_autoware_utils::isClockwise; + using autoware::universe_utils::inverseClockwise; + using autoware::universe_utils::isClockwise; // empty Polygon2d empty_polygon; @@ -100,7 +100,7 @@ TEST(boost_geometry, boost_inverseClockwise) TEST(boost_geometry, boost_rotatePolygon) { constexpr double epsilon = 1e-6; - using tier4_autoware_utils::rotatePolygon; + using autoware::universe_utils::rotatePolygon; // empty geometry_msgs::msg::Polygon empty_polygon; @@ -130,7 +130,7 @@ TEST(boost_geometry, boost_rotatePolygon) TEST(boost_geometry, boost_toPolygon2d) { - using tier4_autoware_utils::toPolygon2d; + using autoware::universe_utils::toPolygon2d; { // bounding box const double x = 1.0; @@ -206,7 +206,7 @@ TEST(boost_geometry, boost_toPolygon2d) TEST(boost_geometry, boost_toFootprint) { - using tier4_autoware_utils::toFootprint; + using autoware::universe_utils::toFootprint; // from base link { @@ -234,7 +234,7 @@ TEST(boost_geometry, boost_toFootprint) TEST(boost_geometry, boost_getArea) { - using tier4_autoware_utils::getArea; + using autoware::universe_utils::getArea; { // bounding box const double x = 1.0; @@ -290,7 +290,7 @@ TEST(boost_geometry, boost_getArea) TEST(boost_geometry, boost_expandPolygon) { - using tier4_autoware_utils::expandPolygon; + using autoware::universe_utils::expandPolygon; { // box with a certain offset Polygon2d box_poly{{{-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}}}; diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp similarity index 90% rename from common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp rename to common/autoware_universe_utils/test/src/geometry/test_geometry.cpp index b492da2d72e88..194cd7d503d12 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include @@ -25,7 +25,7 @@ constexpr double epsilon = 1e-6; TEST(geometry, getPoint) { - using tier4_autoware_utils::getPoint; + using autoware::universe_utils::getPoint; const double x_ans = 1.0; const double y_ans = 2.0; @@ -114,7 +114,7 @@ TEST(geometry, getPoint) TEST(geometry, getPose) { - using tier4_autoware_utils::getPose; + using autoware::universe_utils::getPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -203,7 +203,7 @@ TEST(geometry, getPose) TEST(geometry, getLongitudinalVelocity) { - using tier4_autoware_utils::getLongitudinalVelocity; + using autoware::universe_utils::getLongitudinalVelocity; const double velocity = 1.0; @@ -222,7 +222,7 @@ TEST(geometry, getLongitudinalVelocity) TEST(geometry, setPose) { - using tier4_autoware_utils::setPose; + using autoware::universe_utils::setPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -292,9 +292,9 @@ TEST(geometry, setPose) TEST(geometry, setOrientation) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::setOrientation; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::setOrientation; geometry_msgs::msg::Pose p; const auto orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); @@ -308,7 +308,7 @@ TEST(geometry, setOrientation) TEST(geometry, setLongitudinalVelocity) { - using tier4_autoware_utils::setLongitudinalVelocity; + using autoware::universe_utils::setLongitudinalVelocity; const double velocity = 1.0; @@ -327,7 +327,7 @@ TEST(geometry, setLongitudinalVelocity) TEST(geometry, createPoint) { - using tier4_autoware_utils::createPoint; + using autoware::universe_utils::createPoint; const geometry_msgs::msg::Point p_out = createPoint(1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(p_out.x, 1.0); @@ -337,7 +337,7 @@ TEST(geometry, createPoint) TEST(geometry, createQuaternion) { - using tier4_autoware_utils::createQuaternion; + using autoware::universe_utils::createQuaternion; // (0.18257419, 0.36514837, 0.54772256, 0.73029674) is normalized quaternion of (1, 2, 3, 4) const geometry_msgs::msg::Quaternion q_out = @@ -350,7 +350,7 @@ TEST(geometry, createQuaternion) TEST(geometry, createTranslation) { - using tier4_autoware_utils::createTranslation; + using autoware::universe_utils::createTranslation; const geometry_msgs::msg::Vector3 v_out = createTranslation(1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(v_out.x, 1.0); @@ -360,8 +360,8 @@ TEST(geometry, createTranslation) TEST(geometry, createQuaternionFromRPY) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; { const auto q_out = createQuaternionFromRPY(0, 0, 0); @@ -390,8 +390,8 @@ TEST(geometry, createQuaternionFromRPY) TEST(geometry, createQuaternionFromYaw) { - using tier4_autoware_utils::createQuaternionFromYaw; - using tier4_autoware_utils::deg2rad; + using autoware::universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::deg2rad; { const auto q_out = createQuaternionFromYaw(0); @@ -420,9 +420,9 @@ TEST(geometry, createQuaternionFromYaw) TEST(geometry, calcElevationAngle) { - using tier4_autoware_utils::calcElevationAngle; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; + using autoware::universe_utils::calcElevationAngle; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; { const auto p1 = createPoint(1.0, 1.0, 1.0); @@ -468,9 +468,9 @@ TEST(geometry, calcElevationAngle) TEST(geometry, calcAzimuthAngle) { - using tier4_autoware_utils::calcAzimuthAngle; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; + using autoware::universe_utils::calcAzimuthAngle; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::deg2rad; { const auto p1 = createPoint(0.0, 0.0, 9.0); @@ -521,7 +521,7 @@ TEST(geometry, calcAzimuthAngle) TEST(geometry, calcDistance2d) { - using tier4_autoware_utils::calcDistance2d; + using autoware::universe_utils::calcDistance2d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -538,7 +538,7 @@ TEST(geometry, calcDistance2d) TEST(geometry, calcSquaredDistance2d) { - using tier4_autoware_utils::calcSquaredDistance2d; + using autoware::universe_utils::calcSquaredDistance2d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -555,7 +555,7 @@ TEST(geometry, calcSquaredDistance2d) TEST(geometry, calcDistance3d) { - using tier4_autoware_utils::calcDistance3d; + using autoware::universe_utils::calcDistance3d; geometry_msgs::msg::Point point; point.x = 1.0; @@ -572,9 +572,9 @@ TEST(geometry, calcDistance3d) TEST(geometry, getRPY) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getRPY; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getRPY; { const double ans_roll = deg2rad(5); @@ -610,9 +610,9 @@ TEST(geometry, getRPY) TEST(geometry, getRPY_wrapper) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getRPY; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getRPY; { const double ans_roll = deg2rad(45); @@ -652,9 +652,9 @@ TEST(geometry, getRPY_wrapper) TEST(geometry, transform2pose) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::transform2pose; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::transform2pose; { geometry_msgs::msg::Transform transform; @@ -703,9 +703,9 @@ TEST(geometry, transform2pose) TEST(geometry, pose2transform) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::pose2transform; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::pose2transform; { geometry_msgs::msg::Pose pose; @@ -756,9 +756,9 @@ TEST(geometry, pose2transform) TEST(geometry, point2tfVector) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::point2tfVector; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::point2tfVector; // Point { @@ -823,11 +823,11 @@ TEST(geometry, point2tfVector) TEST(geometry, transformPoint) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::Point2d; - using tier4_autoware_utils::Point3d; - using tier4_autoware_utils::transformPoint; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::Point2d; + using autoware::universe_utils::Point3d; + using autoware::universe_utils::transformPoint; { const Point2d p(1.0, 2.0); @@ -916,9 +916,9 @@ TEST(geometry, transformPoint) TEST(geometry, transformPose) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::transformPose; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::transformPose; geometry_msgs::msg::Pose pose; pose.position.x = 2.0; @@ -967,9 +967,9 @@ TEST(geometry, transformPose) TEST(geometry, inverseTransformPose) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::inverseTransformPose; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::inverseTransformPose; geometry_msgs::msg::Pose pose; pose.position.x = 2.0; @@ -1018,10 +1018,10 @@ TEST(geometry, inverseTransformPose) TEST(geometry, inverseTransformPoint) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::inverseTransformPoint; - using tier4_autoware_utils::inverseTransformPose; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::inverseTransformPoint; + using autoware::universe_utils::inverseTransformPose; geometry_msgs::msg::Pose pose_transform; pose_transform.position.x = 1.0; @@ -1050,10 +1050,10 @@ TEST(geometry, inverseTransformPoint) TEST(geometry, transformVector) { - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::MultiPoint3d; - using tier4_autoware_utils::transformVector; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::MultiPoint3d; + using autoware::universe_utils::transformVector; { const MultiPoint3d ps{{1.0, 2.0, 3.0}, {2.0, 3.0, 4.0}}; @@ -1078,51 +1078,51 @@ TEST(geometry, transformVector) TEST(geometry, calcCurvature) { - using tier4_autoware_utils::calcCurvature; + using autoware::universe_utils::calcCurvature; // Straight Line { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); - geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(1.0, 0.0, 0.0); + geometry_msgs::msg::Point p3 = autoware::universe_utils::createPoint(2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 0.0); } // Clockwise Curved Road with a 1[m] radius { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(1.0, 1.0, 0.0); - geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(1.0, 1.0, 0.0); + geometry_msgs::msg::Point p3 = autoware::universe_utils::createPoint(2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), -1.0); } // Clockwise Curved Road with a 5[m] radius { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(5.0, 5.0, 0.0); - geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(10.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(5.0, 5.0, 0.0); + geometry_msgs::msg::Point p3 = autoware::universe_utils::createPoint(10.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), -0.2); } // Counter-Clockwise Curved Road with a 1[m] radius { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(-1.0, 1.0, 0.0); - geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(-2.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(-1.0, 1.0, 0.0); + geometry_msgs::msg::Point p3 = autoware::universe_utils::createPoint(-2.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 1.0); } // Counter-Clockwise Curved Road with a 5[m] radius { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(-5.0, 5.0, 0.0); - geometry_msgs::msg::Point p3 = tier4_autoware_utils::createPoint(-10.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(-5.0, 5.0, 0.0); + geometry_msgs::msg::Point p3 = autoware::universe_utils::createPoint(-10.0, 0.0, 0.0); EXPECT_DOUBLE_EQ(calcCurvature(p1, p2, p3), 0.2); } // Give same points { - geometry_msgs::msg::Point p1 = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - geometry_msgs::msg::Point p2 = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); + geometry_msgs::msg::Point p1 = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + geometry_msgs::msg::Point p2 = autoware::universe_utils::createPoint(1.0, 0.0, 0.0); ASSERT_ANY_THROW(calcCurvature(p1, p1, p1)); ASSERT_ANY_THROW(calcCurvature(p1, p1, p2)); ASSERT_ANY_THROW(calcCurvature(p1, p2, p1)); @@ -1132,11 +1132,11 @@ TEST(geometry, calcCurvature) TEST(geometry, calcOffsetPose) { - using tier4_autoware_utils::calcOffsetPose; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternion; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; + using autoware::universe_utils::calcOffsetPose; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::createQuaternion; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; // Only translation { @@ -1224,12 +1224,12 @@ TEST(geometry, calcOffsetPose) TEST(geometry, isDrivingForward) { - using tier4_autoware_utils::calcInterpolatedPoint; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternion; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::isDrivingForward; + using autoware::universe_utils::calcInterpolatedPoint; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::createQuaternion; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::isDrivingForward; const double epsilon = 1e-3; @@ -1286,8 +1286,8 @@ TEST(geometry, isDrivingForward) TEST(geometry, calcInterpolatedPoint) { - using tier4_autoware_utils::calcInterpolatedPoint; - using tier4_autoware_utils::createPoint; + using autoware::universe_utils::calcInterpolatedPoint; + using autoware::universe_utils::createPoint; { const auto src_point = createPoint(0.0, 0.0, 0.0); @@ -1343,11 +1343,11 @@ TEST(geometry, calcInterpolatedPoint) TEST(geometry, calcInterpolatedPose) { - using tier4_autoware_utils::calcInterpolatedPose; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternion; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; + using autoware::universe_utils::calcInterpolatedPose; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::createQuaternion; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; const double epsilon = 1e-3; // Position Interpolation @@ -1527,11 +1527,11 @@ TEST(geometry, calcInterpolatedPose) TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation) { - using tier4_autoware_utils::calcInterpolatedPose; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternion; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; + using autoware::universe_utils::calcInterpolatedPose; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::createQuaternion; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; const double epsilon = 1e-3; // Position Interpolation @@ -1675,7 +1675,7 @@ TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation) TEST(geometry, getTwist) { - using tier4_autoware_utils::createVector3; + using autoware::universe_utils::createVector3; geometry_msgs::msg::Vector3 velocity = createVector3(1.0, 2.0, 3.0); geometry_msgs::msg::Vector3 angular = createVector3(0.1, 0.2, 0.3); @@ -1691,7 +1691,7 @@ TEST(geometry, getTwist) // getTwist { - auto t_out = tier4_autoware_utils::createTwist(velocity, angular); + auto t_out = autoware::universe_utils::createTwist(velocity, angular); EXPECT_DOUBLE_EQ(t_out.linear.x, twist.linear.x); EXPECT_DOUBLE_EQ(t_out.linear.y, twist.linear.y); EXPECT_DOUBLE_EQ(t_out.linear.z, twist.linear.z); @@ -1703,32 +1703,32 @@ TEST(geometry, getTwist) TEST(geometry, getTwistNorm) { - using tier4_autoware_utils::createVector3; + using autoware::universe_utils::createVector3; geometry_msgs::msg::TwistWithCovariance twist_with_covariance; twist_with_covariance.twist = geometry_msgs::build() .linear(createVector3(3.0, 4.0, 0.0)) .angular(createVector3(0.1, 0.2, 0.3)); - EXPECT_NEAR(tier4_autoware_utils::calcNorm(twist_with_covariance.twist.linear), 5.0, epsilon); + EXPECT_NEAR(autoware::universe_utils::calcNorm(twist_with_covariance.twist.linear), 5.0, epsilon); } TEST(geometry, isTwistCovarianceValid) { - using tier4_autoware_utils::createVector3; + using autoware::universe_utils::createVector3; geometry_msgs::msg::TwistWithCovariance twist_with_covariance; twist_with_covariance.twist = geometry_msgs::build() .linear(createVector3(1.0, 2.0, 3.0)) .angular(createVector3(0.1, 0.2, 0.3)); - EXPECT_EQ(tier4_autoware_utils::isTwistCovarianceValid(twist_with_covariance), false); + EXPECT_EQ(autoware::universe_utils::isTwistCovarianceValid(twist_with_covariance), false); twist_with_covariance.covariance.at(0) = 1.0; - EXPECT_EQ(tier4_autoware_utils::isTwistCovarianceValid(twist_with_covariance), true); + EXPECT_EQ(autoware::universe_utils::isTwistCovarianceValid(twist_with_covariance), true); } TEST(geometry, intersect) { - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::intersect; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::intersect; { // Normally crossing const auto p1 = createPoint(0.0, -1.0, 0.0); diff --git a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp similarity index 91% rename from common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp rename to common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp index 5c55a09af95b1..4ed1c5497c6ae 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include TEST(geometry, getPoint_PathWithLaneId) { - using tier4_autoware_utils::getPoint; + using autoware::universe_utils::getPoint; const double x_ans = 1.0; const double y_ans = 2.0; @@ -36,7 +36,7 @@ TEST(geometry, getPoint_PathWithLaneId) TEST(geometry, getPose_PathWithLaneId) { - using tier4_autoware_utils::getPose; + using autoware::universe_utils::getPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -66,7 +66,7 @@ TEST(geometry, getPose_PathWithLaneId) TEST(geometry, getLongitudinalVelocity_PathWithLaneId) { - using tier4_autoware_utils::getLongitudinalVelocity; + using autoware::universe_utils::getLongitudinalVelocity; const double velocity = 1.0; @@ -77,7 +77,7 @@ TEST(geometry, getLongitudinalVelocity_PathWithLaneId) TEST(geometry, setPose_PathWithLaneId) { - using tier4_autoware_utils::setPose; + using autoware::universe_utils::setPose; const double x_ans = 1.0; const double y_ans = 2.0; @@ -109,7 +109,7 @@ TEST(geometry, setPose_PathWithLaneId) TEST(geometry, setLongitudinalVelocity_PathWithLaneId) { - using tier4_autoware_utils::setLongitudinalVelocity; + using autoware::universe_utils::setLongitudinalVelocity; const double velocity = 1.0; diff --git a/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp b/common/autoware_universe_utils/test/src/geometry/test_pose_deviation.cpp similarity index 78% rename from common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp rename to common/autoware_universe_utils/test/src/geometry/test_pose_deviation.cpp index 76e742edde59c..c9a5c2cec4fac 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_pose_deviation.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include TEST(geometry, pose_deviation) { - using tier4_autoware_utils::calcPoseDeviation; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; + using autoware::universe_utils::calcPoseDeviation; + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; geometry_msgs::msg::Pose pose1; pose1.position.x = 1.0; diff --git a/common/tier4_autoware_utils/test/src/math/test_constants.cpp b/common/autoware_universe_utils/test/src/math/test_constants.cpp similarity index 84% rename from common/tier4_autoware_utils/test/src/math/test_constants.cpp rename to common/autoware_universe_utils/test/src/math/test_constants.cpp index 80db5414a30f4..6c449834157b0 100644 --- a/common/tier4_autoware_utils/test/src/math/test_constants.cpp +++ b/common/autoware_universe_utils/test/src/math/test_constants.cpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/constants.hpp" +#include "autoware/universe_utils/math/constants.hpp" #include TEST(constants, pi) { - using tier4_autoware_utils::pi; + using autoware::universe_utils::pi; EXPECT_DOUBLE_EQ(pi, 3.14159265358979323846); } TEST(constants, gravity) { - using tier4_autoware_utils::gravity; + using autoware::universe_utils::gravity; EXPECT_DOUBLE_EQ(gravity, 9.80665); } diff --git a/common/tier4_autoware_utils/test/src/math/test_normalization.cpp b/common/autoware_universe_utils/test/src/math/test_normalization.cpp similarity index 94% rename from common/tier4_autoware_utils/test/src/math/test_normalization.cpp rename to common/autoware_universe_utils/test/src/math/test_normalization.cpp index 0ae15ea320850..ae7f3cd7a7a4c 100644 --- a/common/tier4_autoware_utils/test/src/math/test_normalization.cpp +++ b/common/autoware_universe_utils/test/src/math/test_normalization.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include TEST(normalization, normalizeDegree) { - using tier4_autoware_utils::normalizeDegree; + using autoware::universe_utils::normalizeDegree; // -180 <= deg < 180 { @@ -51,7 +51,7 @@ TEST(normalization, normalizeDegree) TEST(normalization, normalizeRadian) { - using tier4_autoware_utils::normalizeRadian; + using autoware::universe_utils::normalizeRadian; // -M_PI <= deg < M_PI { diff --git a/common/tier4_autoware_utils/test/src/math/test_range.cpp b/common/autoware_universe_utils/test/src/math/test_range.cpp similarity index 96% rename from common/tier4_autoware_utils/test/src/math/test_range.cpp rename to common/autoware_universe_utils/test/src/math/test_range.cpp index 429c08938949f..12be5b646957b 100644 --- a/common/tier4_autoware_utils/test/src/math/test_range.cpp +++ b/common/autoware_universe_utils/test/src/math/test_range.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/range.hpp" +#include "autoware/universe_utils/math/range.hpp" #include @@ -44,7 +44,7 @@ void expect_eq_vector(const std::vector & input, const std::vector & e TEST(arange_Test, arange_double) { - using tier4_autoware_utils::arange; + using autoware::universe_utils::arange; // general cases { @@ -82,7 +82,7 @@ TEST(arange_Test, arange_double) TEST(arange_Test, arange_float) { - using tier4_autoware_utils::arange; + using autoware::universe_utils::arange; // general cases { @@ -121,7 +121,7 @@ TEST(arange_Test, arange_float) TEST(arange_Test, arange_int) { - using tier4_autoware_utils::arange; + using autoware::universe_utils::arange; // general cases { @@ -154,7 +154,7 @@ TEST(arange_Test, arange_int) TEST(test_linspace, linspace_double) { - using tier4_autoware_utils::linspace; + using autoware::universe_utils::linspace; // general cases { @@ -182,7 +182,7 @@ TEST(test_linspace, linspace_double) TEST(test_linspace, linspace_float) { - using tier4_autoware_utils::linspace; + using autoware::universe_utils::linspace; // general cases { @@ -211,7 +211,7 @@ TEST(test_linspace, linspace_float) TEST(test_linspace, linspace_int) { - using tier4_autoware_utils::linspace; + using autoware::universe_utils::linspace; // general cases { diff --git a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp similarity index 69% rename from common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp rename to common/autoware_universe_utils/test/src/math/test_trigonometry.cpp index d7106fd823682..b55b27a34a6ac 100644 --- a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp +++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/trigonometry.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/trigonometry.hpp" #include @@ -21,31 +21,31 @@ TEST(trigonometry, sin) { - float x = 4.f * tier4_autoware_utils::pi / 128.f; + float x = 4.f * autoware::universe_utils::pi / 128.f; for (int i = 0; i < 128; i++) { EXPECT_TRUE( std::abs( std::sin(x * static_cast(i)) - - tier4_autoware_utils::sin(x * static_cast(i))) < 10e-5); + autoware::universe_utils::sin(x * static_cast(i))) < 10e-5); } } TEST(trigonometry, cos) { - float x = 4.f * tier4_autoware_utils::pi / 128.f; + float x = 4.f * autoware::universe_utils::pi / 128.f; for (int i = 0; i < 128; i++) { EXPECT_TRUE( std::abs( std::cos(x * static_cast(i)) - - tier4_autoware_utils::cos(x * static_cast(i))) < 10e-5); + autoware::universe_utils::cos(x * static_cast(i))) < 10e-5); } } TEST(trigonometry, sin_and_cos) { - float x = 4.f * tier4_autoware_utils::pi / 128.f; + float x = 4.f * autoware::universe_utils::pi / 128.f; for (int i = 0; i < 128; i++) { - const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast(i)); + const auto sin_and_cos = autoware::universe_utils::sin_and_cos(x * static_cast(i)); EXPECT_TRUE(std::abs(std::sin(x * static_cast(i)) - sin_and_cos.first) < 10e-7); EXPECT_TRUE(std::abs(std::cos(x * static_cast(i)) - sin_and_cos.second) < 10e-7); } diff --git a/common/tier4_autoware_utils/test/src/math/test_unit_conversion.cpp b/common/autoware_universe_utils/test/src/math/test_unit_conversion.cpp similarity index 85% rename from common/tier4_autoware_utils/test/src/math/test_unit_conversion.cpp rename to common/autoware_universe_utils/test/src/math/test_unit_conversion.cpp index 9b689d1257932..605b0e7d3c5a6 100644 --- a/common/tier4_autoware_utils/test/src/math/test_unit_conversion.cpp +++ b/common/autoware_universe_utils/test/src/math/test_unit_conversion.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include -using tier4_autoware_utils::pi; +using autoware::universe_utils::pi; TEST(unit_conversion, deg2rad) { - using tier4_autoware_utils::deg2rad; + using autoware::universe_utils::deg2rad; EXPECT_DOUBLE_EQ(deg2rad(-720), -4 * pi); EXPECT_DOUBLE_EQ(deg2rad(0), 0); @@ -33,7 +33,7 @@ TEST(unit_conversion, deg2rad) TEST(unit_conversion, rad2deg) { - using tier4_autoware_utils::rad2deg; + using autoware::universe_utils::rad2deg; EXPECT_DOUBLE_EQ(rad2deg(-4 * pi), -720); EXPECT_DOUBLE_EQ(rad2deg(0), 0); @@ -46,7 +46,7 @@ TEST(unit_conversion, rad2deg) TEST(unit_conversion, kmph2mps) { - using tier4_autoware_utils::kmph2mps; + using autoware::universe_utils::kmph2mps; EXPECT_DOUBLE_EQ(kmph2mps(0), 0); EXPECT_DOUBLE_EQ(kmph2mps(36), 10); @@ -56,7 +56,7 @@ TEST(unit_conversion, kmph2mps) TEST(unit_conversion, mps2kmph) { - using tier4_autoware_utils::mps2kmph; + using autoware::universe_utils::mps2kmph; EXPECT_DOUBLE_EQ(mps2kmph(0), 0); EXPECT_DOUBLE_EQ(mps2kmph(10), 36); diff --git a/common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp b/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp similarity index 95% rename from common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp rename to common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp index 133cb01f9b348..f9c0bd45c4fb4 100644 --- a/common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp +++ b/common/autoware_universe_utils/test/src/ros/test_published_time_publisher.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include #include @@ -24,7 +24,7 @@ class PublishedTimePublisherWithSubscriptionTest : public ::testing::Test { protected: std::shared_ptr node_{nullptr}; - std::shared_ptr published_time_publisher_ptr_{ + std::shared_ptr published_time_publisher_ptr_{ nullptr}; std::shared_ptr> first_test_publisher_ptr_{nullptr}; @@ -70,7 +70,7 @@ class PublishedTimePublisherWithSubscriptionTest : public ::testing::Test // Create a PublishedTimePublisher published_time_publisher_ptr_ = - std::make_shared(node_.get()); + std::make_shared(node_.get()); // Create the first subscriber first_test_subscriber_ptr_ = @@ -98,7 +98,7 @@ class PublishedTimePublisherWithoutSubscriptionTest : public ::testing::Test { protected: std::shared_ptr node_{nullptr}; - std::shared_ptr published_time_publisher_ptr_{ + std::shared_ptr published_time_publisher_ptr_{ nullptr}; std::shared_ptr> first_test_publisher_ptr_{nullptr}; @@ -135,7 +135,7 @@ class PublishedTimePublisherWithoutSubscriptionTest : public ::testing::Test // Create a PublishedTimePublisher published_time_publisher_ptr_ = - std::make_shared(node_.get()); + std::make_shared(node_.get()); rclcpp::spin_some(node_); } diff --git a/common/tier4_autoware_utils/test/src/system/test_stop_watch.cpp b/common/autoware_universe_utils/test/src/system/test_stop_watch.cpp similarity index 91% rename from common/tier4_autoware_utils/test/src/system/test_stop_watch.cpp rename to common/autoware_universe_utils/test/src/system/test_stop_watch.cpp index cb201ab0dd5ec..55e10e9bd2ffd 100644 --- a/common/tier4_autoware_utils/test/src/system/test_stop_watch.cpp +++ b/common/autoware_universe_utils/test/src/system/test_stop_watch.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include @@ -21,7 +21,7 @@ TEST(system, StopWatch_sec) { - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::StopWatch; StopWatch stop_watch; @@ -50,7 +50,7 @@ TEST(system, StopWatch_sec) TEST(system, StopWatch_msec) { - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::StopWatch; StopWatch stop_watch; diff --git a/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp b/common/autoware_universe_utils/test/src/test_autoware_utils.cpp similarity index 100% rename from common/tier4_autoware_utils/test/src/test_autoware_utils.cpp rename to common/autoware_universe_utils/test/src/test_autoware_utils.cpp diff --git a/common/tier4_autoware_utils/test/src/transform/test_transform.cpp b/common/autoware_universe_utils/test/src/transform/test_transform.cpp similarity index 85% rename from common/tier4_autoware_utils/test/src/transform/test_transform.cpp rename to common/autoware_universe_utils/test/src/transform/test_transform.cpp index aae8cc22ea9fb..2935600c9f446 100644 --- a/common/tier4_autoware_utils/test/src/transform/test_transform.cpp +++ b/common/autoware_universe_utils/test/src/transform/test_transform.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/transform/transforms.hpp" +#include "autoware/universe_utils/transform/transforms.hpp" #include #include @@ -31,7 +31,7 @@ TEST(system, transform_point_cloud) 42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000; pcl::PointCloud cloud_transformed; - tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform); + autoware::universe_utils::transformPointCloud(cloud, cloud_transformed, transform); pcl::PointXYZI pt1_gt(89603.187500, 42270.878906, -13.056946, 4); @@ -52,8 +52,9 @@ TEST(system, empty_point_cloud) pcl::PointCloud cloud_transformed; - EXPECT_NO_THROW(tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform)); + EXPECT_NO_THROW( + autoware::universe_utils::transformPointCloud(cloud, cloud_transformed, transform)); EXPECT_NO_FATAL_FAILURE( - tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform)); + autoware::universe_utils::transformPointCloud(cloud, cloud_transformed, transform)); EXPECT_EQ(cloud_transformed.size(), 0ul); } diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp index 32b6b6ca2565c..8099bea36784e 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp @@ -94,8 +94,7 @@ class NodeAdaptor C & cli, S & srv, CallbackGroup group, std::optional timeout = std::nullopt) const { init_cli(cli); - init_srv( - srv, [cli, timeout](auto req, auto res) { *res = *cli->call(req, timeout); }, group); + init_srv(srv, [cli, timeout](auto req, auto res) { *res = *cli->call(req, timeout); }, group); } /// Create a subscription wrapper. diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp index 3329c1349b707..69c8840b39421 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp @@ -15,8 +15,8 @@ #ifndef GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_ #define GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_ +#include #include -#include #include #include @@ -25,7 +25,7 @@ namespace goal_distance_calculator { -using tier4_autoware_utils::PoseDeviation; +using autoware::universe_utils::PoseDeviation; struct Param { diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp index 680c4a3cdfff1..602688ffe51d7 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp @@ -17,9 +17,10 @@ #include "goal_distance_calculator/goal_distance_calculator.hpp" +#include +#include +#include #include -#include -#include #include #include @@ -44,25 +45,18 @@ class GoalDistanceCalculatorNode : public rclcpp::Node private: // Subscriber - rclcpp::Subscription::SharedPtr sub_initial_pose_; - tier4_autoware_utils::SelfPoseListener self_pose_listener_; - rclcpp::Subscription::SharedPtr sub_route_; - - // Data Buffer - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - autoware_planning_msgs::msg::LaneletRoute::SharedPtr route_; - - // Callback - void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & msg); + autoware::universe_utils::SelfPoseListener self_pose_listener_; + autoware::universe_utils::InterProcessPollingSubscriber + sub_route_{this, "/planning/mission_planning/route"}; // Publisher - tier4_autoware_utils::DebugPublisher debug_publisher_; + autoware::universe_utils::DebugPublisher debug_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; - bool isDataReady(); - bool isDataTimeout(); + bool tryGetCurrentPose(geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose); + bool tryGetRoute(autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route); void onTimer(); // Parameter @@ -70,8 +64,6 @@ class GoalDistanceCalculatorNode : public rclcpp::Node Param param_; // Core - Input input_; - Output output_; std::unique_ptr goal_distance_calculator_; }; } // namespace goal_distance_calculator diff --git a/common/goal_distance_calculator/package.xml b/common/goal_distance_calculator/package.xml index eb71dc45f31a3..04ea8a37a5053 100644 --- a/common/goal_distance_calculator/package.xml +++ b/common/goal_distance_calculator/package.xml @@ -11,10 +11,10 @@ autoware_cmake autoware_planning_msgs + autoware_universe_utils geometry_msgs rclcpp rclcpp_components - tier4_autoware_utils tier4_debug_msgs ament_lint_auto diff --git a/common/goal_distance_calculator/src/goal_distance_calculator.cpp b/common/goal_distance_calculator/src/goal_distance_calculator.cpp index 719baef283791..a577d43675224 100755 --- a/common/goal_distance_calculator/src/goal_distance_calculator.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator.cpp @@ -21,7 +21,7 @@ Output GoalDistanceCalculator::update(const Input & input) Output output{}; output.goal_deviation = - tier4_autoware_utils::calcPoseDeviation(input.route->goal_pose, input.current_pose->pose); + autoware::universe_utils::calcPoseDeviation(input.route->goal_pose, input.current_pose->pose); return output; } diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp index ab1c35e246719..24d54ee2fcf87 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -14,9 +14,9 @@ #include "goal_distance_calculator/goal_distance_calculator_node.hpp" +#include #include #include -#include #include @@ -33,12 +33,6 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions self_pose_listener_(this), debug_publisher_(this, "goal_distance_calculator") { - using std::placeholders::_1; - - static constexpr std::size_t queue_size = 1; - rclcpp::QoS durable_qos(queue_size); - durable_qos.transient_local(); - // Node Parameter node_param_.update_rate = declare_parameter("update_rate"); node_param_.oneshot = declare_parameter("oneshot"); @@ -47,11 +41,6 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions goal_distance_calculator_ = std::make_unique(); goal_distance_calculator_->setParam(param_); - // Subscriber - sub_route_ = create_subscription( - "/planning/mission_planning/route", queue_size, - [&](const autoware_planning_msgs::msg::LaneletRoute::SharedPtr msg_ptr) { route_ = msg_ptr; }); - // Wait for first self pose self_pose_listener_.waitForFirstPose(); @@ -62,53 +51,54 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions goal_distance_calculator_ = std::make_unique(); } -bool GoalDistanceCalculatorNode::isDataReady() +bool GoalDistanceCalculatorNode::tryGetCurrentPose( + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose) { - if (!current_pose_) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "waiting for current_pose..."); - return false; - } - - if (!route_) { - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "waiting for route msg..."); - return false; - } - + auto current_pose_tmp = self_pose_listener_.getCurrentPose(); + if (!current_pose_tmp) return false; + current_pose = current_pose_tmp; return true; } -bool GoalDistanceCalculatorNode::isDataTimeout() +bool GoalDistanceCalculatorNode::tryGetRoute( + autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route) { - constexpr double th_pose_timeout = 1.0; - const auto pose_time_diff = rclcpp::Time(current_pose_->header.stamp) - now(); - if (pose_time_diff.seconds() > th_pose_timeout) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "pose is timeout..."); - return true; - } - return false; + auto route_tmp = sub_route_.takeData(); + if (!route_tmp) return false; + route = route_tmp; + return true; } void GoalDistanceCalculatorNode::onTimer() { - current_pose_ = self_pose_listener_.getCurrentPose(); + Input input = Input(); - if (!isDataReady()) { + if (!tryGetCurrentPose(input.current_pose)) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "waiting for current_pose..."); return; } - if (isDataTimeout()) { + if (!tryGetRoute(input.route)) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "waiting for route msg..."); return; } - input_.current_pose = current_pose_; - input_.route = route_; + // Check pose timeout + { + constexpr double th_pose_timeout = 1.0; + const auto pose_time_diff = rclcpp::Time(input.current_pose->header.stamp) - now(); + if (pose_time_diff.seconds() > th_pose_timeout) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "pose is timeout..."); + return; + } + } - output_ = goal_distance_calculator_->update(input_); + Output output = goal_distance_calculator_->update(input); { - using tier4_autoware_utils::rad2deg; - const auto & deviation = output_.goal_deviation; + using autoware::universe_utils::rad2deg; + const auto & deviation = output.goal_deviation; debug_publisher_.publish( "deviation/lateral", deviation.lateral); diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp index f5befefeae9da..38e7b3fed5b8b 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation.hpp @@ -15,8 +15,8 @@ #ifndef INTERPOLATION__SPLINE_INTERPOLATION_HPP_ #define INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/interpolation_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index 653d6c61d05b6..398d841c54710 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -53,7 +53,7 @@ class SplineInterpolationPoints2d { std::vector points_inner; for (const auto & p : points) { - points_inner.push_back(tier4_autoware_utils::getPoint(p)); + points_inner.push_back(autoware::universe_utils::getPoint(p)); } calcSplineCoefficientsInner(points_inner); } diff --git a/common/interpolation/package.xml b/common/interpolation/package.xml index 6be04482da3ee..bb4af924dd252 100644 --- a/common/interpolation/package.xml +++ b/common/interpolation/package.xml @@ -11,7 +11,7 @@ ament_cmake_auto autoware_cmake - tier4_autoware_utils + autoware_universe_utils ament_cmake_ros ament_lint_auto diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 61c60df7a8984..4d6d1639f2ac7 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -118,7 +118,7 @@ geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( geometry_msgs::msg::Pose pose; pose.position = getSplineInterpolatedPoint(idx, s); pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); + autoware::universe_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); return pose; } diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index 94baf50992cf5..d3cb2d6d3060b 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/spline_interpolation.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp index 09973826387a2..4013832220cd8 100644 --- a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -25,7 +25,7 @@ constexpr double epsilon = 1e-6; TEST(spline_interpolation, splineYawFromPoints) { - using tier4_autoware_utils::createPoint; + using autoware::universe_utils::createPoint; { // straight std::vector points; @@ -96,7 +96,7 @@ TEST(spline_interpolation, splineYawFromPoints) TEST(spline_interpolation, SplineInterpolationPoints2d) { - using tier4_autoware_utils::createPoint; + using autoware::universe_utils::createPoint; // curve std::vector points; @@ -199,8 +199,8 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) { + using autoware::universe_utils::createPoint; using autoware_planning_msgs::msg::TrajectoryPoint; - using tier4_autoware_utils::createPoint; std::vector points; points.push_back(createPoint(-2.0, -10.0, 0.0)); diff --git a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp b/common/object_recognition_utils/include/object_recognition_utils/matching.hpp index e9b924023f62e..ae50227df1166 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/matching.hpp @@ -15,9 +15,9 @@ #ifndef OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ #define OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "object_recognition_utils/geometry.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -28,7 +28,7 @@ namespace object_recognition_utils { -using tier4_autoware_utils::Polygon2d; +using autoware::universe_utils::Polygon2d; // minimum area to avoid division by zero static const double MIN_AREA = 1e-6; @@ -37,7 +37,7 @@ inline double getConvexShapeArea(const Polygon2d & source_polygon, const Polygon boost::geometry::model::multi_polygon union_polygons; boost::geometry::union_(source_polygon, target_polygon, union_polygons); - tier4_autoware_utils::Polygon2d hull; + autoware::universe_utils::Polygon2d hull; boost::geometry::convex_hull(union_polygons, hull); return boost::geometry::area(hull); } @@ -67,9 +67,9 @@ inline double getUnionArea(const Polygon2d & source_polygon, const Polygon2d & t template double get2dIoU(const T1 source_object, const T2 target_object, const double min_union_area = 0.01) { - const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -84,9 +84,9 @@ double get2dIoU(const T1 source_object, const T2 target_object, const double min template double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object) { - const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -100,10 +100,10 @@ double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object) template double get2dPrecision(const T1 source_object, const T2 target_object) { - const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); const double source_area = boost::geometry::area(source_polygon); if (source_area < MIN_AREA) return 0.0; - const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -115,9 +115,9 @@ double get2dPrecision(const T1 source_object, const T2 target_object) template double get2dRecall(const T1 source_object, const T2 target_object) { - const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + const auto source_polygon = autoware::universe_utils::toPolygon2d(source_object); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + const auto target_polygon = autoware::universe_utils::toPolygon2d(target_object); const double target_area = boost::geometry::area(target_polygon); if (target_area < MIN_AREA) return 0.0; diff --git a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp index 1cd838e090560..72e70185d553b 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp @@ -15,7 +15,7 @@ #ifndef OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ #define OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index fdf73e6b26057..e7eaea96907f2 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -14,6 +14,7 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils geometry_msgs interpolation libboost-dev @@ -24,7 +25,6 @@ std_msgs tf2 tf2_eigen - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index fe9499b4eec33..e26c9e6a7e1ea 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -38,7 +38,7 @@ boost::optional calcInterpolatedPose( if (relative_time - epsilon < time_step * path_idx) { const double offset = relative_time - time_step * (path_idx - 1); const double ratio = std::clamp(offset / time_step, 0.0, 1.0); - return tier4_autoware_utils::calcInterpolatedPose(prev_pt, pt, ratio, false); + return autoware::universe_utils::calcInterpolatedPose(prev_pt, pt, ratio, false); } } @@ -90,7 +90,7 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( // Set Position for (size_t i = 0; i < resampled_size; ++i) { - const auto p = tier4_autoware_utils::createPoint( + const auto p = autoware::universe_utils::createPoint( interpolated_x.at(i), interpolated_y.at(i), interpolated_z.at(i)); resampled_path.path.at(i).position = p; resampled_path.path.at(i).orientation = interpolated_quat.at(i); diff --git a/common/object_recognition_utils/test/src/test_conversion.cpp b/common/object_recognition_utils/test/src/test_conversion.cpp index 378f9a1245746..a9b462f9ec4c4 100644 --- a/common/object_recognition_utils/test/src/test_conversion.cpp +++ b/common/object_recognition_utils/test/src/test_conversion.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_recognition_utils/conversion.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/common/object_recognition_utils/test/src/test_matching.cpp b/common/object_recognition_utils/test/src/test_matching.cpp index 2ad34c3ae6bbe..6005ac8d1efbc 100644 --- a/common/object_recognition_utils/test/src/test_matching.cpp +++ b/common/object_recognition_utils/test/src/test_matching.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_recognition_utils/matching.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Point3d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Point3d; constexpr double epsilon = 1e-06; @@ -30,7 +30,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double { geometry_msgs::msg::Pose p; p.position = geometry_msgs::build().x(x).y(y).z(0.0); - p.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + p.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); return p; } } // namespace diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp index 3dc3a8bcba3f7..305a1173acf12 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -12,23 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Point3d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Point3d; constexpr double epsilon = 1e-06; namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::transformPoint; using autoware_perception_msgs::msg::PredictedPath; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::transformPoint; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) @@ -62,10 +62,10 @@ PredictedPath createTestPredictedPath( TEST(predicted_path_utils, testCalcInterpolatedPose) { + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::deg2rad; using object_recognition_utils::calcInterpolatedPose; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::createQuaternionFromYaw; - using tier4_autoware_utils::deg2rad; const auto path = createTestPredictedPath(100, 0.1, 1.0); @@ -128,10 +128,10 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) TEST(predicted_path_utils, resamplePredictedPath_by_vector) { + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::deg2rad; using object_recognition_utils::resamplePredictedPath; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::createQuaternionFromYaw; - using tier4_autoware_utils::deg2rad; const auto path = createTestPredictedPath(10, 1.0, 1.0); @@ -205,10 +205,10 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) TEST(predicted_path_utils, resamplePredictedPath_by_sampling_time) { + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::deg2rad; using object_recognition_utils::resamplePredictedPath; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::createQuaternionFromYaw; - using tier4_autoware_utils::deg2rad; const auto path = createTestPredictedPath(10, 1.0, 1.0); diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index 0ccc6419b13de..1921cbb1c4a20 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -10,11 +10,11 @@ ament_cmake autoware_cmake + autoware_motion_utils autoware_planning_msgs - motion_utils + autoware_universe_utils rclcpp rclcpp_components - tier4_autoware_utils tier4_debug_msgs ament_lint_auto diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index 798f5df70c596..833c806cdd6db 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -14,7 +14,7 @@ #include "path_distance_calculator.hpp" -#include +#include #include #include @@ -25,15 +25,12 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & options) : Node("path_distance_calculator", options), self_pose_listener_(this) { - sub_path_ = create_subscription( - "~/input/path", rclcpp::QoS(1), - [this](const autoware_planning_msgs::msg::Path::SharedPtr msg) { path_ = msg; }); pub_dist_ = create_publisher("~/output/distance", rclcpp::QoS(1)); using std::chrono_literals::operator""s; timer_ = rclcpp::create_timer(this, get_clock(), 1s, [this]() { - const auto path = path_; + const auto path = sub_path_.takeData(); const auto pose = self_pose_listener_.getCurrentPose(); if (!pose) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "no pose"); @@ -48,8 +45,8 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio return; } - const double distance = - motion_utils::calcSignedArcLength(path->points, pose->pose.position, path->points.size() - 1); + const double distance = autoware::motion_utils::calcSignedArcLength( + path->points, pose->pose.position, path->points.size() - 1); tier4_debug_msgs::msg::Float64Stamped msg; msg.stamp = pose->header.stamp; diff --git a/common/path_distance_calculator/src/path_distance_calculator.hpp b/common/path_distance_calculator/src/path_distance_calculator.hpp index 838afb903c55f..6624f316401df 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.hpp +++ b/common/path_distance_calculator/src/path_distance_calculator.hpp @@ -15,8 +15,9 @@ #ifndef PATH_DISTANCE_CALCULATOR_HPP_ #define PATH_DISTANCE_CALCULATOR_HPP_ +#include +#include #include -#include #include #include @@ -27,11 +28,11 @@ class PathDistanceCalculator : public rclcpp::Node explicit PathDistanceCalculator(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_path_; + autoware::universe_utils::InterProcessPollingSubscriber + sub_path_{this, "~/input/path"}; rclcpp::Publisher::SharedPtr pub_dist_; rclcpp::TimerBase::SharedPtr timer_; - tier4_autoware_utils::SelfPoseListener self_pose_listener_; - autoware_planning_msgs::msg::Path::SharedPtr path_; + autoware::universe_utils::SelfPoseListener self_pose_listener_; }; #endif // PATH_DISTANCE_CALCULATOR_HPP_ diff --git a/common/tier4_adapi_rviz_plugin/CMakeLists.txt b/common/tier4_adapi_rviz_plugin/CMakeLists.txt index 1cf1ca95b852e..f06ab33823a3f 100644 --- a/common/tier4_adapi_rviz_plugin/CMakeLists.txt +++ b/common/tier4_adapi_rviz_plugin/CMakeLists.txt @@ -12,6 +12,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) ament_auto_add_library(${PROJECT_NAME} SHARED src/route_tool.cpp src/route_panel.cpp + src/state_panel.cpp src/door_panel.cpp ) diff --git a/common/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png b/common/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png differ diff --git a/common/tier4_adapi_rviz_plugin/package.xml b/common/tier4_adapi_rviz_plugin/package.xml index 19f8857984354..3f8c34be318d1 100644 --- a/common/tier4_adapi_rviz_plugin/package.xml +++ b/common/tier4_adapi_rviz_plugin/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_ad_api_specs + autoware_vehicle_msgs component_interface_utils geometry_msgs libqt5-core @@ -21,6 +22,8 @@ rclcpp rviz_common rviz_default_plugins + tier4_external_api_msgs + tier4_planning_msgs ament_lint_auto autoware_lint_common diff --git a/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml b/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml index 8b095c0e7d037..0b4f00bd56278 100644 --- a/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml @@ -8,6 +8,10 @@ RoutePanel + + StatePanel + + DoorPanel diff --git a/common/tier4_adapi_rviz_plugin/src/state_panel.cpp b/common/tier4_adapi_rviz_plugin/src/state_panel.cpp new file mode 100644 index 0000000000000..982c8fa166b7d --- /dev/null +++ b/common/tier4_adapi_rviz_plugin/src/state_panel.cpp @@ -0,0 +1,637 @@ +// +// Copyright 2020 TIER IV, Inc. All rights reserved. +// +// 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 "state_panel.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +inline std::string Bool2String(const bool var) +{ + return var ? "True" : "False"; +} + +namespace tier4_adapi_rviz_plugins +{ + +StatePanel::StatePanel(QWidget * parent) : rviz_common::Panel(parent) +{ + // Gear + auto * gear_prefix_label_ptr = new QLabel("GEAR: "); + gear_prefix_label_ptr->setAlignment(Qt::AlignRight | Qt::AlignVCenter); + gear_label_ptr_ = new QLabel("INIT"); + gear_label_ptr_->setAlignment(Qt::AlignCenter); + auto * gear_layout = new QHBoxLayout; + gear_layout->addWidget(gear_prefix_label_ptr); + gear_layout->addWidget(gear_label_ptr_); + + // Velocity Limit + velocity_limit_button_ptr_ = new QPushButton("Send Velocity Limit"); + pub_velocity_limit_input_ = new QSpinBox(); + pub_velocity_limit_input_->setRange(-100.0, 100.0); + pub_velocity_limit_input_->setValue(0.0); + pub_velocity_limit_input_->setSingleStep(5.0); + connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit())); + + // Emergency Button + emergency_button_ptr_ = new QPushButton("Set Emergency"); + connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); + + // Layout + auto * v_layout = new QVBoxLayout; + auto * velocity_limit_layout = new QHBoxLayout(); + v_layout->addWidget(makeOperationModeGroup()); + v_layout->addWidget(makeControlModeGroup()); + { + auto * h_layout = new QHBoxLayout(); + h_layout->addWidget(makeRoutingGroup()); + h_layout->addWidget(makeLocalizationGroup()); + h_layout->addWidget(makeMotionGroup()); + h_layout->addWidget(makeFailSafeGroup()); + v_layout->addLayout(h_layout); + } + + v_layout->addLayout(gear_layout); + velocity_limit_layout->addWidget(velocity_limit_button_ptr_); + velocity_limit_layout->addWidget(pub_velocity_limit_input_); + velocity_limit_layout->addWidget(new QLabel(" [km/h]")); + velocity_limit_layout->addWidget(emergency_button_ptr_); + v_layout->addLayout(velocity_limit_layout); + setLayout(v_layout); +} + +QGroupBox * StatePanel::makeOperationModeGroup() +{ + auto * group = new QGroupBox("OperationMode"); + auto * grid = new QGridLayout; + + operation_mode_label_ptr_ = new QLabel("INIT"); + operation_mode_label_ptr_->setAlignment(Qt::AlignCenter); + operation_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(operation_mode_label_ptr_, 0, 0, 0, 1); + + auto_button_ptr_ = new QPushButton("AUTO"); + auto_button_ptr_->setCheckable(true); + connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous())); + grid->addWidget(auto_button_ptr_, 0, 1); + + stop_button_ptr_ = new QPushButton("STOP"); + stop_button_ptr_->setCheckable(true); + connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); + grid->addWidget(stop_button_ptr_, 0, 2); + + local_button_ptr_ = new QPushButton("LOCAL"); + local_button_ptr_->setCheckable(true); + connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal())); + grid->addWidget(local_button_ptr_, 1, 1); + + remote_button_ptr_ = new QPushButton("REMOTE"); + remote_button_ptr_->setCheckable(true); + connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote())); + grid->addWidget(remote_button_ptr_, 1, 2); + + group->setLayout(grid); + return group; +} + +QGroupBox * StatePanel::makeControlModeGroup() +{ + auto * group = new QGroupBox("AutowareControl"); + auto * grid = new QGridLayout; + + control_mode_label_ptr_ = new QLabel("INIT"); + control_mode_label_ptr_->setAlignment(Qt::AlignCenter); + control_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(control_mode_label_ptr_, 0, 0); + + enable_button_ptr_ = new QPushButton("Enable"); + enable_button_ptr_->setCheckable(true); + connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl())); + grid->addWidget(enable_button_ptr_, 0, 1); + + disable_button_ptr_ = new QPushButton("Disable"); + disable_button_ptr_->setCheckable(true); + connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl())); + grid->addWidget(disable_button_ptr_, 0, 2); + + group->setLayout(grid); + return group; +} + +QGroupBox * StatePanel::makeRoutingGroup() +{ + auto * group = new QGroupBox("Routing"); + auto * grid = new QGridLayout; + + routing_label_ptr_ = new QLabel("INIT"); + routing_label_ptr_->setAlignment(Qt::AlignCenter); + routing_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(routing_label_ptr_, 0, 0); + + clear_route_button_ptr_ = new QPushButton("Clear Route"); + clear_route_button_ptr_->setCheckable(true); + connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); + grid->addWidget(clear_route_button_ptr_, 1, 0); + + group->setLayout(grid); + return group; +} + +QGroupBox * StatePanel::makeLocalizationGroup() +{ + auto * group = new QGroupBox("Localization"); + auto * grid = new QGridLayout; + + localization_label_ptr_ = new QLabel("INIT"); + localization_label_ptr_->setAlignment(Qt::AlignCenter); + localization_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(localization_label_ptr_, 0, 0); + + init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS"); + connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); + grid->addWidget(init_by_gnss_button_ptr_, 1, 0); + + group->setLayout(grid); + return group; +} + +QGroupBox * StatePanel::makeMotionGroup() +{ + auto * group = new QGroupBox("Motion"); + auto * grid = new QGridLayout; + + motion_label_ptr_ = new QLabel("INIT"); + motion_label_ptr_->setAlignment(Qt::AlignCenter); + motion_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(motion_label_ptr_, 0, 0); + + accept_start_button_ptr_ = new QPushButton("Accept Start"); + accept_start_button_ptr_->setCheckable(true); + connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); + grid->addWidget(accept_start_button_ptr_, 1, 0); + + group->setLayout(grid); + return group; +} + +QGroupBox * StatePanel::makeFailSafeGroup() +{ + auto * group = new QGroupBox("FailSafe"); + auto * grid = new QGridLayout; + + mrm_state_label_ptr_ = new QLabel("INIT"); + mrm_state_label_ptr_->setAlignment(Qt::AlignCenter); + mrm_state_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(mrm_state_label_ptr_, 0, 0); + + mrm_behavior_label_ptr_ = new QLabel("INIT"); + mrm_behavior_label_ptr_->setAlignment(Qt::AlignCenter); + mrm_behavior_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(mrm_behavior_label_ptr_, 1, 0); + + group->setLayout(grid); + return group; +} + +void StatePanel::onInitialize() +{ + using std::placeholders::_1; + + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + // Operation Mode + sub_operation_mode_ = raw_node_->create_subscription( + "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), + std::bind(&StatePanel::onOperationMode, this, _1)); + + client_change_to_autonomous_ = + raw_node_->create_client("/api/operation_mode/change_to_autonomous"); + + client_change_to_stop_ = + raw_node_->create_client("/api/operation_mode/change_to_stop"); + + client_change_to_local_ = + raw_node_->create_client("/api/operation_mode/change_to_local"); + + client_change_to_remote_ = + raw_node_->create_client("/api/operation_mode/change_to_remote"); + + client_enable_autoware_control_ = + raw_node_->create_client("/api/operation_mode/enable_autoware_control"); + + client_enable_direct_control_ = + raw_node_->create_client("/api/operation_mode/disable_autoware_control"); + + // Routing + sub_route_ = raw_node_->create_subscription( + "/api/routing/state", rclcpp::QoS{1}.transient_local(), + std::bind(&StatePanel::onRoute, this, _1)); + + client_clear_route_ = raw_node_->create_client("/api/routing/clear_route"); + + // Localization + sub_localization_ = raw_node_->create_subscription( + "/api/localization/initialization_state", rclcpp::QoS{1}.transient_local(), + std::bind(&StatePanel::onLocalization, this, _1)); + + client_init_by_gnss_ = + raw_node_->create_client("/api/localization/initialize"); + + // Motion + sub_motion_ = raw_node_->create_subscription( + "/api/motion/state", rclcpp::QoS{1}.transient_local(), + std::bind(&StatePanel::onMotion, this, _1)); + + client_accept_start_ = raw_node_->create_client("/api/motion/accept_start"); + + // FailSafe + sub_mrm_ = raw_node_->create_subscription( + "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), + std::bind(&StatePanel::onMRMState, this, _1)); + + // Others + sub_gear_ = raw_node_->create_subscription( + "/vehicle/status/gear_status", 10, std::bind(&StatePanel::onShift, this, _1)); + + sub_emergency_ = raw_node_->create_subscription( + "/api/autoware/get/emergency", 10, std::bind(&StatePanel::onEmergencyStatus, this, _1)); + + client_emergency_stop_ = raw_node_->create_client( + "/api/autoware/set/emergency"); + + pub_velocity_limit_ = raw_node_->create_publisher( + "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); +} + +void StatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + auto changeButtonState = [this]( + QPushButton * button, const bool is_desired_mode_available, + const uint8_t current_mode = OperationModeState::UNKNOWN, + const uint8_t desired_mode = OperationModeState::STOP) { + if (is_desired_mode_available && current_mode != desired_mode) { + activateButton(button); + } else { + deactivateButton(button); + } + }; + + QString text = ""; + QString style_sheet = ""; + // Operation Mode + switch (msg->mode) { + case OperationModeState::AUTONOMOUS: + text = "AUTONOMOUS"; + style_sheet = "background-color: #00FF00;"; // green + break; + + case OperationModeState::LOCAL: + text = "LOCAL"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case OperationModeState::REMOTE: + text = "REMOTE"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case OperationModeState::STOP: + text = "STOP"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + if (msg->is_in_transition) { + text += "\n(TRANSITION)"; + } + + updateLabel(operation_mode_label_ptr_, text, style_sheet); + + // Control Mode + if (msg->is_autoware_control_enabled) { + updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green + } else { + updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow + } + + // Button + changeButtonState( + auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS); + changeButtonState( + stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP); + changeButtonState( + local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL); + changeButtonState( + remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE); + + changeButtonState(enable_button_ptr_, !msg->is_autoware_control_enabled); + changeButtonState(disable_button_ptr_, msg->is_autoware_control_enabled); +} + +void StatePanel::onRoute(const RouteState::ConstSharedPtr msg) +{ + QString text = ""; + QString style_sheet = ""; + switch (msg->state) { + case RouteState::UNSET: + text = "UNSET"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case RouteState::SET: + text = "SET"; + style_sheet = "background-color: #00FF00;"; // green + break; + + case RouteState::ARRIVED: + text = "ARRIVED"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + case RouteState::CHANGING: + text = "CHANGING"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(routing_label_ptr_, text, style_sheet); + + if (msg->state == RouteState::SET) { + activateButton(clear_route_button_ptr_); + } else { + deactivateButton(clear_route_button_ptr_); + } +} + +void StatePanel::onLocalization(const LocalizationInitializationState::ConstSharedPtr msg) +{ + QString text = ""; + QString style_sheet = ""; + switch (msg->state) { + case LocalizationInitializationState::UNINITIALIZED: + text = "UNINITIALIZED"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case LocalizationInitializationState::INITIALIZING: + text = "INITIALIZING"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + case LocalizationInitializationState::INITIALIZED: + text = "INITIALIZED"; + style_sheet = "background-color: #00FF00;"; // green + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(localization_label_ptr_, text, style_sheet); +} + +void StatePanel::onMotion(const MotionState::ConstSharedPtr msg) +{ + QString text = ""; + QString style_sheet = ""; + switch (msg->state) { + case MotionState::STARTING: + text = "STARTING"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case MotionState::STOPPED: + text = "STOPPED"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + case MotionState::MOVING: + text = "MOVING"; + style_sheet = "background-color: #00FF00;"; // green + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(motion_label_ptr_, text, style_sheet); + + if (msg->state == MotionState::STARTING) { + activateButton(accept_start_button_ptr_); + } else { + deactivateButton(accept_start_button_ptr_); + } +} + +void StatePanel::onMRMState(const MRMState::ConstSharedPtr msg) +{ + // state + { + QString text = ""; + QString style_sheet = ""; + switch (msg->state) { + case MRMState::NONE: + text = "NONE"; + style_sheet = "background-color: #00FF00;"; // green + break; + + case MRMState::MRM_OPERATING: + text = "MRM_OPERATING"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + case MRMState::MRM_SUCCEEDED: + text = "MRM_SUCCEEDED"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case MRMState::MRM_FAILED: + text = "MRM_FAILED"; + style_sheet = "background-color: #FF0000;"; // red + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(mrm_state_label_ptr_, text, style_sheet); + } + + // behavior + { + QString text = ""; + QString style_sheet = ""; + switch (msg->behavior) { + case MRMState::NONE: + text = "NONE"; + style_sheet = "background-color: #00FF00;"; // green + break; + + case MRMState::PULL_OVER: + text = "PULL_OVER"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case MRMState::COMFORTABLE_STOP: + text = "COMFORTABLE_STOP"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case MRMState::EMERGENCY_STOP: + text = "EMERGENCY_STOP"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(mrm_behavior_label_ptr_, text, style_sheet); + } +} + +void StatePanel::onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) +{ + switch (msg->report) { + case autoware_vehicle_msgs::msg::GearReport::PARK: + gear_label_ptr_->setText("PARKING"); + break; + case autoware_vehicle_msgs::msg::GearReport::REVERSE: + gear_label_ptr_->setText("REVERSE"); + break; + case autoware_vehicle_msgs::msg::GearReport::DRIVE: + gear_label_ptr_->setText("DRIVE"); + break; + case autoware_vehicle_msgs::msg::GearReport::NEUTRAL: + gear_label_ptr_->setText("NEUTRAL"); + break; + case autoware_vehicle_msgs::msg::GearReport::LOW: + gear_label_ptr_->setText("LOW"); + break; + } +} + +void StatePanel::onEmergencyStatus( + const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg) +{ + current_emergency_ = msg->emergency; + if (msg->emergency) { + emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency")); + emergency_button_ptr_->setStyleSheet("background-color: #FF0000;"); + } else { + emergency_button_ptr_->setText(QString::fromStdString("Set Emergency")); + emergency_button_ptr_->setStyleSheet("background-color: #00FF00;"); + } +} + +void StatePanel::onClickVelocityLimit() +{ + auto velocity_limit = std::make_shared(); + velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6; + pub_velocity_limit_->publish(*velocity_limit); +} + +void StatePanel::onClickAutonomous() +{ + callServiceWithoutResponse(client_change_to_autonomous_); +} +void StatePanel::onClickStop() +{ + callServiceWithoutResponse(client_change_to_stop_); +} +void StatePanel::onClickLocal() +{ + callServiceWithoutResponse(client_change_to_local_); +} +void StatePanel::onClickRemote() +{ + callServiceWithoutResponse(client_change_to_remote_); +} +void StatePanel::onClickAutowareControl() +{ + callServiceWithoutResponse(client_enable_autoware_control_); +} +void StatePanel::onClickDirectControl() +{ + callServiceWithoutResponse(client_enable_direct_control_); +} + +void StatePanel::onClickClearRoute() +{ + callServiceWithoutResponse(client_clear_route_); +} + +void StatePanel::onClickInitByGnss() +{ + callServiceWithoutResponse(client_init_by_gnss_); +} + +void StatePanel::onClickAcceptStart() +{ + callServiceWithoutResponse(client_accept_start_); +} + +void StatePanel::onClickEmergencyButton() +{ + using tier4_external_api_msgs::msg::ResponseStatus; + using tier4_external_api_msgs::srv::SetEmergency; + + auto request = std::make_shared(); + request->emergency = !current_emergency_; + + RCLCPP_INFO(raw_node_->get_logger(), request->emergency ? "Set Emergency" : "Clear Emergency"); + + client_emergency_stop_->async_send_request( + request, [this](rclcpp::Client::SharedFuture result) { + const auto & response = result.get(); + if (response->status.code == ResponseStatus::SUCCESS) { + RCLCPP_INFO(raw_node_->get_logger(), "service succeeded"); + } else { + RCLCPP_WARN( + raw_node_->get_logger(), "service failed: %s", response->status.message.c_str()); + } + }); +} + +} // namespace tier4_adapi_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::StatePanel, rviz_common::Panel) diff --git a/common/tier4_adapi_rviz_plugin/src/state_panel.hpp b/common/tier4_adapi_rviz_plugin/src/state_panel.hpp new file mode 100644 index 0000000000000..b30494772061e --- /dev/null +++ b/common/tier4_adapi_rviz_plugin/src/state_panel.hpp @@ -0,0 +1,206 @@ +// +// Copyright 2020 TIER IV, Inc. All rights reserved. +// +// 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 STATE_PANEL_HPP_ +#define STATE_PANEL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace tier4_adapi_rviz_plugins +{ + +class StatePanel : public rviz_common::Panel +{ + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + using RouteState = autoware_adapi_v1_msgs::msg::RouteState; + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + using LocalizationInitializationState = + autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization; + using MotionState = autoware_adapi_v1_msgs::msg::MotionState; + using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; + using MRMState = autoware_adapi_v1_msgs::msg::MrmState; + + Q_OBJECT + +public: + explicit StatePanel(QWidget * parent = nullptr); + void onInitialize() override; + +public Q_SLOTS: // NOLINT for Qt + void onClickAutonomous(); + void onClickStop(); + void onClickLocal(); + void onClickRemote(); + void onClickAutowareControl(); + void onClickDirectControl(); + void onClickClearRoute(); + void onClickInitByGnss(); + void onClickAcceptStart(); + void onClickVelocityLimit(); + void onClickEmergencyButton(); + +protected: + // Layout + QGroupBox * makeOperationModeGroup(); + QGroupBox * makeControlModeGroup(); + QGroupBox * makeRoutingGroup(); + QGroupBox * makeLocalizationGroup(); + QGroupBox * makeMotionGroup(); + QGroupBox * makeFailSafeGroup(); + + void onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); + void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); + + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_gear_; + rclcpp::Client::SharedPtr client_emergency_stop_; + rclcpp::Subscription::SharedPtr sub_emergency_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + + // Operation Mode + QLabel * operation_mode_label_ptr_{nullptr}; + QPushButton * stop_button_ptr_{nullptr}; + QPushButton * auto_button_ptr_{nullptr}; + QPushButton * local_button_ptr_{nullptr}; + QPushButton * remote_button_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_operation_mode_; + rclcpp::Client::SharedPtr client_change_to_autonomous_; + rclcpp::Client::SharedPtr client_change_to_stop_; + rclcpp::Client::SharedPtr client_change_to_local_; + rclcpp::Client::SharedPtr client_change_to_remote_; + + // Control Mode + QLabel * control_mode_label_ptr_{nullptr}; + QPushButton * enable_button_ptr_{nullptr}; + QPushButton * disable_button_ptr_{nullptr}; + rclcpp::Client::SharedPtr client_enable_autoware_control_; + rclcpp::Client::SharedPtr client_enable_direct_control_; + + // Functions + void onOperationMode(const OperationModeState::ConstSharedPtr msg); + void changeOperationMode(const rclcpp::Client::SharedPtr client); + + // Routing + QLabel * routing_label_ptr_{nullptr}; + QPushButton * clear_route_button_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Client::SharedPtr client_clear_route_; + + void onRoute(const RouteState::ConstSharedPtr msg); + + // Localization + QLabel * localization_label_ptr_{nullptr}; + QPushButton * init_by_gnss_button_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_localization_; + rclcpp::Client::SharedPtr client_init_by_gnss_; + + void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg); + + // Motion + QLabel * motion_label_ptr_{nullptr}; + QPushButton * accept_start_button_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_motion_; + rclcpp::Client::SharedPtr client_accept_start_; + + void onMotion(const MotionState::ConstSharedPtr msg); + + // FailSafe + QLabel * mrm_state_label_ptr_{nullptr}; + QLabel * mrm_behavior_label_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_mrm_; + + void onMRMState(const MRMState::ConstSharedPtr msg); + + // Others + QPushButton * velocity_limit_button_ptr_; + QLabel * gear_label_ptr_; + + QSpinBox * pub_velocity_limit_input_; + QPushButton * emergency_button_ptr_; + + bool current_emergency_{false}; + + template + void callServiceWithoutResponse(const typename rclcpp::Client::SharedPtr client) + { + auto req = std::make_shared(); + + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); + + if (!client->service_is_ready()) { + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); + return; + } + + client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { + RCLCPP_DEBUG( + raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code, + result.get()->status.message.c_str()); + }); + } + + static void updateLabel(QLabel * label, QString text, QString style_sheet) + { + label->setText(text); + label->setStyleSheet(style_sheet); + } + + static void activateButton(QAbstractButton * button) + { + button->setChecked(false); + button->setEnabled(true); + } + + static void deactivateButton(QAbstractButton * button) + { + button->setChecked(true); + button->setEnabled(false); + } +}; + +} // namespace tier4_adapi_rviz_plugins + +#endif // STATE_PANEL_HPP_ diff --git a/common/tier4_autoware_utils/README.md b/common/tier4_autoware_utils/README.md deleted file mode 100644 index ffcc414cd76ce..0000000000000 --- a/common/tier4_autoware_utils/README.md +++ /dev/null @@ -1,9 +0,0 @@ -# tier4_autoware_utils - -## Purpose - -This package contains many common functions used by other packages, so please refer to them as needed. - -## For developers - -`tier4_autoware_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index ab6a0b25fd3b4..ef5663f024c8d 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -61,22 +61,22 @@ void visualizeBound( const auto [normal_vector_angle, adaptive_width] = [&]() -> std::pair { if (i == 0) { return std::make_pair( - tier4_autoware_utils::calcAzimuthAngle(bound.at(i), bound.at(i + 1)) + M_PI_2, width); + autoware::universe_utils::calcAzimuthAngle(bound.at(i), bound.at(i + 1)) + M_PI_2, width); } if (i == bound.size() - 1) { return std::make_pair( - tier4_autoware_utils::calcAzimuthAngle(bound.at(i - 1), bound.at(i)) + M_PI_2, width); + autoware::universe_utils::calcAzimuthAngle(bound.at(i - 1), bound.at(i)) + M_PI_2, width); } const auto & prev_p = bound.at(i - 1); const auto & curr_p = bound.at(i); const auto & next_p = bound.at(i + 1); - const float curr_to_prev_angle = tier4_autoware_utils::calcAzimuthAngle(curr_p, prev_p); - const float curr_to_next_angle = tier4_autoware_utils::calcAzimuthAngle(curr_p, next_p); + const float curr_to_prev_angle = autoware::universe_utils::calcAzimuthAngle(curr_p, prev_p); + const float curr_to_next_angle = autoware::universe_utils::calcAzimuthAngle(curr_p, next_p); const float normal_vector_angle = (curr_to_prev_angle + curr_to_next_angle) / 2.0; const float diff_angle = - tier4_autoware_utils::normalizeRadian(normal_vector_angle - curr_to_next_angle); + autoware::universe_utils::normalizeRadian(normal_vector_angle - curr_to_next_angle); if (diff_angle == 0.0) { return std::make_pair(normal_vector_angle, width); } diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index 9cb710c239029..25f890799465d 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -86,8 +86,8 @@ bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) { for (auto && path_point : msg_ptr->points) { if ( - !rviz_common::validateFloats(tier4_autoware_utils::getPose(path_point)) && - !rviz_common::validateFloats(tier4_autoware_utils::getLongitudinalVelocity(path_point))) { + !rviz_common::validateFloats(autoware::universe_utils::getPose(path_point)) && + !rviz_common::validateFloats(autoware::universe_utils::getLongitudinalVelocity(path_point))) { return false; } } @@ -358,8 +358,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { const auto & path_point = msg_ptr->points.at(point_idx); - const auto & pose = tier4_autoware_utils::getPose(path_point); - const auto & velocity = tier4_autoware_utils::getLongitudinalVelocity(path_point); + const auto & pose = autoware::universe_utils::getPose(path_point); + const auto & velocity = autoware::universe_utils::getLongitudinalVelocity(path_point); // path if (property_path_view_.getBool()) { @@ -454,9 +454,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay (point_idx != msg_ptr->points.size() - 1) ? point_idx + 1 : point_idx; const auto & prev_path_pos = - tier4_autoware_utils::getPose(msg_ptr->points.at(prev_idx)).position; + autoware::universe_utils::getPose(msg_ptr->points.at(prev_idx)).position; const auto & next_path_pos = - tier4_autoware_utils::getPose(msg_ptr->points.at(next_idx)).position; + autoware::universe_utils::getPose(msg_ptr->points.at(next_idx)).position; Ogre::Vector3 position; position.x = pose.position.x; @@ -466,7 +466,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->setPosition(position); rviz_rendering::MovableText * text = slope_texts_.at(point_idx); - const double slope = tier4_autoware_utils::calcElevationAngle(prev_path_pos, next_path_pos); + const double slope = + autoware::universe_utils::calcElevationAngle(prev_path_pos, next_path_pos); std::stringstream ss; ss << std::fixed << std::setprecision(2) << slope; @@ -510,7 +511,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay for (size_t p_idx = 0; p_idx < msg_ptr->points.size(); p_idx++) { const auto & point = msg_ptr->points.at(p_idx); - const auto & pose = tier4_autoware_utils::getPose(point); + const auto & pose = autoware::universe_utils::getPose(point); // footprint if (property_footprint_view_.getBool()) { Ogre::ColourValue color; diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp index 94943a25f6a64..7e99af8b9efb6 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp @@ -15,8 +15,8 @@ #ifndef TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ #define TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ -#include -#include +#include +#include #include @@ -29,7 +29,7 @@ bool isDrivingForward(const T points_with_twist, size_t target_idx) // 1. check velocity const double target_velocity = - tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.at(target_idx)); + autoware::universe_utils::getLongitudinalVelocity(points_with_twist.at(target_idx)); if (epsilon < target_velocity) { return true; } else if (target_velocity < -epsilon) { @@ -46,13 +46,13 @@ bool isDrivingForward(const T points_with_twist, size_t target_idx) const size_t first_idx = is_last_point ? target_idx - 1 : target_idx; const size_t second_idx = is_last_point ? target_idx : target_idx + 1; - const auto first_pose = tier4_autoware_utils::getPose(points_with_twist.at(first_idx)); - const auto second_pose = tier4_autoware_utils::getPose(points_with_twist.at(second_idx)); + const auto first_pose = autoware::universe_utils::getPose(points_with_twist.at(first_idx)); + const auto second_pose = autoware::universe_utils::getPose(points_with_twist.at(second_idx)); const double first_traj_yaw = tf2::getYaw(first_pose.orientation); const double driving_direction_yaw = - tier4_autoware_utils::calcAzimuthAngle(first_pose.position, second_pose.position); + autoware::universe_utils::calcAzimuthAngle(first_pose.position, second_pose.position); if ( - std::abs(tier4_autoware_utils::normalizeRadian(first_traj_yaw - driving_direction_yaw)) < + std::abs(autoware::universe_utils::normalizeRadian(first_traj_yaw - driving_direction_yaw)) < M_PI_2) { return true; } diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index 95edcb7a96c56..e42d8c5a05a12 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -11,12 +11,13 @@ ament_cmake autoware_cmake + autoware_motion_utils autoware_planning_msgs + autoware_universe_utils autoware_vehicle_info_utils libqt5-core libqt5-gui libqt5-widgets - motion_utils qtbase5-dev rclcpp rviz_common @@ -24,7 +25,6 @@ rviz_rendering tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs ament_lint_auto diff --git a/common/tier4_system_rviz_plugin/package.xml b/common/tier4_system_rviz_plugin/package.xml index bd72ecf97def4..bd888fb085960 100644 --- a/common/tier4_system_rviz_plugin/package.xml +++ b/common/tier4_system_rviz_plugin/package.xml @@ -11,6 +11,7 @@ autoware_cmake autoware_system_msgs + autoware_universe_utils diagnostic_msgs libqt5-core libqt5-gui @@ -19,7 +20,6 @@ rviz_common rviz_default_plugins rviz_ogre_vendor - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/common/tier4_vehicle_rviz_plugin/package.xml b/common/tier4_vehicle_rviz_plugin/package.xml index 922ebd2c5e318..69057701886cb 100644 --- a/common/tier4_vehicle_rviz_plugin/package.xml +++ b/common/tier4_vehicle_rviz_plugin/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils autoware_vehicle_msgs libqt5-core libqt5-gui @@ -18,7 +19,6 @@ rviz_common rviz_default_plugins rviz_ogre_vendor - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp index 3b3810a782a91..1d884d01065fa 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp @@ -231,14 +231,14 @@ void AccelerationMeterDisplay::updateVisualization() inner_arc_.y0 = line_width_ / 2.0; inner_arc_.x1 = w; inner_arc_.y1 = h; - inner_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); - inner_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); + inner_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); outer_arc_.x0 = w * 3 / 8; outer_arc_.y0 = h * 3 / 8; outer_arc_.x1 = w / 4; outer_arc_.y1 = h / 4; - outer_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); - outer_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); } } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp index 8a962cea575e8..03fef97f536a5 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp @@ -21,11 +21,11 @@ #ifndef Q_MOC_RUN #include "jsk_overlay_utils.hpp" +#include #include #include #include #include -#include #include #endif @@ -67,8 +67,8 @@ private Q_SLOTS: private: static constexpr float meter_min_acceleration_ = -10.0f; static constexpr float meter_max_acceleration_ = 10.0f; - static constexpr float meter_min_angle_ = tier4_autoware_utils::deg2rad(40.f); - static constexpr float meter_max_angle_ = tier4_autoware_utils::deg2rad(320.f); + static constexpr float meter_min_angle_ = autoware::universe_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = autoware::universe_utils::deg2rad(320.f); static constexpr int line_width_ = 2; static constexpr int hand_width_ = 4; struct Line // for drawLine diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp index 5a9f7e602efc7..8f7348c87be3c 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp @@ -212,14 +212,14 @@ void ConsoleMeterDisplay::updateVisualization() inner_arc_.y0 = line_width_ / 2.0; inner_arc_.x1 = w; inner_arc_.y1 = h; - inner_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); - inner_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); + inner_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); outer_arc_.x0 = w * 3 / 8; outer_arc_.y0 = h * 3 / 8; outer_arc_.x1 = w / 4; outer_arc_.y1 = h / 4; - outer_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); - outer_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.start_angle = autoware::universe_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = autoware::universe_utils::rad2deg(max_range_theta - min_range_theta); } } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp index 8f1a62fc35361..98cf8bbae4228 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -21,12 +21,12 @@ #ifndef Q_MOC_RUN #include "jsk_overlay_utils.hpp" +#include +#include #include #include #include #include -#include -#include #include #endif @@ -63,10 +63,10 @@ private Q_SLOTS: // QImage hud_; private: - static constexpr float meter_min_velocity_ = tier4_autoware_utils::kmph2mps(0.f); - static constexpr float meter_max_velocity_ = tier4_autoware_utils::kmph2mps(60.f); - static constexpr float meter_min_angle_ = tier4_autoware_utils::deg2rad(40.f); - static constexpr float meter_max_angle_ = tier4_autoware_utils::deg2rad(320.f); + static constexpr float meter_min_velocity_ = autoware::universe_utils::kmph2mps(0.f); + static constexpr float meter_max_velocity_ = autoware::universe_utils::kmph2mps(60.f); + static constexpr float meter_min_angle_ = autoware::universe_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = autoware::universe_utils::deg2rad(320.f); static constexpr int line_width_ = 2; static constexpr int hand_width_ = 4; struct Line // for drawLine diff --git a/common/traffic_light_recognition_marker_publisher/package.xml b/common/traffic_light_recognition_marker_publisher/package.xml index 2b65c5b868313..71a3e5238b388 100644 --- a/common/traffic_light_recognition_marker_publisher/package.xml +++ b/common/traffic_light_recognition_marker_publisher/package.xml @@ -14,11 +14,11 @@ autoware_map_msgs autoware_perception_msgs + autoware_universe_utils geometry_msgs lanelet2_extension rclcpp rclcpp_components - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/common/tvm_utility/example/yolo_v2_tiny/main.cpp b/common/tvm_utility/example/yolo_v2_tiny/main.cpp index 8fd0cde3c306d..38b3355ca38ba 100644 --- a/common/tvm_utility/example/yolo_v2_tiny/main.cpp +++ b/common/tvm_utility/example/yolo_v2_tiny/main.cpp @@ -120,7 +120,6 @@ class PostProcessorYoloV2Tiny : public tvm_utility::pipeline::PostProcessor + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + +
+
+
+
+ +
+
+
+
+ + + + + + + + + + + + + + + +
+
+
+ + + +
+
+
+
+ +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
diff --git a/control/autoware_autonomous_emergency_braking/image/front_vehicle_collision.drawio.svg b/control/autoware_autonomous_emergency_braking/image/front_vehicle_collision.drawio.svg new file mode 100644 index 0000000000000..cb7caefad5a8a --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/front_vehicle_collision.drawio.svg @@ -0,0 +1,557 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+
+ +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Predicted path +
+
+
+
+ +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Front vehicle suddenly brakes +
+
+
+
+ +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ After the reaction time, the AEB has detected a collision an activated the brakes +
+
+
+
+ +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Front vehicle stops +
+
+
+
+ +
+
+
+ + + + + + + +
+
+
+ Ego stops +
+
+
+
+ +
+
+
+ + + + + + + + + + + + +
+
diff --git a/control/autoware_autonomous_emergency_braking/image/object_relative_speed.drawio.svg b/control/autoware_autonomous_emergency_braking/image/object_relative_speed.drawio.svg new file mode 100644 index 0000000000000..7b0b7b8631943 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/object_relative_speed.drawio.svg @@ -0,0 +1,192 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
Closest Point t_0
+
+
+
+ +
+
+
+ + + + + + + + + +
+
+
Closest Point t_1
+
+
+
+ +
+
+
+ + + + + + + + + +
+
+
dx
+
+
+
+ +
+
+
+ + + + + + + + + + + + + + + + + + +
+
diff --git a/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg index c1fa2e16ff7fb..ee5ec62fbc1d4 100644 --- a/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg +++ b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg @@ -1,133 +1,300 @@ - + + - - - - - - - - - -
-
-
- Ego Predicted Path -
-
-
-
- Ego Predicted Path -
-
- - - - - - - - - - - -
-
-
- Pointcloud -
-
-
-
- Pointcloud -
-
- - - - - - -
-
-
- Ignore point outside of the search area -
-
-
-
- Ignore point outside of the search area -
-
- - - - - - - - -
-
-
- Search Area + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
Original Point Cloud
-
- - Search Area - - - - - - - - -
-
-
- Ignore point behind the ego vehicle + + + + + + + + + + + + +
+
+
+ Cropping the Point Cloud with extended path +
-
- - Ignore point behind the ego vehicle - + + + + + + + + + + - - - - Text is not SVG - cannot display - - diff --git a/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg index d0340892aa5e9..1d12fe233ad8e 100644 --- a/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg +++ b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg @@ -1,32 +1,365 @@ - + + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Closest Point +
+
+
+
+ +
+
+
+ + + + + + + +
+
+
+ Filter out points outside clusters and get a 2D convex hull of each cluster +
+
+
+
+ +
+
+
+ + + + + + + +
+
+
+ Find the closest vertex of the convex hull(s) +
+
+
+
+ +
+
+
+ + + + + +
diff --git a/control/autoware_autonomous_emergency_braking/image/occluded_space.drawio.svg b/control/autoware_autonomous_emergency_braking/image/occluded_space.drawio.svg new file mode 100644 index 0000000000000..0cb31d5ab6e72 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/occluded_space.drawio.svg @@ -0,0 +1,453 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+
+ +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Occlusion +
+
+
+
+ +
+
+
+ + + + + + + +
+
+
+ Occlusion +
+
+
+
+ +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Occlusion +
+
+
+
+ +
+
+
+ + + + + + + + + + + + + +
+
+
+ AEB triggers emergency brakes +
+
+
+
+ +
+
+
+ + + + + + + +
+
+
+ Ego stops before collision +
+
+
+
+ +
+
+
+
+
diff --git a/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg index 71d6e6ff8932c..2d1716519631d 100644 --- a/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg +++ b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg @@ -1,76 +1,134 @@ - + + - - - - - - - - - - - - - -
-
-
- obj_dist + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + Closest Point Distance +
-
- - obj_dist - + + + + + + + + + + + + - - - - - - - - - -
-
-
- - rss_dist -
-
+ + + + +
+
+
RSS distance
-
- - rss_dist - + + + + - - - - Text is not SVG - cannot display - - diff --git a/control/autoware_autonomous_emergency_braking/image/wrong-mpc.drawio.svg b/control/autoware_autonomous_emergency_braking/image/wrong-mpc.drawio.svg new file mode 100644 index 0000000000000..a829cb60ec7d6 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/wrong-mpc.drawio.svg @@ -0,0 +1,208 @@ + + + + + + + + + + + + + + + +
+
+
+ + + +
+
+
+
+ +
+
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ IMU +
+
+
+
+ +
+
+
+ + + + + + + +
+
+
+ MPC +
+
+
+
+ +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ MPC path is wrong +
+
+
+
+ +
+
+
+
+
diff --git a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp similarity index 85% rename from control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp rename to control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index aca096aac1d82..8c454d5d583b6 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ -#define AUTOWARE_AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#include +#include +#include #include #include -#include #include #include -#include -#include +#include #include #include #include @@ -59,15 +60,18 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Path = std::vector; using Vector3 = geometry_msgs::msg::Vector3; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; + struct ObjectData { rclcpp::Time stamp; @@ -176,6 +180,13 @@ class CollisionDataKeeper std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed) { + // in case the object comes from predicted objects info, we reuse the speed. + if (closest_object.velocity > 0.0) { + this->setPreviousObjectData(closest_object); + this->updateVelocityHistory(closest_object.velocity, closest_object.stamp); + return this->getMedianObstacleVelocity(); + } + if (this->checkPreviousObjectDataExpired()) { this->setPreviousObjectData(closest_object); this->resetVelocityHistory(); @@ -196,7 +207,8 @@ class CollisionDataKeeper const double p_yaw = std::atan2(p_dy, p_dx); const double p_vel = p_dist / p_dt; - const auto nearest_idx = motion_utils::findNearestIndex(path, nearest_collision_point); + const auto nearest_idx = + autoware::motion_utils::findNearestIndex(path, nearest_collision_point); const auto & nearest_path_pose = path.at(nearest_idx); // When the ego moves backwards, the direction of movement axis is reversed const auto & traj_yaw = (current_ego_speed > 0.0) @@ -229,27 +241,22 @@ class CollisionDataKeeper rclcpp::Clock::SharedPtr clock_; }; -static rclcpp::SensorDataQoS SingleDepthSensorQoS() -{ - rclcpp::SensorDataQoS qos; - qos.get_rmw_qos_profile().depth = 1; - return qos; -} - class AEB : public rclcpp::Node { public: explicit AEB(const rclcpp::NodeOptions & node_options); // subscriber - tier4_autoware_utils::InterProcessPollingSubscriber sub_point_cloud_{ - this, "~/input/pointcloud", SingleDepthSensorQoS()}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_velocity_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_point_cloud_{ + this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; + autoware::universe_utils::InterProcessPollingSubscriber sub_velocity_{ this, "~/input/velocity"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_traj_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_traj_{ this, "~/input/predicted_trajectory"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_autoware_state_{ + autoware::universe_utils::InterProcessPollingSubscriber predicted_objects_sub_{ + this, "~/input/objects"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; @@ -281,6 +288,10 @@ class AEB : public rclcpp::Node std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr); + void createObjectDataUsingPredictedObjects( + const Path & ego_path, const std::vector & ego_polys, + std::vector & objects); + void cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); @@ -304,6 +315,7 @@ class AEB : public rclcpp::Node VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; Vector3::SharedPtr angular_velocity_ptr_{nullptr}; Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; + PredictedObjects::ConstSharedPtr predicted_objects_ptr_{nullptr}; AutowareState::ConstSharedPtr autoware_state_{nullptr}; tf2_ros::Buffer tf_buffer_{get_clock()}; @@ -319,6 +331,9 @@ class AEB : public rclcpp::Node bool publish_debug_pointcloud_; bool use_predicted_trajectory_; bool use_imu_path_; + bool use_pointcloud_data_; + bool use_predicted_object_data_; + bool use_object_velocity_calculation_; double path_footprint_extra_margin_; double detection_range_min_height_; double detection_range_max_height_margin_; @@ -344,4 +359,4 @@ class AEB : public rclcpp::Node }; } // namespace autoware::motion::control::autonomous_emergency_braking -#endif // AUTOWARE_AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp new file mode 100644 index 0000000000000..64bedd2163912 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -0,0 +1,88 @@ +// Copyright 2024 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 AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ +#define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ + +#include + +#include +#include +#include + +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#include + +namespace autoware::motion::control::autonomous_emergency_braking::utils +{ +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; + +/** + * @brief Apply a transform to a predicted object + * @param input the predicted object + * @param transform_stamped the tf2 transform + */ +PredictedObject transformObjectFrame( + const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped); + +/** + * @brief Get the predicted objects polygon as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); + +/** + * @brief Get the predicted objects cylindrical shape as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); + +/** + * @brief Get the predicted objects bounding box shape as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width); + +/** + * @brief Get the predicted object's shape as a geometry polygon + * @param obj the object + */ +Polygon2d convertObjToPolygon(const PredictedObject & obj); +} // namespace autoware::motion::control::autonomous_emergency_braking::utils + +#endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml index 75b1a9dcf822d..fe0df41ca89c7 100644 --- a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml +++ b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml @@ -5,6 +5,7 @@ + @@ -15,5 +16,6 @@ + diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index 0404eec7ca308..c822adb510805 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -15,13 +15,14 @@ autoware_cmake autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_system_msgs + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_updater geometry_msgs - motion_utils nav_msgs pcl_conversions pcl_ros @@ -34,7 +35,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 1d977b58e9ceb..fca3341680997 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -12,16 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_autonomous_emergency_braking/node.hpp" - +#include +#include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include + +#include #include +#include +#include #include #include @@ -46,6 +50,7 @@ namespace autoware::motion::control::autonomous_emergency_braking { +using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; using diagnostic_msgs::msg::DiagnosticStatus; namespace bg = boost::geometry; @@ -70,33 +75,37 @@ Polygon2d createPolygon( appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); + autoware::universe_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); + autoware::universe_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); + polygon, + autoware::universe_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); + polygon, + autoware::universe_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); + autoware::universe_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); + autoware::universe_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); + polygon, + autoware::universe_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); + polygon, + autoware::universe_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); - polygon = tier4_autoware_utils::isClockwise(polygon) + polygon = autoware::universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); Polygon2d hull_polygon; bg::convex_hull(polygon, hull_polygon); - + bg::correct(hull_polygon); return hull_polygon; } @@ -120,6 +129,9 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); + use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); + use_predicted_object_data_ = declare_parameter("use_predicted_object_data"); + use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); detection_range_min_height_ = declare_parameter("detection_range_min_height"); detection_range_max_height_margin_ = @@ -163,10 +175,14 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) rcl_interfaces::msg::SetParametersResult AEB::onParameter( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); + updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); + updateParam(parameters, "use_predicted_object_data", use_predicted_object_data_); + updateParam( + parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); updateParam(parameters, "detection_range_min_height", detection_range_min_height_); updateParam( @@ -291,13 +307,31 @@ bool AEB::fetchLatestData() return missing("ego velocity"); } - const auto pointcloud_ptr = sub_point_cloud_.takeData(); - if (!pointcloud_ptr) { - return missing("object pointcloud message"); + if (use_pointcloud_data_) { + const auto pointcloud_ptr = sub_point_cloud_.takeData(); + if (!pointcloud_ptr) { + return missing("object pointcloud message"); + } + + onPointCloud(pointcloud_ptr); + if (!obstacle_ros_pointcloud_ptr_) { + return missing("object pointcloud"); + } + } else { + obstacle_ros_pointcloud_ptr_.reset(); } - onPointCloud(pointcloud_ptr); - if (!obstacle_ros_pointcloud_ptr_) { - return missing("object pointcloud"); + + if (use_predicted_object_data_) { + predicted_objects_ptr_ = predicted_objects_sub_.takeData(); + if (!predicted_objects_ptr_) { + return missing("predicted objects"); + } + } else { + predicted_objects_ptr_.reset(); + } + + if (!obstacle_ros_pointcloud_ptr_ && !predicted_objects_ptr_) { + return missing("object detection method (pointcloud or predicted objects)"); } const auto imu_ptr = sub_imu_.takeData(); @@ -374,31 +408,39 @@ bool AEB::checkCollision(MarkerArray & debug_markers) const auto & path, const colorTuple & debug_colors, const std::string & debug_ns, pcl::PointCloud::Ptr filtered_objects) { - // Crop out Pointcloud using an extra wide ego path - const auto expanded_ego_polys = - generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_); - cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects); - // Check which points of the cropped point cloud are on the ego path, and get the closest one - std::vector objects_from_point_clusters; const auto ego_polys = generatePathFootprint(path, expand_width_); - const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; - createObjectDataUsingPointCloudClusters( - path, ego_polys, current_time, objects_from_point_clusters, filtered_objects); + auto objects = std::invoke([&]() { + std::vector objects; + // Crop out Pointcloud using an extra wide ego path + if (use_pointcloud_data_) { + const auto expanded_ego_polys = + generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_); + cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects); + const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; + createObjectDataUsingPointCloudClusters( + path, ego_polys, current_time, objects, filtered_objects); + } + if (use_predicted_object_data_) { + createObjectDataUsingPredictedObjects(path, ego_polys, objects); + } + return objects; + }); // Get only the closest object and calculate its speed const auto closest_object_point = std::invoke([&]() -> std::optional { - const auto closest_object_point_itr = std::min_element( - objects_from_point_clusters.begin(), objects_from_point_clusters.end(), - [](const auto & o1, const auto & o2) { + const auto closest_object_point_itr = + std::min_element(objects.begin(), objects.end(), [](const auto & o1, const auto & o2) { return o1.distance_to_object < o2.distance_to_object; }); - if (closest_object_point_itr == objects_from_point_clusters.end()) { + if (closest_object_point_itr == objects.end()) { return std::nullopt; } - const auto closest_object_speed = collision_data_keeper_.calcObjectSpeedFromHistory( - *closest_object_point_itr, path, current_v); + const auto closest_object_speed = (use_object_velocity_calculation_) + ? collision_data_keeper_.calcObjectSpeedFromHistory( + *closest_object_point_itr, path, current_v) + : std::make_optional(0.0); if (!closest_object_speed.has_value()) { return std::nullopt; } @@ -410,8 +452,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers) { const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( - this->get_clock()->now(), path, ego_polys, objects_from_point_clusters, - closest_object_point, color_r, color_g, color_b, color_a, debug_ns, debug_markers); + this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g, + color_b, color_a, debug_ns, debug_markers); } // check collision using rss distance return (closest_object_point.has_value()) @@ -453,9 +495,9 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // Debug print if (!filtered_objects->empty() && publish_debug_pointcloud_) { - const auto filtered_objects_ros_pointcloud_ptr_ = std::make_shared(); - pcl::toROSMsg(*filtered_objects, *filtered_objects_ros_pointcloud_ptr_); - pub_obstacle_pointcloud_->publish(*filtered_objects_ros_pointcloud_ptr_); + const auto filtered_objects_ros_pointcloud_ptr = std::make_shared(); + pcl::toROSMsg(*filtered_objects, *filtered_objects_ros_pointcloud_ptr); + pub_obstacle_pointcloud_->publish(*filtered_objects_ros_pointcloud_ptr); } return has_collision; } @@ -485,8 +527,8 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) double curr_y = 0.0; double curr_yaw = 0.0; geometry_msgs::msg::Pose ini_pose; - ini_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); - ini_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + ini_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); + ini_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); if (std::abs(curr_v) < 0.1) { @@ -502,23 +544,23 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose; - current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); - if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); + if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { continue; } path.push_back(current_pose); } // If path is shorter than minimum path length - while (motion_utils::calcArcLength(path) < min_generated_path_length_) { + while (autoware::motion_utils::calcArcLength(path) < min_generated_path_length_) { curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose; - current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); - if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); + if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { continue; } path.push_back(current_pose); @@ -568,6 +610,93 @@ std::vector AEB::generatePathFootprint( return polygons; } +void AEB::createObjectDataUsingPredictedObjects( + const Path & ego_path, const std::vector & ego_polys, + std::vector & object_data_vector) +{ + if (predicted_objects_ptr_->objects.empty()) return; + + const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity; + const auto & objects = predicted_objects_ptr_->objects; + const auto & stamp = predicted_objects_ptr_->header.stamp; + + // Ego position + const auto current_p = [&]() { + const auto & first_point_of_path = ego_path.front(); + const auto & p = first_point_of_path.position; + return autoware::universe_utils::createPoint(p.x, p.y, p.z); + }(); + + auto get_object_tangent_velocity = + [&](const PredictedObject & predicted_object, const auto & obj_pose) { + const double obj_vel_norm = std::hypot( + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); + + const auto obj_yaw = tf2::getYaw(obj_pose.orientation); + const auto obj_idx = autoware::motion_utils::findNearestIndex(ego_path, obj_pose.position); + const auto path_yaw = (current_ego_speed > 0.0) + ? tf2::getYaw(ego_path.at(obj_idx).orientation) + : tf2::getYaw(ego_path.at(obj_idx).orientation) + M_PI; + return obj_vel_norm * std::cos(obj_yaw - path_yaw); + }; + + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", predicted_objects_ptr_->header.frame_id, stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map"); + return; + } + + // Check which objects collide with the ego footprints + std::for_each(objects.begin(), objects.end(), [&](const auto & predicted_object) { + // get objects in base_link frame + const auto t_predicted_object = + utils::transformObjectFrame(predicted_object, transform_stamped); + const auto & obj_pose = t_predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto obj_poly = convertObjToPolygon(t_predicted_object); + const double obj_tangent_velocity = get_object_tangent_velocity(t_predicted_object, obj_pose); + + for (const auto & ego_poly : ego_polys) { + // check collision with 2d polygon + std::vector collision_points_bg; + bg::intersection(ego_poly, obj_poly, collision_points_bg); + if (collision_points_bg.empty()) continue; + + // Create an object for each intersection point + bool collision_points_added{false}; + for (const auto & collision_point : collision_points_bg) { + const auto obj_position = + autoware::universe_utils::createPoint(collision_point.x(), collision_point.y(), 0.0); + const double obj_arc_length = + autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); + if (std::isnan(obj_arc_length)) continue; + + // If the object is behind the ego, we need to use the backward long offset. The + // distance should be a positive number in any case + const bool is_object_in_front_of_ego = obj_arc_length > 0.0; + const double dist_ego_to_object = + (is_object_in_front_of_ego) ? obj_arc_length - vehicle_info_.max_longitudinal_offset_m + : obj_arc_length + vehicle_info_.min_longitudinal_offset_m; + + ObjectData obj; + obj.stamp = stamp; + obj.position = obj_position; + obj.velocity = (obj_tangent_velocity > 0.0) ? obj_tangent_velocity : 0.0; + obj.distance_to_object = std::abs(dist_ego_to_object); + object_data_vector.push_back(obj); + collision_points_added = true; + } + // The ego polygons are in order, so the first intersection points found are the closest + // points. It is not necessary to continue iterating the ego polys for the same object. + if (collision_points_added) break; + } + }); +} + void AEB::createObjectDataUsingPointCloudClusters( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr) @@ -615,13 +744,13 @@ void AEB::createObjectDataUsingPointCloudClusters( const auto current_p = [&]() { const auto & first_point_of_path = ego_path.front(); const auto & p = first_point_of_path.position; - return tier4_autoware_utils::createPoint(p.x, p.y, p.z); + return autoware::universe_utils::createPoint(p.x, p.y, p.z); }(); for (const auto & p : *points_belonging_to_cluster_hulls) { - const auto obj_position = tier4_autoware_utils::createPoint(p.x, p.y, p.z); + const auto obj_position = autoware::universe_utils::createPoint(p.x, p.y, p.z); const double obj_arc_length = - motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); + autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); if (std::isnan(obj_arc_length)) continue; // If the object is behind the ego, we need to use the backward long offset. The distance should @@ -683,36 +812,38 @@ void AEB::addMarker( const double color_r, const double color_g, const double color_b, const double color_a, const std::string & ns, MarkerArray & debug_markers) { - auto path_marker = tier4_autoware_utils::createDefaultMarker( + auto path_marker = autoware::universe_utils::createDefaultMarker( "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.2, 0.2, 0.2), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware::universe_utils::createMarkerScale(0.2, 0.2, 0.2), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); path_marker.points.resize(path.size()); for (size_t i = 0; i < path.size(); ++i) { path_marker.points.at(i) = path.at(i).position; } debug_markers.markers.push_back(path_marker); - auto polygon_marker = tier4_autoware_utils::createDefaultMarker( + auto polygon_marker = autoware::universe_utils::createDefaultMarker( "base_link", current_time, ns + "_polygon", 0, Marker::LINE_LIST, - tier4_autoware_utils::createMarkerScale(0.03, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware::universe_utils::createMarkerScale(0.03, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto & poly : polygons) { for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { const auto & boost_cp = poly.outer().at(dp_idx); const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); - const auto curr_point = tier4_autoware_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); - const auto next_point = tier4_autoware_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); + const auto curr_point = + autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); + const auto next_point = + autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); polygon_marker.points.push_back(curr_point); polygon_marker.points.push_back(next_point); } } debug_markers.markers.push_back(polygon_marker); - auto object_data_marker = tier4_autoware_utils::createDefaultMarker( + auto object_data_marker = autoware::universe_utils::createDefaultMarker( "base_link", current_time, ns + "_objects", 0, Marker::SPHERE_LIST, - tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto & e : objects) { object_data_marker.points.push_back(e.position); } @@ -721,11 +852,11 @@ void AEB::addMarker( // Visualize planner type text if (closest_object.has_value()) { const auto & obj = closest_object.value(); - const auto color = tier4_autoware_utils::createMarkerColor(0.95, 0.95, 0.95, 0.999); - auto closest_object_velocity_marker_array = tier4_autoware_utils::createDefaultMarker( + const auto color = autoware::universe_utils::createMarkerColor(0.95, 0.95, 0.95, 0.999); + auto closest_object_velocity_marker_array = autoware::universe_utils::createDefaultMarker( "base_link", obj.stamp, ns + "_closest_object_velocity", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.0, 0.0, 0.7), color); + autoware::universe_utils::createMarkerScale(0.0, 0.0, 0.7), color); closest_object_velocity_marker_array.pose.position = obj.position; const auto ego_velocity = current_velocity_ptr_->longitudinal_velocity; closest_object_velocity_marker_array.text = @@ -739,10 +870,10 @@ void AEB::addMarker( void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) { - auto point_marker = tier4_autoware_utils::createDefaultMarker( + auto point_marker = autoware::universe_utils::createDefaultMarker( "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.3, 0.3, 0.3), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); + autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); point_marker.pose.position = data.position; debug_markers.markers.push_back(point_marker); } diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp new file mode 100644 index 0000000000000..c284ee3c99dbb --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -0,0 +1,150 @@ +// Copyright 2024 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 + +namespace autoware::motion::control::autonomous_emergency_braking::utils +{ +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Vector3; + +PredictedObject transformObjectFrame( + const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped) +{ + PredictedObject output = input; + const auto & linear_twist = input.kinematics.initial_twist_with_covariance.twist.linear; + const auto & angular_twist = input.kinematics.initial_twist_with_covariance.twist.angular; + const auto & pose = input.kinematics.initial_pose_with_covariance.pose; + + geometry_msgs::msg::Pose t_pose; + Vector3 t_linear_twist; + Vector3 t_angular_twist; + + tf2::doTransform(pose, t_pose, transform_stamped); + tf2::doTransform(linear_twist, t_linear_twist, transform_stamped); + tf2::doTransform(angular_twist, t_angular_twist, transform_stamped); + + output.kinematics.initial_pose_with_covariance.pose = t_pose; + output.kinematics.initial_twist_with_covariance.twist.linear = t_linear_twist; + output.kinematics.initial_twist_with_covariance.twist.angular = t_angular_twist; + return output; +} + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + tf2::Transform tf_map2obj; + fromMsg(current_pose, tf_map2obj); + const auto obj_points = obj_shape.footprint.points; + object_polygon.outer().reserve(obj_points.size() + 1); + for (const auto & obj_point : obj_points) { + tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); + tf2::Vector3 tf_obj = tf_map2obj * obj; + object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + + const double obj_x = current_pose.position.x; + const double obj_y = current_pose.position.y; + + constexpr int N = 20; + const double r = obj_shape.dimensions.x / 2; + object_polygon.outer().reserve(N + 1); + for (int i = 0; i < N; ++i) { + object_polygon.outer().emplace_back( + obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); + } + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width) +{ + const auto mapped_point = [](const double & length_scalar, const double & width_scalar) { + tf2::Vector3 map; + map.setX(length_scalar); + map.setY(width_scalar); + map.setZ(0.0); + map.setW(1.0); + return map; + }; + + // set vertices at map coordinate + const tf2::Vector3 p1_map = std::invoke(mapped_point, base_to_front, -base_to_width); + const tf2::Vector3 p2_map = std::invoke(mapped_point, base_to_front, base_to_width); + const tf2::Vector3 p3_map = std::invoke(mapped_point, -base_to_rear, base_to_width); + const tf2::Vector3 p4_map = std::invoke(mapped_point, -base_to_rear, -base_to_width); + + // transform vertices from map coordinate to object coordinate + tf2::Transform tf_map2obj; + tf2::fromMsg(current_pose, tf_map2obj); + const tf2::Vector3 p1_obj = tf_map2obj * p1_map; + const tf2::Vector3 p2_obj = tf_map2obj * p2_map; + const tf2::Vector3 p3_obj = tf_map2obj * p3_map; + const tf2::Vector3 p4_obj = tf_map2obj * p4_map; + + Polygon2d object_polygon; + object_polygon.outer().reserve(5); + object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); + object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y()); + object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y()); + object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertObjToPolygon(const PredictedObject & obj) +{ + Polygon2d object_polygon{}; + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + object_polygon = utils::convertCylindricalObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + const double & length_m = obj.shape.dimensions.x / 2; + const double & width_m = obj.shape.dimensions.y / 2; + object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { + object_polygon = utils::convertPolygonObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else { + throw std::runtime_error("Unsupported shape type"); + } + return object_polygon; +} +} // namespace autoware::motion::control::autonomous_emergency_braking::utils diff --git a/control/autoware_control_validator/CMakeLists.txt b/control/autoware_control_validator/CMakeLists.txt index 1a4119e47a723..2b25b5b984c12 100644 --- a/control/autoware_control_validator/CMakeLists.txt +++ b/control/autoware_control_validator/CMakeLists.txt @@ -11,7 +11,7 @@ ament_auto_add_library(autoware_control_validator_helpers SHARED # control validator ament_auto_add_library(autoware_control_validator_component SHARED - include/autoware_control_validator/control_validator.hpp + include/autoware/control_validator/control_validator.hpp src/control_validator.cpp ) target_link_libraries(autoware_control_validator_component autoware_control_validator_helpers) diff --git a/control/autoware_control_validator/include/autoware_control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp similarity index 85% rename from control/autoware_control_validator/include/autoware_control_validator/control_validator.hpp rename to control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index 2e31de295f1f2..ecb46aee123e3 100644 --- a/control/autoware_control_validator/include/autoware_control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ -#define AUTOWARE_CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ +#ifndef AUTOWARE__CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ +#define AUTOWARE__CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ -#include "autoware_control_validator/debug_marker.hpp" +#include "autoware/control_validator/debug_marker.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware_control_validator/msg/control_validator_status.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include #include @@ -69,9 +69,9 @@ class ControlValidator : public rclcpp::Node void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); rclcpp::Subscription::SharedPtr sub_predicted_traj_; - tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_kinematics_{ this, "~/input/kinematics"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_reference_traj_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_reference_traj_{ this, "~/input/reference_trajectory"}; rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_markers_; @@ -98,4 +98,4 @@ class ControlValidator : public rclcpp::Node }; } // namespace autoware::control_validator -#endif // AUTOWARE_CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ +#endif // AUTOWARE__CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ diff --git a/control/autoware_control_validator/include/autoware_control_validator/debug_marker.hpp b/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp similarity index 91% rename from control/autoware_control_validator/include/autoware_control_validator/debug_marker.hpp rename to control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp index cde596ed7e1ab..c03ecb46b8117 100644 --- a/control/autoware_control_validator/include/autoware_control_validator/debug_marker.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ -#define AUTOWARE_CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE__CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ +#define AUTOWARE__CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ #include @@ -57,4 +57,4 @@ class ControlValidatorDebugMarkerPublisher } }; -#endif // AUTOWARE_CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ +#endif // AUTOWARE__CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/control/autoware_control_validator/include/autoware_control_validator/utils.hpp b/control/autoware_control_validator/include/autoware/control_validator/utils.hpp similarity index 84% rename from control/autoware_control_validator/include/autoware_control_validator/utils.hpp rename to control/autoware_control_validator/include/autoware/control_validator/utils.hpp index 69faa2fbe4abc..375ad557d02df 100644 --- a/control/autoware_control_validator/include/autoware_control_validator/utils.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_CONTROL_VALIDATOR__UTILS_HPP_ -#define AUTOWARE_CONTROL_VALIDATOR__UTILS_HPP_ +#ifndef AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_ +#define AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_ -#include +#include #include #include @@ -26,11 +26,11 @@ namespace autoware::control_validator { +using autoware::motion_utils::convertToTrajectory; +using autoware::motion_utils::convertToTrajectoryPointArray; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; -using motion_utils::convertToTrajectory; -using motion_utils::convertToTrajectoryPointArray; using TrajectoryPoints = std::vector; void shiftPose(Pose & pose, double longitudinal); @@ -52,4 +52,4 @@ double calcMaxLateralDistance( const Trajectory & trajectory, const Trajectory & predicted_trajectory); } // namespace autoware::control_validator -#endif // AUTOWARE_CONTROL_VALIDATOR__UTILS_HPP_ +#endif // AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_ diff --git a/control/autoware_control_validator/package.xml b/control/autoware_control_validator/package.xml index 2059308ef9029..71fefafbaffb1 100644 --- a/control/autoware_control_validator/package.xml +++ b/control/autoware_control_validator/package.xml @@ -20,15 +20,15 @@ autoware_cmake rosidl_default_generators + autoware_motion_utils autoware_planning_msgs + autoware_universe_utils autoware_vehicle_info_utils diagnostic_updater geometry_msgs - motion_utils nav_msgs rclcpp rclcpp_components - tier4_autoware_utils visualization_msgs ament_cmake_ros diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 7e4a86d42598e..4c8ca3c831824 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_control_validator/control_validator.hpp" +#include "autoware/control_validator/control_validator.hpp" -#include "autoware_control_validator/utils.hpp" +#include "autoware/control_validator/utils.hpp" #include #include diff --git a/control/autoware_control_validator/src/debug_marker.cpp b/control/autoware_control_validator/src/debug_marker.cpp index 9ef2786c0aef6..d5d8644515ab1 100644 --- a/control/autoware_control_validator/src/debug_marker.cpp +++ b/control/autoware_control_validator/src/debug_marker.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_control_validator/debug_marker.hpp" +#include "autoware/control_validator/debug_marker.hpp" -#include -#include +#include +#include #include #include @@ -48,7 +48,7 @@ void ControlValidatorDebugMarkerPublisher::pushPoseMarker( void ControlValidatorDebugMarkerPublisher::pushPoseMarker( const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) { - using tier4_autoware_utils::createMarkerColor; + using autoware::universe_utils::createMarkerColor; // append arrow marker std_msgs::msg::ColorRGBA color; @@ -64,9 +64,9 @@ void ControlValidatorDebugMarkerPublisher::pushPoseMarker( { color = createMarkerColor(0.0, 0.0, 1.0, 0.999); } - Marker marker = tier4_autoware_utils::createDefaultMarker( + Marker marker = autoware::universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, - tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3), color); + autoware::universe_utils::createMarkerScale(0.2, 0.1, 0.3), color); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; @@ -76,10 +76,10 @@ void ControlValidatorDebugMarkerPublisher::pushPoseMarker( void ControlValidatorDebugMarkerPublisher::pushWarningMsg( const geometry_msgs::msg::Pose & pose, const std::string & msg) { - visualization_msgs::msg::Marker marker = tier4_autoware_utils::createDefaultMarker( + visualization_msgs::msg::Marker marker = autoware::universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); + autoware::universe_utils::createMarkerScale(0.0, 0.0, 1.0), + autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; marker.text = msg; @@ -90,8 +90,8 @@ void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs:: { const auto now = node_->get_clock()->now(); const auto stop_wall_marker = - motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); - tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); + autoware::motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); + autoware::universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } void ControlValidatorDebugMarkerPublisher::publish() diff --git a/control/autoware_control_validator/src/utils.cpp b/control/autoware_control_validator/src/utils.cpp index e7cace049af6e..821d64c3af6e6 100644 --- a/control/autoware_control_validator/src/utils.cpp +++ b/control/autoware_control_validator/src/utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_control_validator/utils.hpp" +#include "autoware/control_validator/utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -37,8 +37,8 @@ void insertPointInPredictedTrajectory( TrajectoryPoints & modified_trajectory, const Pose & reference_pose, const TrajectoryPoints & predicted_trajectory) { - const auto point_to_interpolate = - motion_utils::calcInterpolatedPoint(convertToTrajectory(predicted_trajectory), reference_pose); + const auto point_to_interpolate = autoware::motion_utils::calcInterpolatedPoint( + convertToTrajectory(predicted_trajectory), reference_pose); modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate); } @@ -59,8 +59,8 @@ bool removeFrontTrajectoryPoint( bool predicted_trajectory_point_removed = false; for (const auto & point : predicted_trajectory_points) { if ( - motion_utils::calcLongitudinalOffsetToSegment(trajectory_points, 0, point.pose.position) < - 0.0) { + autoware::motion_utils::calcLongitudinalOffsetToSegment( + trajectory_points, 0, point.pose.position) < 0.0) { modified_trajectory_points.erase(modified_trajectory_points.begin()); predicted_trajectory_point_removed = true; @@ -75,7 +75,7 @@ bool removeFrontTrajectoryPoint( Trajectory alignTrajectoryWithReferenceTrajectory( const Trajectory & trajectory, const Trajectory & predicted_trajectory) { - const auto last_seg_length = motion_utils::calcSignedArcLength( + const auto last_seg_length = autoware::motion_utils::calcSignedArcLength( trajectory.points, trajectory.points.size() - 2, trajectory.points.size() - 1); // If no overlapping between trajectory and predicted_trajectory, return empty trajectory @@ -85,9 +85,9 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // predicted_trajectory: p1------------------pN // trajectory: t1------------------tN const bool & is_p_n_before_t1 = - motion_utils::calcLongitudinalOffsetToSegment( + autoware::motion_utils::calcLongitudinalOffsetToSegment( trajectory.points, 0, predicted_trajectory.points.back().pose.position) < 0.0; - const bool & is_p1_behind_t_n = motion_utils::calcLongitudinalOffsetToSegment( + const bool & is_p1_behind_t_n = autoware::motion_utils::calcLongitudinalOffsetToSegment( trajectory.points, trajectory.points.size() - 2, predicted_trajectory.points.front().pose.position) - last_seg_length > @@ -149,12 +149,12 @@ double calcMaxLateralDistance( alignTrajectoryWithReferenceTrajectory(reference_trajectory, predicted_trajectory); double max_dist = 0; for (const auto & point : alined_predicted_trajectory.points) { - const auto p0 = tier4_autoware_utils::getPoint(point); + const auto p0 = autoware::universe_utils::getPoint(point); // find nearest segment const size_t nearest_segment_idx = - motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); - const double temp_dist = std::abs( - motion_utils::calcLateralOffset(reference_trajectory.points, p0, nearest_segment_idx)); + autoware::motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); + const double temp_dist = std::abs(autoware::motion_utils::calcLateralOffset( + reference_trajectory.points, p0, nearest_segment_idx)); if (temp_dist > max_dist) { max_dist = temp_dist; } diff --git a/control/autoware_external_cmd_selector/include/autoware_external_cmd_selector/external_cmd_selector_node.hpp b/control/autoware_external_cmd_selector/include/autoware/external_cmd_selector/external_cmd_selector_node.hpp similarity index 95% rename from control/autoware_external_cmd_selector/include/autoware_external_cmd_selector/external_cmd_selector_node.hpp rename to control/autoware_external_cmd_selector/include/autoware/external_cmd_selector/external_cmd_selector_node.hpp index 1517ca83ad1f0..464ca0be8c163 100644 --- a/control/autoware_external_cmd_selector/include/autoware_external_cmd_selector/external_cmd_selector_node.hpp +++ b/control/autoware_external_cmd_selector/include/autoware/external_cmd_selector/external_cmd_selector_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ -#define AUTOWARE_EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ +#ifndef AUTOWARE__EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ +#define AUTOWARE__EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ #include #include @@ -103,4 +103,4 @@ class ExternalCmdSelector : public rclcpp::Node diagnostic_updater::Updater updater_{this}; }; } // namespace autoware::external_cmd_selector -#endif // AUTOWARE_EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ +#endif // AUTOWARE__EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ diff --git a/control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp b/control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp index 59a8caa5c1568..4156f54025d35 100644 --- a/control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp +++ b/control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_external_cmd_selector/external_cmd_selector_node.hpp" +#include "autoware/external_cmd_selector/external_cmd_selector_node.hpp" #include diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_controller.hpp b/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp similarity index 89% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_controller.hpp rename to control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp index 88371455def62..fd2d89f446dbb 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONTROLLER_HPP_ -#define AUTOWARE_JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#ifndef AUTOWARE__JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#define AUTOWARE__JOY_CONTROLLER__JOY_CONTROLLER_HPP_ -#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" +#include #include -#include #include #include @@ -70,9 +70,9 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr callback_group_services_; // Subscriber - tier4_autoware_utils::InterProcessPollingSubscriber sub_joy_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_joy_{ this, "input/joy"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ this, "input/odometry"}; rclcpp::Time last_joy_received_time_; @@ -122,4 +122,4 @@ class AutowareJoyControllerNode : public rclcpp::Node }; } // namespace autoware::joy_controller -#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONTROLLER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/ds4_joy_converter.hpp similarity index 92% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/ds4_joy_converter.hpp index 7cfac1af8662b..26878e1bd73cb 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/ds4_joy_converter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ -#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#define AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ -#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" #include @@ -92,4 +92,4 @@ class DS4JoyConverter : public JoyConverterBase }; } // namespace autoware::joy_controller -#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/g29_joy_converter.hpp similarity index 91% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/g29_joy_converter.hpp index a36ad3a580287..ea253e86305b7 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/g29_joy_converter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ -#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#define AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ -#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" namespace autoware::joy_controller { @@ -90,4 +90,4 @@ class G29JoyConverter : public JoyConverterBase }; } // namespace autoware::joy_controller -#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/joy_converter_base.hpp similarity index 86% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp rename to control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/joy_converter_base.hpp index 98a8b799d71a9..fa6081721511c 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/joy_converter_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ -#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#ifndef AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#define AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ #include @@ -52,4 +52,4 @@ class JoyConverterBase }; } // namespace autoware::joy_controller -#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/p65_joy_converter.hpp b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/p65_joy_converter.hpp similarity index 90% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/p65_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/p65_joy_converter.hpp index a6f51cb95b44d..aebe38433e219 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/p65_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/p65_joy_converter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ -#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#define AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ -#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" #include @@ -78,4 +78,4 @@ class P65JoyConverter : public JoyConverterBase }; } // namespace autoware::joy_controller -#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/xbox_joy_converter.hpp b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/xbox_joy_converter.hpp similarity index 90% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/xbox_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/xbox_joy_converter.hpp index 776b0c98b7835..9d0c53b3e42cf 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/xbox_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/xbox_joy_converter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ -#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#define AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ -#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" #include @@ -78,4 +78,4 @@ class XBOXJoyConverter : public JoyConverterBase }; } // namespace autoware::joy_controller -#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index 10e33d5d6564a..3a9acf07a35bf 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -17,6 +17,7 @@ autoware_cmake autoware_control_msgs + autoware_universe_utils autoware_vehicle_msgs geometry_msgs joy @@ -26,7 +27,6 @@ sensor_msgs std_srvs tier4_api_utils - tier4_autoware_utils tier4_control_msgs tier4_external_api_msgs diff --git a/control/autoware_joy_controller/src/joy_controller_node.cpp b/control/autoware_joy_controller/src/joy_controller_node.cpp index 2491893a2806c..cab15c93d753f 100644 --- a/control/autoware_joy_controller/src/joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/joy_controller_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_joy_controller/joy_controller.hpp" -#include "autoware_joy_controller/joy_converter/ds4_joy_converter.hpp" -#include "autoware_joy_controller/joy_converter/g29_joy_converter.hpp" -#include "autoware_joy_controller/joy_converter/p65_joy_converter.hpp" -#include "autoware_joy_controller/joy_converter/xbox_joy_converter.hpp" +#include "autoware/joy_controller/joy_controller.hpp" +#include "autoware/joy_controller/joy_converter/ds4_joy_converter.hpp" +#include "autoware/joy_controller/joy_converter/g29_joy_converter.hpp" +#include "autoware/joy_controller/joy_converter/p65_joy_converter.hpp" +#include "autoware/joy_controller/joy_converter/xbox_joy_converter.hpp" #include @@ -151,6 +151,10 @@ namespace autoware::joy_controller void AutowareJoyControllerNode::onJoy() { const auto msg = sub_joy_.takeData(); + if (!msg) { + return; + } + last_joy_received_time_ = msg->header.stamp; if (joy_type_ == "G29") { joy_ = std::make_shared(*msg); @@ -198,6 +202,10 @@ void AutowareJoyControllerNode::onOdometry() } const auto msg = sub_odom_.takeData(); + if (!msg) { + return; + } + auto twist = std::make_shared(); twist->header = msg->header; twist->twist = msg->twist.twist; diff --git a/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp similarity index 91% rename from control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp rename to control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index e29623bf69891..b43a7d0d0369a 100644 --- a/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ -#define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#define AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#include +#include #include #include -#include -#include #include #include @@ -47,12 +47,12 @@ namespace autoware::lane_departure_checker { +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::PoseDeviation; +using autoware::universe_utils::Segment2d; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::PoseDeviation; -using tier4_autoware_utils::Segment2d; using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; typedef boost::geometry::index::rtree> SegmentRtree; @@ -122,7 +122,7 @@ class LaneDepartureChecker std::vector> getLaneletsFromPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; - std::optional getFusedLaneletPolygonForPath( + std::optional getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; bool checkPathWillLeaveLane( @@ -172,4 +172,4 @@ class LaneDepartureChecker }; } // namespace autoware::lane_departure_checker -#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#endif // AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ diff --git a/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp similarity index 81% rename from control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp rename to control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index d37ef30d6b8e8..e6421b2af84bb 100644 --- a/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ -#define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#define AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ -#include "autoware_lane_departure_checker/lane_departure_checker.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include "autoware/lane_departure_checker/lane_departure_checker.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include +#include #include #include -#include -#include #include #include @@ -67,15 +67,15 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber - tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_lanelet_map_bin_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_lanelet_map_bin_{ this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_route_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_route_{ this, "~/input/route"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ this, "~/input/reference_trajectory"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ this, "~/input/predicted_trajectory"}; // Data Buffer @@ -99,8 +99,8 @@ class LaneDepartureCheckerNode : public rclcpp::Node void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); // Publisher - tier4_autoware_utils::DebugPublisher debug_publisher_{this, "~/debug"}; - tier4_autoware_utils::ProcessingTimePublisher processing_time_publisher_{this}; + autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; + autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this}; // Timer rclcpp::TimerBase::SharedPtr timer_; @@ -157,4 +157,4 @@ class LaneDepartureCheckerNode : public rclcpp::Node }; } // namespace autoware::lane_departure_checker -#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#endif // AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ diff --git a/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp similarity index 89% rename from control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp rename to control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp index c85e37212b16f..4435f282054ad 100644 --- a/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp @@ -25,12 +25,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ -#define AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#define AUTOWARE__LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ #include +#include #include -#include #include @@ -63,4 +63,4 @@ inline FootprintMargin calcFootprintMargin( return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale}; } -#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#endif // AUTOWARE__LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/control/autoware_lane_departure_checker/package.xml b/control/autoware_lane_departure_checker/package.xml index 7a08cd2907120..2a08db7288568 100644 --- a/control/autoware_lane_departure_checker/package.xml +++ b/control/autoware_lane_departure_checker/package.xml @@ -14,14 +14,15 @@ autoware_cmake autoware_map_msgs + autoware_motion_utils autoware_planning_msgs + autoware_universe_utils autoware_vehicle_info_utils boost diagnostic_updater eigen geometry_msgs lanelet2_extension - motion_utils nav_msgs rclcpp rclcpp_components @@ -29,7 +30,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_lint_auto diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 5e90df6773b87..132eba8dde0c9 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_lane_departure_checker/lane_departure_checker.hpp" +#include "autoware/lane_departure_checker/lane_departure_checker.hpp" -#include "autoware_lane_departure_checker/util/create_vehicle_footprint.hpp" +#include "autoware/lane_departure_checker/util/create_vehicle_footprint.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include @@ -30,12 +30,12 @@ #include #include -using motion_utils::calcArcLength; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::MultiPoint2d; -using tier4_autoware_utils::MultiPolygon2d; -using tier4_autoware_utils::Point2d; +using autoware::motion_utils::calcArcLength; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPolygon2d; +using autoware::universe_utils::Point2d; namespace { @@ -102,7 +102,7 @@ Output LaneDepartureChecker::update(const Input & input) { Output output{}; - tier4_autoware_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; output.trajectory_deviation = calcTrajectoryDeviation( *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold, @@ -169,9 +169,9 @@ PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold) { - const auto nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, pose, dist_threshold, yaw_threshold); - return tier4_autoware_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); + return autoware::universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); } TrajectoryPoints LaneDepartureChecker::resampleTrajectory( @@ -183,8 +183,8 @@ TrajectoryPoints LaneDepartureChecker::resampleTrajectory( for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = tier4_autoware_utils::fromMsg(resampled.back().pose.position); - const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); + const auto p1 = autoware::universe_utils::fromMsg(resampled.back().pose.position); + const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); if (boost::geometry::distance(p1.to_2d(), p2.to_2d()) > interval) { resampled.push_back(point); @@ -205,8 +205,8 @@ TrajectoryPoints LaneDepartureChecker::cutTrajectory( for (size_t i = 1; i < trajectory.size(); ++i) { const auto & point = trajectory.at(i); - const auto p1 = tier4_autoware_utils::fromMsg(cut.back().pose.position); - const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); + const auto p1 = autoware::universe_utils::fromMsg(cut.back().pose.position); + const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -251,7 +251,7 @@ std::vector LaneDepartureChecker::createVehicleFootprints( std::vector vehicle_footprints; for (const auto & p : trajectory) { vehicle_footprints.push_back( - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(p.pose))); + transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(p.pose))); } return vehicle_footprints; @@ -267,8 +267,8 @@ std::vector LaneDepartureChecker::createVehicleFootprints( // Create vehicle footprint on each Path point std::vector vehicle_footprints; for (const auto & p : path.points) { - vehicle_footprints.push_back( - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(p.point.pose))); + vehicle_footprints.push_back(transformVector( + local_vehicle_footprint, autoware::universe_utils::pose2transform(p.point.pose))); } return vehicle_footprints; @@ -322,16 +322,18 @@ std::vector> LaneDepartureChecker::getLanele lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0); } -std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( +std::optional +LaneDepartureChecker::getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); - auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d { - tier4_autoware_utils::Polygon2d p; + auto to_polygon2d = + [](const lanelet::BasicPolygon2d & poly) -> autoware::universe_utils::Polygon2d { + autoware::universe_utils::Polygon2d p; auto & outer = p.outer(); for (const auto & p : poly) { - tier4_autoware_utils::Point2d p2d(p.x(), p.y()); + autoware::universe_utils::Point2d p2d(p.x(), p.y()); outer.push_back(p2d); } boost::geometry::correct(p); @@ -339,15 +341,15 @@ std::optional LaneDepartureChecker::getFusedLan }; // Fuse lanelets into a single BasicPolygon2d - auto fused_lanelets = [&]() -> std::optional { + auto fused_lanelets = [&]() -> std::optional { if (lanelets_distance_pair.empty()) return std::nullopt; - tier4_autoware_utils::MultiPolygon2d lanelet_unions; - tier4_autoware_utils::MultiPolygon2d result; + autoware::universe_utils::MultiPolygon2d lanelet_unions; + autoware::universe_utils::MultiPolygon2d result; for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) { const auto & route_lanelet = lanelets_distance_pair.at(i).second; const auto & p = route_lanelet.polygon2d().basicPolygon(); - tier4_autoware_utils::Polygon2d poly = to_polygon2d(p); + autoware::universe_utils::Polygon2d poly = to_polygon2d(p); boost::geometry::union_(lanelet_unions, poly, result); lanelet_unions = result; result.clear(); diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index f95ae0f4ac2ed..427cf0898470e 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_lane_departure_checker/lane_departure_checker_node.hpp" +#include "autoware/lane_departure_checker/lane_departure_checker_node.hpp" +#include +#include +#include #include #include #include #include #include -#include -#include -#include #include @@ -31,7 +31,7 @@ #include #include -using tier4_autoware_utils::rad2deg; +using autoware::universe_utils::rad2deg; namespace { @@ -251,7 +251,7 @@ bool LaneDepartureCheckerNode::isDataValid() void LaneDepartureCheckerNode::onTimer() { std::map processing_time_map; - tier4_autoware_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; stop_watch.tic("Total"); current_odom_ = sub_odom_.takeData(); @@ -453,9 +453,9 @@ void LaneDepartureCheckerNode::checkTrajectoryDeviation( visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray() const { - using tier4_autoware_utils::createDefaultMarker; - using tier4_autoware_utils::createMarkerColor; - using tier4_autoware_utils::createMarkerScale; + using autoware::universe_utils::createDefaultMarker; + using autoware::universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray marker_array; diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/lowpass_filter.hpp similarity index 95% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/lowpass_filter.hpp index 73cef7cddda39..8a32d06c8066e 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/lowpass_filter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ #include #include @@ -102,4 +102,4 @@ namespace MoveAverageFilter bool filt_vector(const int num, std::vector & u); } // namespace MoveAverageFilter } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp similarity index 97% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 03abae66e4986..058eb45bfaaff 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ - -#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" -#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" -#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" -#include "autoware_mpc_lateral_controller/steering_predictor.hpp" -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_HPP_ + +#include "autoware/mpc_lateral_controller/lowpass_filter.hpp" +#include "autoware/mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware/mpc_lateral_controller/steering_predictor.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/lateral.hpp" @@ -525,4 +525,4 @@ class MPC }; // class MPC } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp similarity index 90% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp index 50ab923f5b4d6..09399d1fa2dce 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -12,16 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ - -#include "autoware_mpc_lateral_controller/mpc.hpp" -#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" -#include "autoware_mpc_lateral_controller/mpc_utils.hpp" -#include "autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp" -#include "autoware_trajectory_follower_base/lateral_controller_base.hpp" +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ + +#include "autoware/mpc_lateral_controller/mpc.hpp" +#include "autoware/mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware/mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/mpc_lateral_controller/steering_offset/steering_offset.hpp" +#include "autoware/trajectory_follower_base/lateral_controller_base.hpp" #include "rclcpp/rclcpp.hpp" +#include + #include "autoware_control_msgs/msg/lateral.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" @@ -52,7 +54,8 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase { public: /// \param node Reference to the node used only for the component and parameter initialization. - explicit MpcLateralController(rclcpp::Node & node); + explicit MpcLateralController( + rclcpp::Node & node, std::shared_ptr diag_updater); virtual ~MpcLateralController(); private: @@ -63,6 +66,9 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase rclcpp::Publisher::SharedPtr m_pub_debug_values; rclcpp::Publisher::SharedPtr m_pub_steer_offset; + std::shared_ptr + diag_updater_{}; // Diagnostic updater for publishing diagnostic data. + //!< @brief parameters for path smoothing TrajectoryFilteringParam m_trajectory_filtering_param; @@ -87,9 +93,16 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // Flag indicating whether to keep the steering control until it converges. bool m_keep_steer_control_until_converged; + // MPC solver checker. + bool m_is_mpc_solved{true}; + // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; + void setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + + void setupDiag(); + std::unique_ptr m_mpc; // MPC object for trajectory following. // Check is mpc output converged @@ -281,4 +294,4 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp similarity index 91% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp index 4a0ae16bc9b1c..b761aa9f19d23 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "geometry_msgs/msg/point.hpp" @@ -117,7 +117,7 @@ class MPCTrajectory point.pose.position.x = x.at(i); point.pose.position.y = y.at(i); point.pose.position.z = z.at(i); - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw.at(i)); + point.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw.at(i)); point.longitudinal_velocity_mps = vx.at(i); points.push_back(point); } @@ -125,4 +125,4 @@ class MPCTrajectory } }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_utils.hpp similarity index 97% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_utils.hpp index e97a004438e4b..87f6c0064a2b8 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -26,7 +26,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware/mpc_lateral_controller/mpc_trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -230,4 +230,4 @@ void update_param( } // namespace MPCUtils } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp similarity index 89% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index 6447437d6c274..9756d547c97ff 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ #include @@ -51,4 +51,4 @@ class QPSolverInterface virtual double getObjVal() const { return 0.0; } }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp similarity index 89% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 8c81092960097..1e1295cb06b1e 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" @@ -62,4 +62,4 @@ class QPSolverOSQP : public QPSolverInterface rclcpp::Logger logger_; }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp similarity index 87% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp index 15c8137b78890..79d7d25aeff91 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ -#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" namespace autoware::motion::control::mpc_lateral_controller { @@ -62,4 +62,4 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface private: }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/steering_offset/steering_offset.hpp similarity index 84% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/steering_offset/steering_offset.hpp index 707c1ce56a9a6..f5e357ddc717b 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/steering_offset/steering_offset.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ #include @@ -45,4 +45,4 @@ class SteeringOffsetEstimator double steering_offset_ = 0.0; }; -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/steering_predictor.hpp similarity index 92% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/steering_predictor.hpp index 0c5f04497d058..474c98bfa13b2 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/steering_predictor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ #include "rclcpp/rclcpp.hpp" @@ -81,4 +81,4 @@ class SteeringPredictor } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp similarity index 93% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 20787b1f281b5..654b7665f942a 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -44,10 +44,10 @@ * Tracking", Robotics Institute, Carnegie Mellon University, February 2009. */ -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -122,4 +122,4 @@ class DynamicsBicycleModel : public VehicleModelInterface double m_cr; //!< @brief rear cornering power [N/rad] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp similarity index 91% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index 91d3ea826c7d3..d638142daa844 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -38,10 +38,10 @@ * */ -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -106,4 +106,4 @@ class KinematicsBicycleModel : public VehicleModelInterface double m_steer_tau; //!< @brief steering time constant for 1d-model [s] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp similarity index 89% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index 5731a2025d833..83d0f8f9c1ae1 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -35,10 +35,10 @@ * */ -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT -#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT +#define AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -102,5 +102,5 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface }; } // namespace autoware::motion::control::mpc_lateral_controller // clang-format off -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT // clang-format on diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp similarity index 93% rename from control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index 9e2d14184d5e7..1b3db8fb3f80f 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware/mpc_lateral_controller/mpc_trajectory.hpp" #include @@ -149,4 +149,4 @@ class VehicleModelInterface const MPCTrajectory & reference_trajectory, const double dt) const = 0; }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index 1e250dd89ce3f..e25352797a0d0 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -18,8 +18,10 @@ autoware_cmake autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs @@ -27,14 +29,12 @@ eigen geometry_msgs interpolation - motion_utils osqp_interface rclcpp rclcpp_components std_msgs tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_ros diff --git a/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp index e3bbe78e2428b..bd90b2aa896f6 100644 --- a/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" +#include "autoware/mpc_lateral_controller/lowpass_filter.hpp" #include diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index e6fa2ce3c0102..b6ddba60490d6 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/mpc.hpp" +#include "autoware/mpc_lateral_controller/mpc.hpp" -#include "autoware_mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "interpolation/linear_interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include namespace autoware::motion::control::mpc_lateral_controller { -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::normalizeRadian; -using tier4_autoware_utils::rad2deg; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::normalizeRadian; +using autoware::universe_utils::rad2deg; MPC::MPC(rclcpp::Node & node) { @@ -166,10 +166,11 @@ void MPC::setReferenceTrajectory( const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param, const Odometry & current_kinematics) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + const size_t nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + const double ego_offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position); const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg); @@ -183,7 +184,7 @@ void MPC::setReferenceTrajectory( } const auto is_forward_shift = - motion_utils::isDrivingForward(mpc_traj_resampled.toTrajectoryPoints()); + autoware::motion_utils::isDrivingForward(mpc_traj_resampled.toTrajectoryPoints()); // if driving direction is unknown, use previous value m_is_forward_shift = is_forward_shift ? is_forward_shift.value() : m_is_forward_shift; @@ -243,7 +244,7 @@ void MPC::setReferenceTrajectory( void MPC::resetPrevResult(const SteeringReport & current_steer) { - // Consider limit. The prev value larger than limitation brakes the optimization constraint and + // Consider limit. The prev value larger than limitation breaks the optimization constraint and // results in optimization failure. const float steer_lim_f = static_cast(m_steer_lim); m_raw_steer_cmd_prev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f); @@ -389,9 +390,10 @@ MPCTrajectory MPC::applyVelocityDynamicsFilter( return input; } - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); + const size_t nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); MPCTrajectory output = input; MPCUtils::dynamicSmoothingVelocity( @@ -499,7 +501,7 @@ MPCMatrix MPC::generateMPCMatrix( // get reference input (feed-forward) m_vehicle_model_ptr->setCurvature(ref_smooth_k); m_vehicle_model_ptr->calculateReferenceInput(Uref); - if (std::fabs(Uref(0, 0)) < tier4_autoware_utils::deg2rad(m_param.zero_ff_steer_deg)) { + if (std::fabs(Uref(0, 0)) < autoware::universe_utils::deg2rad(m_param.zero_ff_steer_deg)) { Uref(0, 0) = 0.0; // ignore curvature noise } m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref; @@ -676,7 +678,7 @@ double MPC::getPredictionDeltaTime( { // Calculate the time min_prediction_length ahead from current_pose const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input); - const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); double sum_dist = 0; diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index cf8a4ed5a251b..43acbb862b573 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/mpc_lateral_controller.hpp" - -#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "autoware/mpc_lateral_controller/mpc_lateral_controller.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" #include "tf2_ros/create_timer_ros.h" @@ -35,13 +35,16 @@ namespace autoware::motion::control::mpc_lateral_controller { -MpcLateralController::MpcLateralController(rclcpp::Node & node) +MpcLateralController::MpcLateralController( + rclcpp::Node & node, std::shared_ptr diag_updater) : clock_(node.get_clock()), logger_(node.get_logger().get_child("lateral_controller")) { const auto dp_int = [&](const std::string & s) { return node.declare_parameter(s); }; const auto dp_bool = [&](const std::string & s) { return node.declare_parameter(s); }; const auto dp_double = [&](const std::string & s) { return node.declare_parameter(s); }; + diag_updater_ = diag_updater; + m_mpc = std::make_unique(node); m_mpc->m_ctrl_period = node.get_parameter("ctrl_period").as_double(); @@ -152,6 +155,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) m_mpc->setLogger(logger_); m_mpc->setClock(clock_); + + setupDiag(); } MpcLateralController::~MpcLateralController() @@ -227,6 +232,24 @@ std::shared_ptr MpcLateralController::createSteerOffset return steering_offset_; } +void MpcLateralController::setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (m_is_mpc_solved) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "MPC succeeded."); + } else { + const std::string error_msg = "The MPC solver failed. Call MRM to stop the car."; + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, error_msg); + } +} + +void MpcLateralController::setupDiag() +{ + auto & d = diag_updater_; + d->setHardwareID("mpc_lateral_controller"); + + d->add("MPC_solve_checker", [&](auto & stat) { setStatus(stat); }); +} + trajectory_follower::LateralOutput MpcLateralController::run( trajectory_follower::InputData const & input_data) { @@ -255,12 +278,16 @@ trajectory_follower::LateralOutput MpcLateralController::run( const bool is_mpc_solved = m_mpc->calculateMPC( m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); + m_is_mpc_solved = is_mpc_solved; // for diagnostic updater + + diag_updater_->force_update(); + // reset previous MPC result // Note: When a large deviation from the trajectory occurs, the optimization stops and // the vehicle will return to the path by re-planning the trajectory or external operation. // After the recovery, the previous value of the optimization may deviate greatly from // the actual steer angle, and it may make the optimization result unstable. - if (!is_mpc_solved) { + if (!is_mpc_solved || !is_under_control) { m_mpc->resetPrevResult(m_current_steering); } else { setSteeringToHistory(ctrl_cmd); @@ -408,7 +435,7 @@ bool MpcLateralController::isStoppedState() const // for the stop state judgement. However, it has been removed since the steering // control was turned off when approaching/exceeding the stop line on a curve or // emergency stop situation and it caused large tracking error. - const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t nearest = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( m_current_trajectory.points, m_current_kinematic_state.pose.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); @@ -497,7 +524,7 @@ bool MpcLateralController::isMpcConverged() // Find the maximum and minimum values of the steering angle in the past 1 second. double min_steering_value = m_mpc_steering_history[0].first.steering_tire_angle; - double max_steering_value = m_mpc_steering_history[0].first.steering_tire_angle; + double max_steering_value = min_steering_value; for (size_t i = 1; i < m_mpc_steering_history.size(); i++) { if (m_mpc_steering_history.at(i).first.steering_tire_angle < min_steering_value) { min_steering_value = m_mpc_steering_history.at(i).first.steering_tire_angle; @@ -616,7 +643,7 @@ bool MpcLateralController::isTrajectoryShapeChanged() const // TODO(Horibe): update implementation to check trajectory shape around ego vehicle. // Now temporally check the goal position. for (const auto & trajectory : m_trajectory_buffer) { - const auto change_distance = tier4_autoware_utils::calcDistance2d( + const auto change_distance = autoware::universe_utils::calcDistance2d( trajectory.points.back().pose, m_current_trajectory.points.back().pose); if (change_distance > m_new_traj_end_dist) { return true; @@ -633,8 +660,8 @@ bool MpcLateralController::isValidTrajectory(const Trajectory & traj) const !isfinite(p.pose.orientation.w) || !isfinite(p.pose.orientation.x) || !isfinite(p.pose.orientation.y) || !isfinite(p.pose.orientation.z) || !isfinite(p.longitudinal_velocity_mps) || !isfinite(p.lateral_velocity_mps) || - !isfinite(p.lateral_velocity_mps) || !isfinite(p.heading_rate_rps) || - !isfinite(p.front_wheel_angle_rad) || !isfinite(p.rear_wheel_angle_rad)) { + !isfinite(p.heading_rate_rps) || !isfinite(p.front_wheel_angle_rad) || + !isfinite(p.rear_wheel_angle_rad)) { return false; } } diff --git a/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp b/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp index e84e6cf79a25a..842689527c847 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware/mpc_lateral_controller/mpc_trajectory.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index 1a0062372eb22..37eb47ab0396e 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" #include #include @@ -42,9 +42,9 @@ double calcLongitudinalOffset( namespace MPCUtils { -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::createQuaternionFromYaw; -using tier4_autoware_utils::normalizeRadian; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::createQuaternionFromYaw; +using autoware::universe_utils::normalizeRadian; double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2) { @@ -240,7 +240,7 @@ std::vector calcTrajectoryCurvature( p2.y = traj.y.at(curr_idx); p3.y = traj.y.at(next_idx); try { - curvature_vec.at(curr_idx) = tier4_autoware_utils::calcCurvature(p1, p2, p3); + curvature_vec.at(curr_idx) = autoware::universe_utils::calcCurvature(p1, p2, p3); } catch (...) { std::cerr << "[MPC] 2 points are too close to calculate curvature." << std::endl; curvature_vec.at(curr_idx) = 0.0; @@ -281,7 +281,7 @@ Trajectory convertToAutowareTrajectory(const MPCTrajectory & input) p.pose.position.x = input.x.at(i); p.pose.position.y = input.y.at(i); p.pose.position.z = input.z.at(i); - p.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(input.yaw.at(i)); + p.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(input.yaw.at(i)); p.longitudinal_velocity_mps = static_cast(input.vx.at(i)); output.points.push_back(p); @@ -346,7 +346,7 @@ bool calcNearestPoseInterp( return false; } - *nearest_index = motion_utils::findFirstNearestIndexWithSoftConstraints( + *nearest_index = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( autoware_traj.points, self_pose, max_dist, max_yaw); const size_t traj_size = traj.size(); @@ -389,7 +389,7 @@ bool calcNearestPoseInterp( prev_traj_point.x = traj.x.at(prev); prev_traj_point.y = traj.y.at(prev); const double traj_seg_length = - tier4_autoware_utils::calcDistance2d(prev_traj_point, next_traj_point); + autoware::universe_utils::calcDistance2d(prev_traj_point, next_traj_point); /* if distance between two points are too close */ if (traj_seg_length < 1.0E-5) { nearest_pose->position.x = traj.x.at(*nearest_index); @@ -460,7 +460,7 @@ void extendTrajectoryInYawDirection( const double dt = interval / extend_vel; const size_t num_extended_point = static_cast(extend_dist / interval); for (size_t i = 0; i < num_extended_point; ++i) { - extended_pose = tier4_autoware_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0); + extended_pose = autoware::universe_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0); traj.push_back( extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(), extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt); diff --git a/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index 66981b0c5c6d4..66a7dcacbeae1 100644 --- a/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include #include diff --git a/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp index 315e29d063aad..5c69c176a1858 100644 --- a/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp +++ b/control/autoware_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 "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include diff --git a/control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp b/control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp index 6425d5da5e941..63c0c097c2ca1 100644 --- a/control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp +++ b/control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp" +#include "autoware/mpc_lateral_controller/steering_offset/steering_offset.hpp" #include #include diff --git a/control/autoware_mpc_lateral_controller/src/steering_predictor.cpp b/control/autoware_mpc_lateral_controller/src/steering_predictor.cpp index 759a36a16de62..7683d2f8153ff 100644 --- a/control/autoware_mpc_lateral_controller/src/steering_predictor.cpp +++ b/control/autoware_mpc_lateral_controller/src/steering_predictor.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/steering_predictor.hpp" +#include "autoware/mpc_lateral_controller/steering_predictor.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index d5272c48fcbdf..b5fa3ef1ea275 100644 --- a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include diff --git a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 97bcafb161cb0..32d97a783627c 100644 --- a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include diff --git a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index d2857d8b42182..824dc25220482 100644 --- a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include diff --git a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index e1b17d3d96278..78cf8ba63b59b 100644 --- a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp index bfb695cdeee8a..a642976efc497 100644 --- a/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" +#include "autoware/mpc_lateral_controller/lowpass_filter.hpp" #include "gtest/gtest.h" #include diff --git a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp index 3fd4b010852c0..c4a67552a6083 100644 --- a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/mpc.hpp" -#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "autoware/mpc_lateral_controller/mpc.hpp" +#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" diff --git a/control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp index b55435dffd024..2f6b93139cbd2 100644 --- a/control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" -#include "autoware_mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware/mpc_lateral_controller/mpc_utils.hpp" #include "gtest/gtest.h" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/control/autoware_operation_mode_transition_manager/package.xml b/control/autoware_operation_mode_transition_manager/package.xml index c93b38c537a73..de514e70a0f2e 100644 --- a/control/autoware_operation_mode_transition_manager/package.xml +++ b/control/autoware_operation_mode_transition_manager/package.xml @@ -13,16 +13,16 @@ rosidl_default_generators autoware_control_msgs + autoware_motion_utils + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs component_interface_specs component_interface_utils geometry_msgs - motion_utils rclcpp rclcpp_components std_srvs - tier4_autoware_utils tier4_control_msgs tier4_system_msgs tier4_vehicle_msgs diff --git a/control/autoware_operation_mode_transition_manager/src/node.hpp b/control/autoware_operation_mode_transition_manager/src/node.hpp index 0cacb083fc293..d69d79f9640d8 100644 --- a/control/autoware_operation_mode_transition_manager/src/node.hpp +++ b/control/autoware_operation_mode_transition_manager/src/node.hpp @@ -18,10 +18,10 @@ #include "compatibility.hpp" #include "state.hpp" +#include #include #include #include -#include #include #include @@ -49,10 +49,10 @@ class OperationModeTransitionManager : public rclcpp::Node const ChangeOperationModeAPI::Service::Response::SharedPtr response); using ControlModeCommandType = ControlModeCommand::Request::_mode_type; - tier4_autoware_utils::InterProcessPollingSubscriber sub_control_mode_report_{ - this, "control_mode_report"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_gate_operation_mode_{ - this, "gate_operation_mode"}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_control_mode_report_{this, "control_mode_report"}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_gate_operation_mode_{this, "gate_operation_mode"}; rclcpp::Client::SharedPtr cli_control_mode_; rclcpp::Publisher::SharedPtr pub_debug_info_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_operation_mode_transition_manager/src/state.cpp b/control/autoware_operation_mode_transition_manager/src/state.cpp index 4eaf0ddf8e764..31cd32e6311a9 100644 --- a/control/autoware_operation_mode_transition_manager/src/state.cpp +++ b/control/autoware_operation_mode_transition_manager/src/state.cpp @@ -16,9 +16,9 @@ #include "util.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -26,9 +26,9 @@ namespace autoware::operation_mode_transition_manager { -using motion_utils::findNearestIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcYawDeviation; +using autoware::motion_utils::findNearestIndex; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcYawDeviation; AutonomousMode::AutonomousMode(rclcpp::Node * node) : logger_(node->get_logger()), clock_(node->get_clock()) @@ -126,7 +126,7 @@ bool AutonomousMode::isModeChangeCompleted() // check for lateral deviation const auto dist_deviation = - motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position); + autoware::motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position); if (std::isnan(dist_deviation)) { RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); return unstable(); @@ -138,7 +138,7 @@ bool AutonomousMode::isModeChangeCompleted() // check for yaw deviation const auto yaw_deviation = - motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose); + autoware::motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose); if (std::isnan(yaw_deviation)) { RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); return unstable(); diff --git a/control/autoware_pid_longitudinal_controller/README.md b/control/autoware_pid_longitudinal_controller/README.md index 9929d955e5ba7..b147b3b795391 100644 --- a/control/autoware_pid_longitudinal_controller/README.md +++ b/control/autoware_pid_longitudinal_controller/README.md @@ -212,8 +212,8 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | max_d_effort | double | max value of acceleration with d gain | 0.0 | | min_d_effort | double | min value of acceleration with d gain | 0.0 | | lpf_vel_error_gain | double | gain of low-pass filter for velocity error | 0.9 | -| enable_integration_at_low_speed | bool | Whether to enable integration of acceleration errors when the vehicle speed is lower than `current_vel_threshold_pid_integration` or not. | -| current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s] | +| enable_integration_at_low_speed | bool | Whether to enable integration of acceleration errors when the vehicle speed is lower than `current_vel_threshold_pid_integration` or not. | false | +| current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s] | 0.5 | | time_threshold_before_pid_integration | double | How much time without the vehicle moving must past to enable PID error integration. [s] | 5.0 | | brake_keeping_acc | double | If `enable_brake_keeping_before_stop` is true, a certain acceleration is kept during DRIVE state before the ego stops [m/s^2] See [Brake keeping](#brake-keeping). | 0.2 | diff --git a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/debug_values.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp similarity index 93% rename from control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/debug_values.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp index bd682f9566134..e8834665de3ec 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/debug_values.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ -#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ #include #include @@ -93,4 +93,4 @@ class DebugValues }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp similarity index 88% rename from control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 171571b493774..a36a0b4eefccd 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ -#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spherical_linear_interpolation.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" #include @@ -87,12 +87,13 @@ std::pair lerpTrajectoryPoint( const T & points, const Pose & pose, const double max_dist, const double max_yaw) { TrajectoryPoint interpolated_point; - const size_t seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(points, pose, max_dist, max_yaw); + const size_t seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, pose, max_dist, max_yaw); const double len_to_interpolated = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); - const double len_segment = motion_utils::calcSignedArcLength(points, seg_idx, seg_idx + 1); + autoware::motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); + const double len_segment = + autoware::motion_utils::calcSignedArcLength(points, seg_idx, seg_idx + 1); const double interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0); { @@ -155,4 +156,4 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/lowpass_filter.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp similarity index 89% rename from control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/lowpass_filter.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp index a743be573cf60..081e78e2de214 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/lowpass_filter.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ -#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ #include #include @@ -63,4 +63,4 @@ class LowpassFilter1d } }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid.hpp similarity index 94% rename from control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid.hpp index 8323e35b5da27..42138ecc1fb8c 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_HPP_ -#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_HPP_ +#ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__PID_HPP_ +#define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__PID_HPP_ #include @@ -91,4 +91,4 @@ class PIDController }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_HPP_ +#endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__PID_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp similarity index 94% rename from control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 98882509aaceb..9291d538b022f 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ -#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ - -#include "autoware_pid_longitudinal_controller/debug_values.hpp" -#include "autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp" -#include "autoware_pid_longitudinal_controller/lowpass_filter.hpp" -#include "autoware_pid_longitudinal_controller/pid.hpp" -#include "autoware_pid_longitudinal_controller/smooth_stop.hpp" -#include "autoware_trajectory_follower_base/longitudinal_controller_base.hpp" +#ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ + +#include "autoware/pid_longitudinal_controller/debug_values.hpp" +#include "autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp" +#include "autoware/pid_longitudinal_controller/lowpass_filter.hpp" +#include "autoware/pid_longitudinal_controller/pid.hpp" +#include "autoware/pid_longitudinal_controller/smooth_stop.hpp" +#include "autoware/trajectory_follower_base/longitudinal_controller_base.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "tier4_autoware_utils/ros/marker_helper.hpp" #include #include @@ -50,10 +50,10 @@ namespace autoware::motion::control::pid_longitudinal_controller { +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; using autoware_adapi_v1_msgs::msg::OperationModeState; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; using visualization_msgs::msg::Marker; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -408,4 +408,4 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/smooth_stop.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp similarity index 95% rename from control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/smooth_stop.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp index e433fd158026b..e84b44d95c886 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/smooth_stop.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ -#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ #include "rclcpp/rclcpp.hpp" @@ -112,4 +112,4 @@ class SmoothStop }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml index a84083eae2725..91d77e454b7ff 100644 --- a/control/autoware_pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -20,8 +20,10 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs @@ -29,13 +31,11 @@ eigen geometry_msgs interpolation - motion_utils rclcpp rclcpp_components std_msgs tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_ros diff --git a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 0d8707afccad8..5eb6c87e063eb 100644 --- a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp" +#include "autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/LinearMath/Quaternion.h" @@ -58,12 +58,12 @@ bool isValidTrajectory(const Trajectory & traj) double calcStopDistance( const Pose & current_pose, const Trajectory & traj, const double max_dist, const double max_yaw) { - const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points); + const auto stop_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(traj.points); const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; - const size_t seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const size_t seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( traj.points, current_pose, max_dist, max_yaw); - const double signed_length_on_traj = motion_utils::calcSignedArcLength( + const double signed_length_on_traj = autoware::motion_utils::calcSignedArcLength( traj.points, current_pose.position, seg_idx, traj.points.at(end_idx).pose.position, std::min(end_idx, traj.points.size() - 2)); @@ -93,7 +93,7 @@ double getPitchByTraj( const auto [prev_idx, next_idx] = [&]() { for (size_t i = start_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = tier4_autoware_utils::calcDistance3d( + const double dist = autoware::universe_utils::calcDistance3d( trajectory.points.at(start_idx), trajectory.points.at(i)); if (dist > wheel_base) { // calculate pitch from trajectory between rear wheel (nearest) and front center (i) @@ -105,7 +105,7 @@ double getPitchByTraj( std::min(start_idx, trajectory.points.size() - 2), trajectory.points.size() - 1); }(); - return tier4_autoware_utils::calcElevationAngle( + return autoware::universe_utils::calcElevationAngle( trajectory.points.at(prev_idx).pose.position, trajectory.points.at(next_idx).pose.position); } @@ -166,7 +166,7 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( double remain_dist = distance; geometry_msgs::msg::Pose p = trajectory.points.back().pose; for (size_t i = src_idx; i < trajectory.points.size() - 1; ++i) { - const double dist = tier4_autoware_utils::calcDistance3d( + const double dist = autoware::universe_utils::calcDistance3d( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose); if (remain_dist < dist) { if (remain_dist <= 0.0) { diff --git a/control/autoware_pid_longitudinal_controller/src/pid.cpp b/control/autoware_pid_longitudinal_controller/src/pid.cpp index 6d2c61f639746..64ffdce8a4243 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_pid_longitudinal_controller/pid.hpp" +#include "autoware/pid_longitudinal_controller/pid.hpp" #include #include diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 4cca525f6ebf3..78c7ccf832514 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp" +#include "autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include #include @@ -465,10 +465,10 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // check if the deviation is worth emergency m_diagnostic_data.trans_deviation = - tier4_autoware_utils::calcDistance2d(current_interpolated_pose.first, current_pose); + autoware::universe_utils::calcDistance2d(current_interpolated_pose.first, current_pose); const bool is_dist_deviation_large = m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation; - m_diagnostic_data.rot_deviation = std::abs(tier4_autoware_utils::normalizeRadian( + m_diagnostic_data.rot_deviation = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(current_interpolated_pose.first.pose.orientation) - tf2::getYaw(current_pose.orientation))); const bool is_yaw_deviation_large = @@ -509,11 +509,11 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // ========================================================================================== // Remove overlapped points after inserting the interpolated points control_data.interpolated_traj.points = - motion_utils::removeOverlapPoints(control_data.interpolated_traj.points); - control_data.nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + autoware::motion_utils::removeOverlapPoints(control_data.interpolated_traj.points); + control_data.nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( control_data.interpolated_traj.points, nearest_point.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); - control_data.target_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + control_data.target_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( control_data.interpolated_traj.points, target_point.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); @@ -594,7 +594,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist; const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist; - // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity + // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity static constexpr double vel_epsilon = 1e-3; // Let vehicle start after the steering is converged for control accuracy @@ -605,7 +605,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d auto marker = createDefaultMarker( "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - marker.pose = tier4_autoware_utils::calcOffsetPose( + marker.pose = autoware::universe_utils::calcOffsetPose( m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, m_vehicle_width / 2 + 2.0, 1.5); marker.text = "steering not\nconverged"; @@ -971,7 +971,7 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop } const auto traj = control_data.interpolated_traj; - const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj.points); + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(traj.points); if (!stop_idx) { return output_motion; } diff --git a/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp index 9d8ad05235f11..cb87157fe1114 100644 --- a/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_pid_longitudinal_controller/smooth_stop.hpp" +#include "autoware/pid_longitudinal_controller/smooth_stop.hpp" #include // NOLINT diff --git a/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index de4d32008aa28..3c547e762bce6 100644 --- a/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "gtest/gtest.h" #include "interpolation/spherical_linear_interpolation.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include "tf2/LinearMath/Quaternion.h" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/control/autoware_pid_longitudinal_controller/test/test_pid.cpp b/control/autoware_pid_longitudinal_controller/test/test_pid.cpp index 0e3e0c70945dc..a125707f3867f 100644 --- a/control/autoware_pid_longitudinal_controller/test/test_pid.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_pid.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_pid_longitudinal_controller/pid.hpp" +#include "autoware/pid_longitudinal_controller/pid.hpp" #include "gtest/gtest.h" #include diff --git a/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp index 4f4fbb2354fa1..a6d03b8032fe6 100644 --- a/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_pid_longitudinal_controller/smooth_stop.hpp" +#include "autoware/pid_longitudinal_controller/smooth_stop.hpp" #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit.hpp similarity index 94% rename from control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit.hpp rename to control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit.hpp index 7998cbefa4e39..4b16d2bf5717d 100644 --- a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit.hpp @@ -27,8 +27,8 @@ * limitations under the License. */ -#ifndef AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_ -#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_ +#ifndef AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_ +#define AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_ #include @@ -85,4 +85,4 @@ class PurePursuit } // namespace autoware::pure_pursuit -#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_ +#endif // AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_ diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp similarity index 90% rename from control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp rename to control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp index f5c6ff50712eb..16cb6471deb2c 100644 --- a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp @@ -27,19 +27,19 @@ * limitations under the License. */ -#ifndef AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ -#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ +#ifndef AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ +#define AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ -#include "autoware_pure_pursuit/autoware_pure_pursuit.hpp" -#include "autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp" -#include "autoware_trajectory_follower_base/lateral_controller_base.hpp" +#include "autoware/pure_pursuit/autoware_pure_pursuit.hpp" +#include "autoware/pure_pursuit/autoware_pure_pursuit_viz.hpp" +#include "autoware/trajectory_follower_base/lateral_controller_base.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include -#include -#include +#include +#include +#include #include "autoware_control_msgs/msg/lateral.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -176,4 +176,4 @@ class PurePursuitLateralController : public LateralControllerBase } // namespace autoware::pure_pursuit -#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ +#endif // AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_node.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp similarity index 83% rename from control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_node.hpp rename to control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp index 1a67dbe1a178b..58ce03a0dbf26 100644 --- a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_node.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp @@ -27,15 +27,15 @@ * limitations under the License. */ -#ifndef AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_ -#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_ +#ifndef AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_ +#define AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_ -#include "autoware_pure_pursuit/autoware_pure_pursuit.hpp" -#include "autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp" +#include "autoware/pure_pursuit/autoware_pure_pursuit.hpp" +#include "autoware/pure_pursuit/autoware_pure_pursuit_viz.hpp" +#include +#include #include -#include -#include #include #include @@ -78,10 +78,10 @@ class PurePursuitNode : public rclcpp::Node private: // Subscriber - tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::SelfPoseListener self_pose_listener_{this}; + autoware::universe_utils::InterProcessPollingSubscriber sub_trajectory_{this, "input/reference_trajectory"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_current_odometry_{this, "input/current_odometry"}; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; @@ -126,4 +126,4 @@ class PurePursuitNode : public rclcpp::Node } // namespace autoware::pure_pursuit -#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_ +#endif // AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_ diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_viz.hpp similarity index 89% rename from control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp rename to control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_viz.hpp index e5fe80d9dc68c..ffe328b225d61 100644 --- a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_viz.hpp @@ -27,8 +27,8 @@ * limitations under the License. */ -#ifndef AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_ -#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_ +#ifndef AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_ +#define AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_ #include #include @@ -44,4 +44,4 @@ visualization_msgs::msg::Marker createTrajectoryCircleMarker( const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose); } // namespace autoware::pure_pursuit -#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_ +#endif // AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_ diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/interpolate.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/interpolate.hpp similarity index 91% rename from control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/interpolate.hpp rename to control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/interpolate.hpp index eb7703b56c1a2..01d64fe83c54f 100644 --- a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/interpolate.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/interpolate.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ -#define AUTOWARE_PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ +#ifndef AUTOWARE__PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ +#define AUTOWARE__PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ #include #include @@ -52,4 +52,4 @@ class SplineInterpolate }; } // namespace autoware::pure_pursuit -#endif // AUTOWARE_PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ +#endif // AUTOWARE__PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/marker_helper.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/marker_helper.hpp similarity index 93% rename from control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/marker_helper.hpp rename to control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/marker_helper.hpp index d265ab751313a..694a9f9ed0ce5 100644 --- a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/marker_helper.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/marker_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ -#define AUTOWARE_PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ +#ifndef AUTOWARE__PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ +#define AUTOWARE__PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ #include @@ -104,4 +104,4 @@ inline void appendMarkerArray( } } // namespace autoware::pure_pursuit -#endif // AUTOWARE_PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ +#endif // AUTOWARE__PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/planning_utils.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/planning_utils.hpp similarity index 96% rename from control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/planning_utils.hpp rename to control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/planning_utils.hpp index e5f7072435c9c..229cc6654c868 100644 --- a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/planning_utils.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/planning_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ -#define AUTOWARE_PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ +#ifndef AUTOWARE__PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ +#define AUTOWARE__PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ #define EIGEN_MPL2_ONLY @@ -141,4 +141,4 @@ geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double _yaw); } // namespace planning_utils } // namespace autoware::pure_pursuit -#endif // AUTOWARE_PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ +#endif // AUTOWARE__PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/tf_utils.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/tf_utils.hpp similarity index 94% rename from control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/tf_utils.hpp rename to control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/tf_utils.hpp index b5b635feebcc7..05701f678dc3a 100644 --- a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/tf_utils.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/tf_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PURE_PURSUIT__UTIL__TF_UTILS_HPP_ -#define AUTOWARE_PURE_PURSUIT__UTIL__TF_UTILS_HPP_ +#ifndef AUTOWARE__PURE_PURSUIT__UTIL__TF_UTILS_HPP_ +#define AUTOWARE__PURE_PURSUIT__UTIL__TF_UTILS_HPP_ #include @@ -94,4 +94,4 @@ inline boost::optional getCurrentPose( } // namespace tf_utils } // namespace autoware::pure_pursuit -#endif // AUTOWARE_PURE_PURSUIT__UTIL__TF_UTILS_HPP_ +#endif // AUTOWARE__PURE_PURSUIT__UTIL__TF_UTILS_HPP_ diff --git a/control/autoware_pure_pursuit/package.xml b/control/autoware_pure_pursuit/package.xml index 138eda06926b5..fc5c0e41a15a0 100644 --- a/control/autoware_pure_pursuit/package.xml +++ b/control/autoware_pure_pursuit/package.xml @@ -15,13 +15,14 @@ autoware_cmake autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base + autoware_universe_utils autoware_vehicle_info_utils boost geometry_msgs libboost-dev - motion_utils nav_msgs rclcpp rclcpp_components @@ -31,7 +32,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs visualization_msgs diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp index ba1d97bf66d28..803a6ef1e8617 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp @@ -28,11 +28,11 @@ * limitations under the License. */ -#include "autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp" +#include "autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp" -#include "autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp" -#include "autoware_pure_pursuit/util/planning_utils.hpp" -#include "autoware_pure_pursuit/util/tf_utils.hpp" +#include "autoware/pure_pursuit/autoware_pure_pursuit_viz.hpp" +#include "autoware/pure_pursuit/util/planning_utils.hpp" +#include "autoware/pure_pursuit/util/tf_utils.hpp" #include @@ -141,7 +141,7 @@ TrajectoryPoint PurePursuitLateralController::calcNextPose( const double ds, TrajectoryPoint & point, Lateral cmd) const { geometry_msgs::msg::Transform transform; - transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); + transform.translation = autoware::universe_utils::createTranslation(ds, 0.0, 0.0); transform.rotation = planning_utils::getQuaternionFromYaw(((tan(cmd.steering_tire_angle) * ds) / param_.wheel_base)); TrajectoryPoint output_p; @@ -158,17 +158,17 @@ void PurePursuitLateralController::setResampledTrajectory() { // Interpolate with constant interval distance. std::vector out_arclength; - const auto input_tp_array = motion_utils::convertToTrajectoryPointArray(trajectory_); - const auto traj_length = motion_utils::calcArcLength(input_tp_array); + const auto input_tp_array = autoware::motion_utils::convertToTrajectoryPointArray(trajectory_); + const auto traj_length = autoware::motion_utils::calcArcLength(input_tp_array); for (double s = 0; s < traj_length; s += param_.resampling_ds) { out_arclength.push_back(s); } - trajectory_resampled_ = - std::make_shared(motion_utils::resampleTrajectory( - motion_utils::convertToTrajectory(input_tp_array), out_arclength)); + trajectory_resampled_ = std::make_shared( + autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input_tp_array), out_arclength)); trajectory_resampled_->points.back() = trajectory_.points.back(); trajectory_resampled_->header = trajectory_.header; - output_tp_array_ = motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_); + output_tp_array_ = autoware::motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_); } double PurePursuitLateralController::calcCurvature(const size_t closest_idx) @@ -202,10 +202,10 @@ double PurePursuitLateralController::calcCurvature(const size_t closest_idx) double current_curvature = 0.0; try { - current_curvature = tier4_autoware_utils::calcCurvature( - tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(prev_idx)), - tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(closest_idx)), - tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(next_idx))); + current_curvature = autoware::universe_utils::calcCurvature( + autoware::universe_utils::getPoint(trajectory_resampled_->points.at(prev_idx)), + autoware::universe_utils::getPoint(trajectory_resampled_->points.at(closest_idx)), + autoware::universe_utils::getPoint(trajectory_resampled_->points.at(next_idx))); } catch (std::exception const & e) { // ...code that handles the error... RCLCPP_WARN(rclcpp::get_logger("pure_pursuit"), "%s", e.what()); @@ -268,8 +268,8 @@ void PurePursuitLateralController::averageFilterTrajectory( boost::optional PurePursuitLateralController::generatePredictedTrajectory() { - const auto closest_idx_result = - motion_utils::findNearestIndex(output_tp_array_, current_odometry_.pose.pose, 3.0, M_PI_4); + const auto closest_idx_result = autoware::motion_utils::findNearestIndex( + output_tp_array_, current_odometry_.pose.pose, 3.0, M_PI_4); if (!closest_idx_result) { return boost::none; @@ -427,7 +427,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( // Calculate target point for velocity/acceleration const auto closest_idx_result = - motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4); + autoware::motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4); if (!closest_idx_result) { RCLCPP_ERROR(logger_, "cannot find closest waypoint"); return {}; @@ -439,7 +439,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( // calculate the lateral error const double lateral_error = - motion_utils::calcLateralOffset(trajectory_resampled_->points, pose.position); + autoware::motion_utils::calcLateralOffset(trajectory_resampled_->points, pose.position); // calculate the current curvature diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp index 23ce643e18eed..9a3b90b86b98e 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp @@ -28,11 +28,11 @@ * limitations under the License. */ -#include "autoware_pure_pursuit/autoware_pure_pursuit_node.hpp" +#include "autoware/pure_pursuit/autoware_pure_pursuit_node.hpp" -#include "autoware_pure_pursuit/pure_pursuit_viz.hpp" -#include "autoware_pure_pursuit/util/planning_utils.hpp" -#include "autoware_pure_pursuit/util/tf_utils.hpp" +#include "autoware/pure_pursuit/pure_pursuit_viz.hpp" +#include "autoware/pure_pursuit/util/planning_utils.hpp" +#include "autoware/pure_pursuit/util/tf_utils.hpp" #include diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_viz.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_viz.cpp index 0a2f5fd92a0eb..ceb4601ce2d1e 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_viz.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_viz.cpp @@ -28,10 +28,10 @@ * limitations under the License. */ -#include "autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp" +#include "autoware/pure_pursuit/autoware_pure_pursuit_viz.hpp" -#include "autoware_pure_pursuit/util/marker_helper.hpp" -#include "autoware_pure_pursuit/util/planning_utils.hpp" +#include "autoware/pure_pursuit/util/marker_helper.hpp" +#include "autoware/pure_pursuit/util/planning_utils.hpp" #include diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp index 25a3334eb26f8..ed9579f296fea 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp @@ -27,9 +27,9 @@ * limitations under the License. */ -#include "autoware_pure_pursuit/autoware_pure_pursuit.hpp" +#include "autoware/pure_pursuit/autoware_pure_pursuit.hpp" -#include "autoware_pure_pursuit/util/planning_utils.hpp" +#include "autoware/pure_pursuit/util/planning_utils.hpp" #include #include diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp index 8d7078c87cf39..62f5f7a5737c9 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_pure_pursuit/util/interpolate.hpp" +#include "autoware/pure_pursuit/util/interpolate.hpp" #include #include diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp index bed6fafcf5bfd..71c48fb35e998 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_pure_pursuit/util/planning_utils.hpp" +#include "autoware/pure_pursuit/util/planning_utils.hpp" #include #include diff --git a/control/autoware_shift_decider/include/autoware_shift_decider/autoware_shift_decider.hpp b/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp similarity index 78% rename from control/autoware_shift_decider/include/autoware_shift_decider/autoware_shift_decider.hpp rename to control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp index 4702ef17e49bf..c7ce822d1ac18 100644 --- a/control/autoware_shift_decider/include/autoware_shift_decider/autoware_shift_decider.hpp +++ b/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ -#define AUTOWARE_SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ +#ifndef AUTOWARE__SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ +#define AUTOWARE__SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include @@ -43,11 +43,11 @@ class ShiftDecider : public rclcpp::Node void initTimer(double period_s); rclcpp::Publisher::SharedPtr pub_shift_cmd_; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_control_cmd_{this, "input/control_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_autoware_state_{this, "input/state"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_current_gear_{this, "input/current_gear"}; rclcpp::TimerBase::SharedPtr timer_; @@ -62,4 +62,4 @@ class ShiftDecider : public rclcpp::Node }; } // namespace autoware::shift_decider -#endif // AUTOWARE_SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ +#endif // AUTOWARE__SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ diff --git a/control/autoware_shift_decider/package.xml b/control/autoware_shift_decider/package.xml index 40708c2c73cf1..ebe86ddaac6d6 100644 --- a/control/autoware_shift_decider/package.xml +++ b/control/autoware_shift_decider/package.xml @@ -15,10 +15,10 @@ autoware_control_msgs autoware_system_msgs + autoware_universe_utils autoware_vehicle_msgs rclcpp rclcpp_components - tier4_autoware_utils ament_cmake_cppcheck ament_cmake_cpplint diff --git a/control/autoware_shift_decider/src/autoware_shift_decider.cpp b/control/autoware_shift_decider/src/autoware_shift_decider.cpp index 91345376c2681..846eaabee4b88 100644 --- a/control/autoware_shift_decider/src/autoware_shift_decider.cpp +++ b/control/autoware_shift_decider/src/autoware_shift_decider.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_shift_decider/autoware_shift_decider.hpp" +#include "autoware/shift_decider/autoware_shift_decider.hpp" #include diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py index 228b0c37c5e7b..3877f7177720b 100644 --- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py @@ -560,12 +560,12 @@ def update_input_queue( np.array(time_stamp), np.array(steer_history) ) - self.acc_input_queue[ - drive_functions.acc_ctrl_queue_size - acc_num : - ] = acc_interpolate(time_stamp_acc) - self.steer_input_queue[ - drive_functions.steer_ctrl_queue_size - steer_num : - ] = steer_interpolate(time_stamp_steer) + self.acc_input_queue[drive_functions.acc_ctrl_queue_size - acc_num :] = ( + acc_interpolate(time_stamp_acc) + ) + self.steer_input_queue[drive_functions.steer_ctrl_queue_size - steer_num :] = ( + steer_interpolate(time_stamp_steer) + ) if ( acc_num == drive_functions.acc_ctrl_queue_size diff --git a/control/autoware_smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml index a05f06cd865ff..01b9478aeeaee 100644 --- a/control/autoware_smart_mpc_trajectory_follower/package.xml +++ b/control/autoware_smart_mpc_trajectory_follower/package.xml @@ -21,15 +21,15 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_system_msgs + autoware_universe_utils autoware_vehicle_msgs - motion_utils pybind11_vendor python3-scipy rclcpp rclcpp_components - tier4_autoware_utils ros2launch diff --git a/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/input_data.hpp b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/input_data.hpp similarity index 87% rename from control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/input_data.hpp rename to control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/input_data.hpp index 679f6334b7187..25ff7c21c38c0 100644 --- a/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/input_data.hpp +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/input_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ -#define AUTOWARE_TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ +#ifndef AUTOWARE__TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -33,4 +33,4 @@ struct InputData }; } // namespace autoware::motion::control::trajectory_follower -#endif // AUTOWARE_TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ +#endif // AUTOWARE__TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ diff --git a/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/lateral_controller_base.hpp b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/lateral_controller_base.hpp similarity index 78% rename from control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/lateral_controller_base.hpp rename to control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/lateral_controller_base.hpp index f3501336928a3..891b3982ddf49 100644 --- a/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/lateral_controller_base.hpp +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/lateral_controller_base.hpp @@ -12,17 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ -#define AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ +#ifndef AUTOWARE__TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ -#include "autoware_trajectory_follower_base/input_data.hpp" -#include "autoware_trajectory_follower_base/sync_data.hpp" +#include "autoware/trajectory_follower_base/input_data.hpp" +#include "autoware/trajectory_follower_base/sync_data.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/lateral.hpp" #include - namespace autoware::motion::control::trajectory_follower { struct LateralOutput @@ -44,4 +43,4 @@ class LateralControllerBase } // namespace autoware::motion::control::trajectory_follower -#endif // AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ +#endif // AUTOWARE__TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ diff --git a/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/longitudinal_controller_base.hpp b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/longitudinal_controller_base.hpp similarity index 79% rename from control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/longitudinal_controller_base.hpp rename to control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/longitudinal_controller_base.hpp index b0f715692d079..176141572d6a8 100644 --- a/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/longitudinal_controller_base.hpp +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/longitudinal_controller_base.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ -#define AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#ifndef AUTOWARE__TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ -#include "autoware_trajectory_follower_base/input_data.hpp" -#include "autoware_trajectory_follower_base/sync_data.hpp" +#include "autoware/trajectory_follower_base/input_data.hpp" +#include "autoware/trajectory_follower_base/sync_data.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/longitudinal.hpp" @@ -46,4 +46,4 @@ class LongitudinalControllerBase } // namespace autoware::motion::control::trajectory_follower -#endif // AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#endif // AUTOWARE__TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ diff --git a/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/sync_data.hpp b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/sync_data.hpp similarity index 83% rename from control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/sync_data.hpp rename to control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/sync_data.hpp index 6034719241430..494423c7f01ac 100644 --- a/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/sync_data.hpp +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/sync_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ -#define AUTOWARE_TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ +#ifndef AUTOWARE__TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ namespace autoware::motion::control::trajectory_follower { @@ -30,4 +30,4 @@ struct LongitudinalSyncData } // namespace autoware::motion::control::trajectory_follower -#endif // AUTOWARE_TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ +#endif // AUTOWARE__TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ diff --git a/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/visibility_control.hpp b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/visibility_control.hpp similarity index 88% rename from control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/visibility_control.hpp rename to control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/visibility_control.hpp index 1a50424ebec43..7eb71d6d0d9aa 100644 --- a/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/visibility_control.hpp +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE_TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE__TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -34,4 +34,4 @@ #error "Unsupported Build Configuration" #endif -#endif // AUTOWARE_TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE__TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index 283ab791c1ef9..fa8f5847d6c31 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -21,7 +21,9 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_motion_utils autoware_planning_msgs + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs @@ -29,14 +31,12 @@ eigen geometry_msgs interpolation - motion_utils osqp_interface rclcpp rclcpp_components std_msgs tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_ros diff --git a/control/autoware_trajectory_follower_base/src/lateral_controller_base.cpp b/control/autoware_trajectory_follower_base/src/lateral_controller_base.cpp index ce5c521089328..f5839fb4bc944 100644 --- a/control/autoware_trajectory_follower_base/src/lateral_controller_base.cpp +++ b/control/autoware_trajectory_follower_base/src/lateral_controller_base.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_trajectory_follower_base/lateral_controller_base.hpp" +#include "autoware/trajectory_follower_base/lateral_controller_base.hpp" namespace autoware::motion::control::trajectory_follower { diff --git a/control/autoware_trajectory_follower_base/src/longitudinal_controller_base.cpp b/control/autoware_trajectory_follower_base/src/longitudinal_controller_base.cpp index 8fac86b3883a1..a8303fc82235e 100644 --- a/control/autoware_trajectory_follower_base/src/longitudinal_controller_base.cpp +++ b/control/autoware_trajectory_follower_base/src/longitudinal_controller_base.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_trajectory_follower_base/longitudinal_controller_base.hpp" +#include "autoware/trajectory_follower_base/longitudinal_controller_base.hpp" namespace autoware::motion::control::trajectory_follower { diff --git a/control/autoware_trajectory_follower_node/CMakeLists.txt b/control/autoware_trajectory_follower_node/CMakeLists.txt index cca8267d4d3be..945d3224d76e5 100644 --- a/control/autoware_trajectory_follower_node/CMakeLists.txt +++ b/control/autoware_trajectory_follower_node/CMakeLists.txt @@ -6,7 +6,7 @@ autoware_package() set(CONTROLLER_NODE controller_node) ament_auto_add_library(${CONTROLLER_NODE} SHARED - include/autoware_trajectory_follower_node/controller_node.hpp + include/autoware/trajectory_follower_node/controller_node.hpp src/controller_node.cpp ) @@ -18,7 +18,7 @@ rclcpp_components_register_node(${CONTROLLER_NODE} # simple trajectory follower set(SIMPLE_TRAJECTORY_FOLLOWER_NODE simple_trajectory_follower) ament_auto_add_library(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} SHARED - include/autoware_trajectory_follower_node/simple_trajectory_follower.hpp + include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp src/simple_trajectory_follower.cpp ) diff --git a/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/controller_node.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp similarity index 74% rename from control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/controller_node.hpp rename to control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp index cd6ddf427d005..a5f8665328f34 100644 --- a/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/controller_node.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp @@ -12,24 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ -#define AUTOWARE_TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ - -#include "autoware_trajectory_follower_base/lateral_controller_base.hpp" -#include "autoware_trajectory_follower_base/longitudinal_controller_base.hpp" -#include "autoware_trajectory_follower_node/visibility_control.hpp" +#ifndef AUTOWARE__TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ + +#include "autoware/trajectory_follower_base/lateral_controller_base.hpp" +#include "autoware/trajectory_follower_base/longitudinal_controller_base.hpp" +#include "autoware/trajectory_follower_node/visibility_control.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include -#include +#include +#include #include "autoware_control_msgs/msg/control.hpp" #include "autoware_control_msgs/msg/longitudinal.hpp" @@ -54,8 +55,8 @@ using trajectory_follower::LongitudinalOutput; namespace trajectory_follower_node { +using autoware::universe_utils::StopWatch; using autoware_adapi_v1_msgs::msg::OperationModeState; -using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -73,24 +74,29 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node double timeout_thr_sec_; boost::optional longitudinal_output_{boost::none}; + std::shared_ptr diag_updater_ = + std::make_shared( + this); // Diagnostic updater for publishing diagnostic data. + std::shared_ptr longitudinal_controller_; std::shared_ptr lateral_controller_; // Subscribers - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_ref_path_{this, "~/input/reference_trajectory"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_odometry_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/current_odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber< + autoware_vehicle_msgs::msg::SteeringReport> sub_steering_{this, "~/input/current_steering"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< geometry_msgs::msg::AccelWithCovarianceStamped> sub_accel_{this, "~/input/current_accel"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ this, "~/input/current_operation_mode"}; // Publishers @@ -129,9 +135,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node const trajectory_follower::InputData & input_data, const trajectory_follower::LateralOutput & lat_out) const; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void publishProcessingTime( const double t_ms, const rclcpp::Publisher::SharedPtr pub); @@ -142,4 +148,4 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node } // namespace trajectory_follower_node } // namespace autoware::motion::control -#endif // AUTOWARE_TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ +#endif // AUTOWARE__TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ diff --git a/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/simple_trajectory_follower.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp similarity index 80% rename from control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/simple_trajectory_follower.hpp rename to control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp index 094f6fb74f2ad..c23128ebfb695 100644 --- a/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/simple_trajectory_follower.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ -#define AUTOWARE_TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#ifndef AUTOWARE__TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include @@ -44,9 +44,9 @@ class SimpleTrajectoryFollower : public rclcpp::Node ~SimpleTrajectoryFollower() = default; private: - tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_kinematics_{ this, "~/input/kinematics"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_trajectory_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_trajectory_{ this, "~/input/trajectory"}; rclcpp::Publisher::SharedPtr pub_cmd_; rclcpp::TimerBase::SharedPtr timer_; @@ -67,4 +67,4 @@ class SimpleTrajectoryFollower : public rclcpp::Node } // namespace simple_trajectory_follower -#endif // AUTOWARE_TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#endif // AUTOWARE__TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ diff --git a/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/visibility_control.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/visibility_control.hpp similarity index 88% rename from control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/visibility_control.hpp rename to control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/visibility_control.hpp index 8f25e1a815538..c38c2ab03629c 100644 --- a/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/visibility_control.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE_TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE__TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -34,4 +34,4 @@ #error "Unsupported Build Configuration" #endif -#endif // AUTOWARE_TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE__TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ diff --git a/control/autoware_trajectory_follower_node/package.xml b/control/autoware_trajectory_follower_node/package.xml index 0726b919d387e..55ac989ad4a58 100644 --- a/control/autoware_trajectory_follower_node/package.xml +++ b/control/autoware_trajectory_follower_node/package.xml @@ -21,17 +21,17 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_motion_utils autoware_mpc_lateral_controller autoware_pid_longitudinal_controller autoware_planning_msgs autoware_pure_pursuit autoware_trajectory_follower_base + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs - motion_utils rclcpp rclcpp_components - tier4_autoware_utils visualization_msgs ros2launch diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index 09a8bd4e9506d..c1e5fe646cdaa 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_trajectory_follower_node/controller_node.hpp" +#include "autoware/trajectory_follower_node/controller_node.hpp" -#include "autoware_mpc_lateral_controller/mpc_lateral_controller.hpp" -#include "autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp" -#include "autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "autoware/mpc_lateral_controller/mpc_lateral_controller.hpp" +#include "autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp" +#include "autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include #include @@ -39,7 +39,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control getLateralControllerMode(declare_parameter("lateral_controller_mode")); switch (lateral_controller_mode) { case LateralControllerMode::MPC: { - lateral_controller_ = std::make_shared(*this); + lateral_controller_ = + std::make_shared(*this, diag_updater_); break; } case LateralControllerMode::PURE_PURSUIT: { @@ -80,9 +81,10 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } Controller::LateralControllerMode Controller::getLateralControllerMode( @@ -222,10 +224,10 @@ void Controller::publishDebugMarker( // steer converged marker { - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); + autoware::universe_utils::createMarkerScale(0.0, 0.0, 1.0), + autoware::universe_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); marker.pose = input_data.current_odometry.pose.pose; std::stringstream ss; diff --git a/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp index e34c67b72d30f..95082e608d50f 100644 --- a/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_trajectory_follower_node/simple_trajectory_follower.hpp" +#include "autoware/trajectory_follower_node/simple_trajectory_follower.hpp" -#include -#include +#include +#include #include namespace simple_trajectory_follower { -using motion_utils::findNearestIndex; -using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::calcYawDeviation; +using autoware::motion_utils::findNearestIndex; +using autoware::universe_utils::calcLateralDeviation; +using autoware::universe_utils::calcYawDeviation; SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) : Node("simple_trajectory_follower", options) diff --git a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp index 0ef35b222d4a1..e0dc5ede906a9 100644 --- a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp +++ b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "ament_index_cpp/get_package_share_directory.hpp" -#include "autoware_trajectory_follower_node/controller_node.hpp" +#include "autoware/trajectory_follower_node/controller_node.hpp" #include "fake_test_node/fake_test_node.hpp" #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" diff --git a/control/autoware_vehicle_cmd_gate/package.xml b/control/autoware_vehicle_cmd_gate/package.xml index f67d01a5593ae..60bbf23f9d5a8 100644 --- a/control/autoware_vehicle_cmd_gate/package.xml +++ b/control/autoware_vehicle_cmd_gate/package.xml @@ -17,18 +17,18 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_motion_utils + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs component_interface_specs component_interface_utils diagnostic_updater geometry_msgs - motion_utils rclcpp rclcpp_components std_srvs tier4_api_utils - tier4_autoware_utils tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index e8902c9dbf5b3..87e79f59bc356 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -14,8 +14,8 @@ #include "vehicle_cmd_gate.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "marker_helper.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" #include #include @@ -87,6 +87,37 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) external_emergency_stop_heartbeat_sub_ = create_subscription( "input/external_emergency_stop_heartbeat", 1, std::bind(&VehicleCmdGate::onExternalEmergencyStopHeartbeat, this, _1)); + gate_mode_sub_ = create_subscription( + "input/gate_mode", 1, std::bind(&VehicleCmdGate::onGateMode, this, _1)); + engage_sub_ = create_subscription( + "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1)); + kinematics_sub_ = create_subscription( + "/localization/kinematic_state", 1, + [this](Odometry::SharedPtr msg) { current_kinematics_ = *msg; }); + acc_sub_ = create_subscription( + "input/acceleration", 1, [this](AccelWithCovarianceStamped::SharedPtr msg) { + current_acceleration_ = msg->accel.accel.linear.x; + }); + steer_sub_ = create_subscription( + "input/steering", 1, + [this](SteeringReport::SharedPtr msg) { current_steer_ = msg->steering_tire_angle; }); + operation_mode_sub_ = create_subscription( + "input/operation_mode", rclcpp::QoS(1).transient_local(), + [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; }); + mrm_state_sub_ = create_subscription( + "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1)); + + // Subscriber for auto + auto_control_cmd_sub_ = create_subscription( + "input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1)); + + // Subscriber for external + remote_control_cmd_sub_ = create_subscription( + "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1)); + + // Subscriber for emergency + emergency_control_cmd_sub_ = create_subscription( + "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1)); // Parameter use_emergency_handling_ = declare_parameter("use_emergency_handling"); @@ -179,9 +210,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) timer_pub_status_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this)); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); // Parameter Callback set_param_res_ = @@ -191,7 +223,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) rcl_interfaces::msg::SetParametersResult VehicleCmdGate::onParameter( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; // Parameter updateParam(parameters, "use_emergency_handling", use_emergency_handling_); updateParam( @@ -291,10 +323,9 @@ bool VehicleCmdGate::isDataReady() } // for auto -void VehicleCmdGate::onAutoCtrlCmd() +void VehicleCmdGate::onAutoCtrlCmd(Control::ConstSharedPtr msg) { - const auto msg = auto_control_cmd_sub_.takeData(); - if (msg) auto_commands_.control = *msg; + auto_commands_.control = *msg; if (current_gate_mode_.data == GateMode::AUTO) { publishControlCommands(auto_commands_); @@ -302,10 +333,9 @@ void VehicleCmdGate::onAutoCtrlCmd() } // for remote -void VehicleCmdGate::onRemoteCtrlCmd() +void VehicleCmdGate::onRemoteCtrlCmd(Control::ConstSharedPtr msg) { - const auto msg = remote_control_cmd_sub_.takeData(); - if (msg) remote_commands_.control = *msg; + remote_commands_.control = *msg; if (current_gate_mode_.data == GateMode::EXTERNAL) { publishControlCommands(remote_commands_); @@ -313,10 +343,9 @@ void VehicleCmdGate::onRemoteCtrlCmd() } // for emergency -void VehicleCmdGate::onEmergencyCtrlCmd() +void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg) { - const auto msg = emergency_control_cmd_sub_.takeData(); - if (msg) emergency_commands_.control = *msg; + emergency_commands_.control = *msg; if (use_emergency_handling_ && is_system_emergency_) { publishControlCommands(emergency_commands_); @@ -325,25 +354,7 @@ void VehicleCmdGate::onEmergencyCtrlCmd() void VehicleCmdGate::onTimer() { - onGateMode(); - - const auto msg_kinematics = kinematics_sub_.takeData(); - if (msg_kinematics) current_kinematics_ = *msg_kinematics; - - const auto msg_acceleration = acc_sub_.takeData(); - if (msg_acceleration) current_acceleration_ = msg_acceleration->accel.accel.linear.x; - - const auto msg_steer = steer_sub_.takeData(); - if (msg_steer) current_steer_ = msg_steer->steering_tire_angle; - - const auto msg_operation_mode = operation_mode_sub_.takeData(); - if (msg_operation_mode) current_operation_mode_ = *msg_operation_mode; - - onMrmState(); - // Subscriber for auto - onAutoCtrlCmd(); - const auto msg_auto_command_turn_indicator = auto_turn_indicator_cmd_sub_.takeData(); if (msg_auto_command_turn_indicator) auto_commands_.turn_indicator = *msg_auto_command_turn_indicator; @@ -355,8 +366,6 @@ void VehicleCmdGate::onTimer() if (msg_auto_command_gear) auto_commands_.gear = *msg_auto_command_gear; // Subscribe for external - onRemoteCtrlCmd(); - const auto msg_remote_command_turn_indicator = remote_turn_indicator_cmd_sub_.takeData(); if (msg_remote_command_turn_indicator) remote_commands_.turn_indicator = *msg_remote_command_turn_indicator; @@ -369,8 +378,6 @@ void VehicleCmdGate::onTimer() if (msg_remote_command_gear) remote_commands_.gear = *msg_remote_command_gear; // Subscribe for emergency - onEmergencyCtrlCmd(); - const auto msg_emergency_command_hazard_light = emergency_hazard_light_cmd_sub_.takeData(); if (msg_emergency_command_hazard_light) emergency_commands_.hazard_light = *msg_emergency_command_hazard_light; @@ -378,8 +385,6 @@ void VehicleCmdGate::onTimer() const auto msg_emergency_command_gear = emergency_gear_cmd_sub_.takeData(); if (msg_emergency_command_gear) emergency_commands_.gear = *msg_emergency_command_gear; - onEngage(); - updater_.force_update(); if (!isDataReady()) { @@ -705,12 +710,10 @@ void VehicleCmdGate::onExternalEmergencyStopHeartbeat( external_emergency_stop_heartbeat_received_time_ = std::make_shared(this->now()); } -void VehicleCmdGate::onGateMode() +void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg) { - const auto msg = gate_mode_sub_.takeData(); - if (!msg) return; - const auto prev_gate_mode = current_gate_mode_; + current_gate_mode_ = *msg; if (current_gate_mode_.data != prev_gate_mode.data) { RCLCPP_INFO( @@ -719,10 +722,9 @@ void VehicleCmdGate::onGateMode() } } -void VehicleCmdGate::onEngage() +void VehicleCmdGate::onEngage(EngageMsg::ConstSharedPtr msg) { - const auto msg = engage_sub_.takeData(); - if (msg) is_engaged_ = msg->engage; + is_engaged_ = msg->engage; } void VehicleCmdGate::onEngageService( @@ -732,11 +734,8 @@ void VehicleCmdGate::onEngageService( response->status = tier4_api_utils::response_success(); } -void VehicleCmdGate::onMrmState() +void VehicleCmdGate::onMrmState(MrmState::ConstSharedPtr msg) { - const auto msg = mrm_state_sub_.takeData(); - if (!msg) return; - is_system_emergency_ = (msg->state == MrmState::MRM_OPERATING || msg->state == MrmState::MRM_SUCCEEDED || msg->state == MrmState::MRM_FAILED) && @@ -829,7 +828,8 @@ void VehicleCmdGate::checkExternalEmergencyStop(diagnostic_updater::DiagnosticSt } else if (is_external_emergency_stop_) { status.level = DiagnosticStatus::ERROR; status.message = - "external_emergency_stop is required. Please call `clear_external_emergency_stop` service to " + "external_emergency_stop is required. Please call `clear_external_emergency_stop` service " + "to " "clear state."; } else { status.level = DiagnosticStatus::OK; diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 5ca8db5497e59..42e28d633d16b 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -16,17 +16,17 @@ #define VEHICLE_CMD_GATE_HPP_ #include "adapi_pause_interface.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "moderate_stop_interface.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_cmd_filter.hpp" +#include +#include +#include #include #include #include -#include #include -#include -#include #include #include @@ -81,7 +81,7 @@ using nav_msgs::msg::Odometry; using EngageMsg = autoware_vehicle_msgs::msg::Engage; using EngageSrv = tier4_external_api_msgs::srv::Engage; -using motion_utils::VehicleStopChecker; +using autoware::motion_utils::VehicleStopChecker; struct Commands { Control control; @@ -119,22 +119,16 @@ class VehicleCmdGate : public rclcpp::Node const std::vector & parameters); // Subscription rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber gate_mode_sub_{ - this, "input/gate_mode"}; - tier4_autoware_utils::InterProcessPollingSubscriber operation_mode_sub_{ - this, "input/operation_mode", rclcpp::QoS(1).transient_local()}; - tier4_autoware_utils::InterProcessPollingSubscriber mrm_state_sub_{ - this, "input/mrm_state"}; - tier4_autoware_utils::InterProcessPollingSubscriber kinematics_sub_{ - this, "/localization/kinematic_state"}; // for filter - tier4_autoware_utils::InterProcessPollingSubscriber acc_sub_{ - this, "input/acceleration"}; // for filter - tier4_autoware_utils::InterProcessPollingSubscriber steer_sub_{ - this, "input/steering"}; // for filter - - void onGateMode(); + rclcpp::Subscription::SharedPtr gate_mode_sub_; + rclcpp::Subscription::SharedPtr operation_mode_sub_; + rclcpp::Subscription::SharedPtr mrm_state_sub_; + rclcpp::Subscription::SharedPtr kinematics_sub_; // for filter + rclcpp::Subscription::SharedPtr acc_sub_; // for filter + rclcpp::Subscription::SharedPtr steer_sub_; // for filter + + void onGateMode(GateMode::ConstSharedPtr msg); void onExternalEmergencyStopHeartbeat(Heartbeat::ConstSharedPtr msg); - void onMrmState(); + void onMrmState(MrmState::ConstSharedPtr msg); bool is_engaged_; bool is_system_emergency_ = false; @@ -160,37 +154,34 @@ class VehicleCmdGate : public rclcpp::Node // Subscriber for auto Commands auto_commands_; - tier4_autoware_utils::InterProcessPollingSubscriber auto_control_cmd_sub_{ - this, "input/auto/control_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber + rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; + autoware::universe_utils::InterProcessPollingSubscriber auto_turn_indicator_cmd_sub_{this, "input/auto/turn_indicators_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber auto_hazard_light_cmd_sub_{this, "input/auto/hazard_lights_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber auto_gear_cmd_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber auto_gear_cmd_sub_{ this, "input/auto/gear_cmd"}; - void onAutoCtrlCmd(); + void onAutoCtrlCmd(Control::ConstSharedPtr msg); // Subscription for external Commands remote_commands_; - tier4_autoware_utils::InterProcessPollingSubscriber remote_control_cmd_sub_{ - this, "input/external/control_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber + rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; + autoware::universe_utils::InterProcessPollingSubscriber remote_turn_indicator_cmd_sub_{this, "input/external/turn_indicators_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber remote_hazard_light_cmd_sub_{this, "input/external/hazard_lights_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber remote_gear_cmd_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber remote_gear_cmd_sub_{ this, "input/external/gear_cmd"}; - void onRemoteCtrlCmd(); + void onRemoteCtrlCmd(Control::ConstSharedPtr msg); // Subscription for emergency Commands emergency_commands_; - tier4_autoware_utils::InterProcessPollingSubscriber emergency_control_cmd_sub_{ - this, "input/emergency/control_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber + rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; + autoware::universe_utils::InterProcessPollingSubscriber emergency_hazard_light_cmd_sub_{this, "input/emergency/hazard_lights_cmd"}; - tier4_autoware_utils::InterProcessPollingSubscriber emergency_gear_cmd_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber emergency_gear_cmd_sub_{ this, "input/emergency/gear_cmd"}; - void onEmergencyCtrlCmd(); + void onEmergencyCtrlCmd(Control::ConstSharedPtr msg); // Parameter bool use_emergency_handling_; @@ -216,10 +207,10 @@ class VehicleCmdGate : public rclcpp::Node const SetEmergency::Response::SharedPtr response); // TODO(Takagi, Isamu): deprecated - tier4_autoware_utils::InterProcessPollingSubscriber engage_sub_{this, "input/engage"}; + rclcpp::Subscription::SharedPtr engage_sub_; rclcpp::Service::SharedPtr srv_external_emergency_stop_; rclcpp::Service::SharedPtr srv_clear_external_emergency_stop_; - void onEngage(); + void onEngage(EngageMsg::ConstSharedPtr msg); bool onSetExternalEmergencyStopService( const std::shared_ptr req_header, const Trigger::Request::SharedPtr req, const Trigger::Response::SharedPtr res); @@ -270,9 +261,9 @@ class VehicleCmdGate : public rclcpp::Node MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); void publishMarkers(const IsFilterActivated & filter_activated); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::vehicle_cmd_gate diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp index 6ecf453bf7ab3..db0afc134ff79 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp @@ -15,11 +15,11 @@ #ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ #define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "control_performance_analysis/control_performance_analysis_utils.hpp" #include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" #include "control_performance_analysis/msg/float_stamped.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include #include diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp index 73d48db6400a5..b359eb16387db 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -19,9 +19,9 @@ #include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" +#include #include #include -#include #include #include diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index b18fb8c98ccdc..878136d837678 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -25,12 +25,13 @@ builtin_interfaces autoware_control_msgs + autoware_motion_utils autoware_planning_msgs + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs libboost-dev - motion_utils nav_msgs rclcpp rclcpp_components @@ -40,7 +41,6 @@ tf2 tf2_eigen tf2_ros - tier4_autoware_utils tier4_debug_msgs builtin_interfaces global_parameter_loader diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index 51c818205e047..5f72c8ea316bd 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -14,8 +14,8 @@ #include "control_performance_analysis/control_performance_analysis_core.hpp" -#include "motion_utils/trajectory/interpolation.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include @@ -86,7 +86,7 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - const auto closest_segment = motion_utils::findNearestSegmentIndex( + const auto closest_segment = autoware::motion_utils::findNearestSegmentIndex( current_trajectory_ptr_->points, *current_vec_pose_ptr_, p_.acceptable_max_distance_to_waypoint_, p_.acceptable_max_yaw_difference_rad_); @@ -158,7 +158,7 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() // Compute the yaw angle error. const double heading_yaw_error = - tier4_autoware_utils::calcYawDeviation(pose_interp_wp, *current_vec_pose_ptr_); + autoware::universe_utils::calcYawDeviation(pose_interp_wp, *current_vec_pose_ptr_); // Set the values of ErrorMsgVars. @@ -174,7 +174,7 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() const double Vx = odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x; // Current acceleration calculation - const auto ds = tier4_autoware_utils::calcDistance2d( + const auto ds = autoware::universe_utils::calcDistance2d( odom_history_ptr_->at(odom_size - 1).pose.pose, odom_history_ptr_->at(odom_size - 2).pose.pose); const double vel_mean = (odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x + @@ -371,7 +371,8 @@ std::optional ControlPerformanceAnalysisCore::findCurveRefIdx() } auto fun_distance_cond = [this](auto point_t) { - const double dist = tier4_autoware_utils::calcDistance2d(point_t.pose, *interpolated_pose_ptr_); + const double dist = + autoware::universe_utils::calcDistance2d(point_t.pose, *interpolated_pose_ptr_); return dist > p_.wheelbase_; }; @@ -390,7 +391,7 @@ std::optional ControlPerformanceAnalysisCore::findCurveRefIdx() std::pair ControlPerformanceAnalysisCore::calculateClosestPose() { const auto interp_point = - motion_utils::calcInterpolatedPoint(*current_trajectory_ptr_, *current_vec_pose_ptr_); + autoware::motion_utils::calcInterpolatedPoint(*current_trajectory_ptr_, *current_vec_pose_ptr_); const double interp_steering_angle = std::atan(p_.wheelbase_ * estimateCurvature()); @@ -442,7 +443,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() // Compute arc-length ds between 2 points. const double ds_arc_length = - tier4_autoware_utils::calcDistance2d(front_axleWP_pose_prev, front_axleWP_pose); + autoware::universe_utils::calcDistance2d(front_axleWP_pose_prev, front_axleWP_pose); // Define waypoints 10 meters behind the rear axle if exist. // If not exist, we will take the first point of the @@ -461,7 +462,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() // We compute a curvature estimate from these points. double estimated_curvature = 0.0; try { - estimated_curvature = tier4_autoware_utils::calcCurvature( + estimated_curvature = autoware::universe_utils::calcCurvature( points.at(loc_of_back_idx).pose.position, points.at(idx_curve_ref_wp).pose.position, points.at(loc_of_forward_idx).pose.position); } catch (...) { @@ -486,7 +487,7 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() const double look_ahead_distance_pp = std::max(p_.wheelbase_, 2 * Vx); auto fun_distance_cond = [this, &look_ahead_distance_pp](auto point_t) { - const double dist = tier4_autoware_utils::calcDistance2d(point_t, *interpolated_pose_ptr_); + const double dist = autoware::universe_utils::calcDistance2d(point_t, *interpolated_pose_ptr_); return dist > look_ahead_distance_pp; }; diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp index efaefd765e793..8cce8b88a0e7f 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -15,8 +15,8 @@ #ifndef OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ #define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ +#include #include -#include #include #include @@ -35,7 +35,7 @@ namespace obstacle_collision_checker { -using tier4_autoware_utils::LinearRing2d; +using autoware::universe_utils::LinearRing2d; struct Param { diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp index 9d79a0facac95..b08ecccd57282 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -17,13 +17,13 @@ #include "obstacle_collision_checker/obstacle_collision_checker.hpp" +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include #include #include @@ -48,8 +48,8 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node private: // Subscriber - std::shared_ptr self_pose_listener_; - std::shared_ptr transform_listener_; + std::shared_ptr self_pose_listener_; + std::shared_ptr transform_listener_; rclcpp::Subscription::SharedPtr sub_obstacle_pointcloud_; rclcpp::Subscription::SharedPtr sub_reference_trajectory_; @@ -72,8 +72,8 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); // Publisher - std::shared_ptr debug_publisher_; - std::shared_ptr time_publisher_; + std::shared_ptr debug_publisher_; + std::shared_ptr time_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index 795302abe75f4..45b142fe601fe 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -19,6 +19,7 @@ autoware_cmake autoware_planning_msgs + autoware_universe_utils autoware_vehicle_info_utils boost diagnostic_updater @@ -33,7 +34,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index a844fe6a50cbf..9fb3657b957c7 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -14,12 +14,12 @@ #include "obstacle_collision_checker/obstacle_collision_checker.hpp" +#include +#include +#include +#include #include #include -#include -#include -#include -#include #include @@ -90,7 +90,7 @@ ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) Output ObstacleCollisionChecker::update(const Input & input) { Output output; - tier4_autoware_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; // resample trajectory by braking distance constexpr double min_velocity = 0.01; @@ -131,8 +131,9 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajec for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = tier4_autoware_utils::fromMsg(resampled.points.back().pose.position).to_2d(); - const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position).to_2d(); + const auto p1 = + autoware::universe_utils::fromMsg(resampled.points.back().pose.position).to_2d(); + const auto p2 = autoware::universe_utils::fromMsg(point.pose.position).to_2d(); if (boost::geometry::distance(p1, p2) > interval) { resampled.points.push_back(point); @@ -154,8 +155,8 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( for (size_t i = 1; i < trajectory.points.size(); ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = tier4_autoware_utils::fromMsg(cut.points.back().pose.position); - const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); + const auto p1 = autoware::universe_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -197,8 +198,8 @@ std::vector ObstacleCollisionChecker::createVehicleFootprints( std::vector vehicle_footprints; for (const auto & p : trajectory.points) { vehicle_footprints.push_back( - tier4_autoware_utils::transformVector( - local_vehicle_footprint, tier4_autoware_utils::pose2transform(p.pose))); + autoware::universe_utils::transformVector( + local_vehicle_footprint, autoware::universe_utils::pose2transform(p.pose))); } return vehicle_footprints; @@ -221,7 +222,7 @@ std::vector ObstacleCollisionChecker::createVehiclePassingAreas( LinearRing2d ObstacleCollisionChecker::createHullFromFootprints( const LinearRing2d & area1, const LinearRing2d & area2) { - tier4_autoware_utils::MultiPoint2d combined; + autoware::universe_utils::MultiPoint2d combined; for (const auto & p : area1) { combined.push_back(p); } @@ -256,7 +257,7 @@ bool ObstacleCollisionChecker::hasCollision( { for (const auto & point : obstacle_pointcloud.points) { if (boost::geometry::within( - tier4_autoware_utils::Point2d{point.x, point.y}, vehicle_footprint)) { + autoware::universe_utils::Point2d{point.x, point.y}, vehicle_footprint)) { RCLCPP_WARN( rclcpp::get_logger("obstacle_collision_checker"), "[ObstacleCollisionChecker] Collide to Point x: %f y: %f", point.x, point.y); diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp index 402123c81d361..9af5fe24b17ea 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -14,10 +14,10 @@ #include "obstacle_collision_checker/obstacle_collision_checker_node.hpp" +#include +#include +#include #include -#include -#include -#include #include #include @@ -70,8 +70,8 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt obstacle_collision_checker_->setParam(param_); // Subscriber - self_pose_listener_ = std::make_shared(this); - transform_listener_ = std::make_shared(this); + self_pose_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); sub_obstacle_pointcloud_ = create_subscription( "input/obstacle_pointcloud", rclcpp::SensorDataQoS(), @@ -86,8 +86,9 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt "input/odometry", 1, std::bind(&ObstacleCollisionCheckerNode::onOdom, this, _1)); // Publisher - debug_publisher_ = std::make_shared(this, "debug/marker"); - time_publisher_ = std::make_shared(this); + debug_publisher_ = + std::make_shared(this, "debug/marker"); + time_publisher_ = std::make_shared(this); // Diagnostic Updater updater_.setHardwareID("obstacle_collision_checker"); @@ -282,9 +283,9 @@ void ObstacleCollisionCheckerNode::checkLaneDeparture( visualization_msgs::msg::MarkerArray ObstacleCollisionCheckerNode::createMarkerArray() const { - using tier4_autoware_utils::createDefaultMarker; - using tier4_autoware_utils::createMarkerColor; - using tier4_autoware_utils::createMarkerScale; + using autoware::universe_utils::createDefaultMarker; + using autoware::universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray marker_array; diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp index 2a7a552c1973d..e2c76bec24860 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -15,15 +15,15 @@ #ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ #define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#include +#include +#include +#include +#include #include -#include -#include #include #include #include -#include -#include -#include #include #include @@ -45,12 +45,12 @@ namespace autoware::motion::control::predicted_path_checker using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; using PointArray = std::vector; namespace bg = boost::geometry; diff --git a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp index 02ade624d630c..0f537d52cee06 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp @@ -31,9 +31,9 @@ #define EIGEN_MPL2_ONLY +#include #include #include -#include namespace autoware::motion::control::predicted_path_checker { @@ -52,12 +52,12 @@ class PredictedPathCheckerDebugNode ~PredictedPathCheckerDebugNode() {} bool pushPolygon( - const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); + const autoware::universe_utils::Polygon2d & polygon, const double z, const PolygonType & type); bool pushPolygon(const std::vector & polygon, const PolygonType & type); bool pushPolyhedron( - const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware::universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type); bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index 6c4832a0dac3e..b3afcded60438 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -15,17 +15,17 @@ #ifndef PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ #define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#include +#include +#include +#include #include #include #include #include #include -#include -#include #include #include -#include -#include #include #include @@ -50,11 +50,11 @@ namespace autoware::motion::control::predicted_path_checker using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; struct NodeParam { @@ -88,7 +88,7 @@ class PredictedPathCheckerNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr group_cli_; // Subscriber - std::shared_ptr self_pose_listener_; + std::shared_ptr self_pose_listener_; rclcpp::Subscription::SharedPtr sub_dynamic_objects_; rclcpp::Subscription::SharedPtr sub_reference_trajectory_; diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index d9299f09dfb6b..6f5c0e5cdb883 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -15,10 +15,10 @@ #ifndef PREDICTED_PATH_CHECKER__UTILS_HPP_ #define PREDICTED_PATH_CHECKER__UTILS_HPP_ +#include +#include #include #include -#include -#include #include #include @@ -39,6 +39,8 @@ namespace utils { +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; @@ -47,8 +49,6 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using std_msgs::msg::Header; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; using PointArray = std::vector; using TrajectoryPoints = std::vector; diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index 911344c578414..153d11107a8cb 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/predicted_path_checker/package.xml @@ -12,8 +12,10 @@ ament_cmake autoware_cmake + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs + autoware_universe_utils autoware_vehicle_info_utils boost component_interface_specs @@ -21,7 +23,6 @@ diagnostic_updater eigen geometry_msgs - motion_utils nav_msgs rclcpp rclcpp_components @@ -31,7 +32,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_control_msgs tier4_external_api_msgs tier4_planning_msgs diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp index 7d392571546b4..7771f59d15454 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp @@ -14,8 +14,8 @@ #include "predicted_path_checker/collision_checker.hpp" +#include #include -#include #include #include @@ -80,12 +80,12 @@ CollisionChecker::checkTrajectoryForCollision( double distance_to_current = std::numeric_limits::max(); double distance_to_history = std::numeric_limits::max(); if (found_collision_at_dynamic_objects) { - distance_to_current = tier4_autoware_utils::calcDistance2d( + distance_to_current = autoware::universe_utils::calcDistance2d( p_front, found_collision_at_dynamic_objects.get().first); } if (found_collision_at_history) { distance_to_history = - tier4_autoware_utils::calcDistance2d(p_front, found_collision_at_history.get().first); + autoware::universe_utils::calcDistance2d(p_front, found_collision_at_history.get().first); } else { predicted_object_history_.clear(); } @@ -140,7 +140,7 @@ CollisionChecker::checkObstacleHistory( bool is_init = false; std::pair nearest_collision_object_with_point; for (const auto & p : collision_points_in_history) { - double norm = tier4_autoware_utils::calcDistance2d(p.first, base_pose); + double norm = autoware::universe_utils::calcDistance2d(p.first, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; nearest_collision_object_with_point = p; diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp index 3fb7b69d531c0..3fae5e38e7ede 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp @@ -14,29 +14,29 @@ #include "predicted_path_checker/debug_marker.hpp" -#include -#include -#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif -#include +#include #include #include -using motion_utils::createDeletedStopVirtualWallMarker; -using motion_utils::createStopVirtualWallMarker; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; +using autoware::motion_utils::createDeletedStopVirtualWallMarker; +using autoware::motion_utils::createStopVirtualWallMarker; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; namespace autoware::motion::control::predicted_path_checker { @@ -51,7 +51,7 @@ PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode( } bool PredictedPathCheckerDebugNode::pushPolygon( - const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) + const autoware::universe_utils::Polygon2d & polygon, const double z, const PolygonType & type) { std::vector eigen_polygon; for (const auto & point : polygon.outer()) { @@ -82,7 +82,7 @@ bool PredictedPathCheckerDebugNode::pushPolygon( } bool PredictedPathCheckerDebugNode::pushPolyhedron( - const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const autoware::universe_utils::Polygon2d & polyhedron, const double z_min, const double z_max, const PolygonType & type) { std::vector eigen_polyhedron; diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index 38717a1849ad9..d8cf5c34bf535 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -14,11 +14,11 @@ #include "predicted_path_checker/predicted_path_checker_node.hpp" -#include -#include +#include +#include +#include +#include #include -#include -#include #include #include @@ -67,7 +67,7 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n declare_parameter("collision_checker_params.chattering_threshold", 0.2); // Subscriber - self_pose_listener_ = std::make_shared(this); + self_pose_listener_ = std::make_shared(this); sub_dynamic_objects_ = create_subscription( "~/input/objects", rclcpp::SensorDataQoS(), @@ -241,8 +241,9 @@ void PredictedPathCheckerNode::onTimer() // Convert to trajectory array - TrajectoryPoints predicted_trajectory_array = motion_utils::convertToTrajectoryPointArray( - motion_utils::resampleTrajectory(cut_trajectory, node_param_.resample_interval)); + TrajectoryPoints predicted_trajectory_array = + autoware::motion_utils::convertToTrajectoryPointArray( + autoware::motion_utils::resampleTrajectory(cut_trajectory, node_param_.resample_interval)); // Filter the objects @@ -322,7 +323,7 @@ void PredictedPathCheckerNode::onTimer() // trajectory or not const auto reference_trajectory_array = - motion_utils::convertToTrajectoryPointArray(*reference_trajectory_); + autoware::motion_utils::convertToTrajectoryPointArray(*reference_trajectory_); const auto is_discrete_point = isItDiscretePoint(reference_trajectory_array, predicted_trajectory_array.at(stop_idx)); @@ -366,10 +367,11 @@ TrajectoryPoints PredictedPathCheckerNode::trimTrajectoryFromSelfPose( { TrajectoryPoints output{}; - const size_t min_distance_index = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - input, self_pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold) + - 1; + const size_t min_distance_index = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, self_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold) + + 1; for (size_t i = min_distance_index; i < input.size(); ++i) { output.push_back(input.at(i)); @@ -385,9 +387,9 @@ bool PredictedPathCheckerNode::isThereStopPointOnReferenceTrajectory( trimTrajectoryFromSelfPose(reference_trajectory_array, current_pose_.get()->pose); const auto nearest_stop_point_on_ref_trajectory = - motion_utils::findNearestIndex(trimmed_reference_trajectory_array, pose); + autoware::motion_utils::findNearestIndex(trimmed_reference_trajectory_array, pose); - const auto stop_point_on_trajectory = motion_utils::searchZeroVelocityIndex( + const auto stop_point_on_trajectory = autoware::motion_utils::searchZeroVelocityIndex( trimmed_reference_trajectory_array, 0, *nearest_stop_point_on_ref_trajectory); return !!stop_point_on_trajectory; @@ -425,18 +427,18 @@ bool PredictedPathCheckerNode::isItDiscretePoint( const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const { const auto nearest_segment = - motion_utils::findNearestSegmentIndex(reference_trajectory, collision_point.pose); + autoware::motion_utils::findNearestSegmentIndex(reference_trajectory, collision_point.pose); const auto nearest_point = utils::calcInterpolatedPoint( reference_trajectory, collision_point.pose.position, *nearest_segment, false); - const auto distance = tier4_autoware_utils::calcDistance2d( + const auto distance = autoware::universe_utils::calcDistance2d( nearest_point.pose.position, collision_point.pose.position); const auto yaw_diff = - std::abs(tier4_autoware_utils::calcYawDeviation(nearest_point.pose, collision_point.pose)); + std::abs(autoware::universe_utils::calcYawDeviation(nearest_point.pose, collision_point.pose)); return distance >= node_param_.distinct_point_distance_threshold || - yaw_diff >= tier4_autoware_utils::deg2rad(node_param_.distinct_point_yaw_threshold); + yaw_diff >= autoware::universe_utils::deg2rad(node_param_.distinct_point_yaw_threshold); } Trajectory PredictedPathCheckerNode::cutTrajectory( @@ -452,8 +454,8 @@ Trajectory PredictedPathCheckerNode::cutTrajectory( for (size_t i = 1; i < trajectory.points.size(); ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = tier4_autoware_utils::fromMsg(cut.points.back().pose.position); - const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); + const auto p1 = autoware::universe_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); const auto remain_distance = length - total_length; @@ -480,7 +482,7 @@ Trajectory PredictedPathCheckerNode::cutTrajectory( cut.points.push_back(point); total_length += points_distance; } - motion_utils::removeOverlapPoints(cut.points); + autoware::motion_utils::removeOverlapPoints(cut.points); return cut; } @@ -499,7 +501,7 @@ size_t PredictedPathCheckerNode::insertStopPoint( TrajectoryPoints & trajectory, const geometry_msgs::msg::Point collision_point) { const auto nearest_collision_segment = - motion_utils::findNearestSegmentIndex(trajectory, collision_point); + autoware::motion_utils::findNearestSegmentIndex(trajectory, collision_point); const auto nearest_collision_point = utils::calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true); @@ -526,7 +528,7 @@ std::pair PredictedPathCheckerNode::calculateProjectedVelAndAcc( const auto velocity_obj = object.kinematics.initial_twist_with_covariance.twist.linear.x; const auto acceleration_obj = object.kinematics.initial_acceleration_with_covariance.accel.linear.x; - const auto k = std::cos(tier4_autoware_utils::normalizeRadian( + const auto k = std::cos(autoware::universe_utils::normalizeRadian( tf2::getYaw(orientation_obj) - tf2::getYaw(orientation_stop_point))); const auto projected_velocity = velocity_obj * k; const auto projected_acceleration = acceleration_obj * k; @@ -553,10 +555,10 @@ void PredictedPathCheckerNode::filterObstacles( // Check is it near to trajectory const double max_length = utils::calcObstacleMaxLength(object.shape); - const size_t seg_idx = motion_utils::findNearestSegmentIndex( + const size_t seg_idx = autoware::motion_utils::findNearestSegmentIndex( traj, object.kinematics.initial_pose_with_covariance.pose.position); - const auto p_front = tier4_autoware_utils::getPoint(traj.at(seg_idx)); - const auto p_back = tier4_autoware_utils::getPoint(traj.at(seg_idx + 1)); + const auto p_front = autoware::universe_utils::getPoint(traj.at(seg_idx)); + const auto p_back = autoware::universe_utils::getPoint(traj.at(seg_idx + 1)); const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position; const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index a9c3e2b24f9a5..d750f3aa07248 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -21,10 +21,10 @@ namespace utils { -using motion_utils::findFirstNearestIndexWithSoftConstraints; -using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::getRPY; +using autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; +using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::getRPY; // Utils Functions Polygon2d createOneStepPolygon( @@ -38,40 +38,42 @@ Polygon2d createOneStepPolygon( const double rear_overhang = vehicle_info.rear_overhang_m; { // base step - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) - .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + autoware::universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0).position); + autoware::universe_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + .position); appendPointToPolygon( - polygon, - tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); + polygon, autoware::universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0) + .position); + appendPointToPolygon( + polygon, autoware::universe_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0) + .position); } { // next step - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) - .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + autoware::universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) .position); appendPointToPolygon( polygon, - tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0).position); + autoware::universe_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + .position); appendPointToPolygon( - polygon, - tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); + polygon, autoware::universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0) + .position); + appendPointToPolygon( + polygon, autoware::universe_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0) + .position); } - polygon = tier4_autoware_utils::isClockwise(polygon) + polygon = autoware::universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); Polygon2d hull_polygon; boost::geometry::convex_hull(polygon, hull_polygon); @@ -95,8 +97,8 @@ TrajectoryPoint calcInterpolatedPoint( // Calculate interpolation ratio const auto & curr_pt = trajectory.at(segment_idx); const auto & next_pt = trajectory.at(segment_idx + 1); - const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt, next_pt); - const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt, target_point); + const auto v1 = autoware::universe_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = autoware::universe_utils::point2tfVector(curr_pt, target_point); if (v1.length2() < 1e-3) { return curr_pt; } @@ -109,7 +111,7 @@ TrajectoryPoint calcInterpolatedPoint( // pose interpolation interpolated_point.pose = - tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + autoware::universe_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { @@ -158,7 +160,7 @@ std::pair findStopPoint( for (size_t i = collision_idx; i > 0; i--) { distance_point_to_collision = - motion_utils::calcSignedArcLength(trajectory_array, i - 1, collision_idx); + autoware::motion_utils::calcSignedArcLength(trajectory_array, i - 1, collision_idx); if (distance_point_to_collision >= desired_distance_base_link_to_collision) { stop_segment_idx = i - 1; found_stop_point = true; @@ -174,8 +176,8 @@ std::pair findStopPoint( base_point.pose.position.x - next_point.pose.position.x, base_point.pose.position.y - next_point.pose.position.y)); - geometry_msgs::msg::Pose interpolated_pose = - tier4_autoware_utils::calcInterpolatedPose(base_point.pose, next_point.pose, ratio, false); + geometry_msgs::msg::Pose interpolated_pose = autoware::universe_utils::calcInterpolatedPose( + base_point.pose, next_point.pose, ratio, false); TrajectoryPoint output; output.set__pose(interpolated_pose); return std::make_pair(stop_segment_idx, output); @@ -194,7 +196,7 @@ bool isInBrakeDistance( return false; } - const auto distance_to_obstacle = motion_utils::calcSignedArcLength( + const auto distance_to_obstacle = autoware::motion_utils::calcSignedArcLength( trajectory, trajectory.front().pose.position, trajectory.at(stop_idx).pose.position); const double distance_in_delay = relative_velocity * delay_time_sec + @@ -247,7 +249,7 @@ double getNearestPointAndDistanceForPredictedObject( bool is_init = false; for (const auto & p : points) { - double norm = tier4_autoware_utils::calcDistance2d(p, base_pose); + double norm = autoware::universe_utils::calcDistance2d(p, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; *nearest_collision_point = p; @@ -369,7 +371,7 @@ Polygon2d convertObjToPolygon(const PredictedObject & obj) bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos) { - const auto yaw = tier4_autoware_utils::getRPY(ego_pose).z; + const auto yaw = autoware::universe_utils::getRPY(ego_pose).z; const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); const Eigen::Vector2d obstacle_vec( obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y); @@ -401,7 +403,7 @@ void getCurrentObjectPose( PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) { - const double yaw = tier4_autoware_utils::getRPY( + const double yaw = autoware::universe_utils::getRPY( predicted_object.kinematics.initial_pose_with_covariance.pose.orientation) .z; const double vx = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; @@ -412,8 +414,8 @@ void getCurrentObjectPose( const double delta_yaw = predicted_object.kinematics.initial_twist_with_covariance.twist.angular.z * dt; geometry_msgs::msg::Transform transform; - transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); - transform.rotation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, yaw); + transform.translation = autoware::universe_utils::createTranslation(ds, 0.0, 0.0); + transform.rotation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, yaw); tf2::Transform tf_pose; tf2::Transform tf_offset; @@ -422,8 +424,8 @@ void getCurrentObjectPose( tf2::toMsg(tf_pose * tf_offset, predicted_object.kinematics.initial_pose_with_covariance.pose); predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x += ax * dt; predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY( - 0.0, 0.0, tier4_autoware_utils::normalizeRadian(yaw + delta_yaw)); + autoware::universe_utils::createQuaternionFromRPY( + 0.0, 0.0, autoware::universe_utils::normalizeRadian(yaw + delta_yaw)); } } // namespace utils diff --git a/evaluator/control_evaluator/CMakeLists.txt b/evaluator/autoware_control_evaluator/CMakeLists.txt similarity index 50% rename from evaluator/control_evaluator/CMakeLists.txt rename to evaluator/autoware_control_evaluator/CMakeLists.txt index b97a14e29cdcd..189745349a592 100644 --- a/evaluator/control_evaluator/CMakeLists.txt +++ b/evaluator/autoware_control_evaluator/CMakeLists.txt @@ -1,18 +1,19 @@ cmake_minimum_required(VERSION 3.14) -project(control_evaluator) +project(autoware_control_evaluator) find_package(autoware_cmake REQUIRED) autoware_package() find_package(pluginlib REQUIRED) -ament_auto_add_library(${PROJECT_NAME}_node SHARED - src/${PROJECT_NAME}_node.cpp +ament_auto_add_library(control_evaluator_node SHARED + src/control_evaluator_node.cpp + src/metrics/deviation_metrics.cpp ) -rclcpp_components_register_node(${PROJECT_NAME}_node +rclcpp_components_register_node(control_evaluator_node PLUGIN "control_diagnostics::controlEvaluatorNode" - EXECUTABLE ${PROJECT_NAME} + EXECUTABLE control_evaluator ) diff --git a/evaluator/control_evaluator/README.md b/evaluator/autoware_control_evaluator/README.md similarity index 100% rename from evaluator/control_evaluator/README.md rename to evaluator/autoware_control_evaluator/README.md diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp new file mode 100644 index 0000000000000..279bada80e1b9 --- /dev/null +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -0,0 +1,94 @@ +// Copyright 2024 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 AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ +#define AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ + +#include "autoware/control_evaluator/metrics/deviation_metrics.hpp" + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace control_diagnostics +{ + +using autoware_planning_msgs::msg::Trajectory; +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; + +/** + * @brief Node for control evaluation + */ +class controlEvaluatorNode : public rclcpp::Node +{ +public: + explicit controlEvaluatorNode(const rclcpp::NodeOptions & node_options); + void removeOldDiagnostics(const rclcpp::Time & stamp); + void removeDiagnosticsByName(const std::string & name); + void addDiagnostic(const DiagnosticStatus & diag, const rclcpp::Time & stamp); + void updateDiagnosticQueue( + const DiagnosticArray & input_diagnostics, const std::string & function, + const rclcpp::Time & stamp); + + DiagnosticStatus generateLateralDeviationDiagnosticStatus( + const Trajectory & traj, const Point & ego_point); + DiagnosticStatus generateYawDeviationDiagnosticStatus( + const Trajectory & traj, const Pose & ego_pose); + std::optional generateStopDiagnosticStatus( + const DiagnosticArray & diag, const std::string & function_name); + + DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag); + + void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); + void onTimer(); + +private: + // The diagnostics cycle is faster than timer, and each node publishes diagnostic separately. + // takeData() in onTimer() with a polling subscriber will miss a topic, so save all topics with + // onDiagnostics(). + rclcpp::Subscription::SharedPtr control_diag_sub_; + + autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ + this, "~/input/odometry"}; + autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ + this, "~/input/trajectory"}; + + rclcpp::Publisher::SharedPtr metrics_pub_; + + // Calculator + // Metrics + std::deque stamps_; + + // queue for diagnostics and time stamp + std::deque> diag_queue_; + const std::vector target_functions_ = {"autonomous_emergency_braking"}; + + rclcpp::TimerBase::SharedPtr timer_; +}; +} // namespace control_diagnostics + +#endif // AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp new file mode 100644 index 0000000000000..94fbd53532e7d --- /dev/null +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp @@ -0,0 +1,48 @@ +// Copyright 2024 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 AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#define AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ + +#include +#include + +namespace control_diagnostics +{ +namespace metrics +{ +using autoware_planning_msgs::msg::Trajectory; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; + +/** + * @brief calculate lateral deviation of the given trajectory from the reference trajectory + * @param [in] ref reference trajectory + * @param [in] point input point + * @return lateral deviation + */ +double calcLateralDeviation(const Trajectory & traj, const Point & point); + +/** + * @brief calculate yaw deviation of the given trajectory from the reference trajectory + * @param [in] traj input trajectory + * @param [in] pose input pose + * @return yaw deviation + */ +double calcYawDeviation(const Trajectory & traj, const Pose & pose); + +} // namespace metrics +} // namespace control_diagnostics + +#endif // AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ diff --git a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml new file mode 100644 index 0000000000000..e119f1787d21b --- /dev/null +++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/evaluator/control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml similarity index 83% rename from evaluator/control_evaluator/package.xml rename to evaluator/autoware_control_evaluator/package.xml index 10686e9b73761..49631c566eaf5 100644 --- a/evaluator/control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -1,7 +1,7 @@ - control_evaluator + autoware_control_evaluator 0.1.0 ROS 2 node for evaluating control Daniel SANCHEZ @@ -14,10 +14,14 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils + autoware_planning_msgs + autoware_universe_utils diagnostic_msgs pluginlib rclcpp rclcpp_components + ament_cmake_ros ament_lint_auto diff --git a/evaluator/control_evaluator/param/control_evaluator.defaults.yaml b/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml similarity index 100% rename from evaluator/control_evaluator/param/control_evaluator.defaults.yaml rename to evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp new file mode 100644 index 0000000000000..ce749e6e8d5eb --- /dev/null +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -0,0 +1,180 @@ +// Copyright 2024 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 "autoware/control_evaluator/control_evaluator_node.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace control_diagnostics +{ +controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_options) +: Node("control_evaluator", node_options) +{ + using std::placeholders::_1; + control_diag_sub_ = create_subscription( + "~/input/diagnostics", 1, std::bind(&controlEvaluatorNode::onDiagnostics, this, _1)); + + // Publisher + metrics_pub_ = create_publisher("~/metrics", 1); + + // Timer callback to publish evaluator diagnostics + using namespace std::literals::chrono_literals; + timer_ = + rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this)); +} + +void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) +{ + constexpr double KEEP_TIME = 1.0; + diag_queue_.erase( + std::remove_if( + diag_queue_.begin(), diag_queue_.end(), + [stamp](const std::pair & p) { + return (stamp - p.second).seconds() > KEEP_TIME; + }), + diag_queue_.end()); +} + +void controlEvaluatorNode::removeDiagnosticsByName(const std::string & name) +{ + diag_queue_.erase( + std::remove_if( + diag_queue_.begin(), diag_queue_.end(), + [&name](const std::pair & p) { + return p.first.name.find(name) != std::string::npos; + }), + diag_queue_.end()); +} + +void controlEvaluatorNode::addDiagnostic( + const diagnostic_msgs::msg::DiagnosticStatus & diag, const rclcpp::Time & stamp) +{ + diag_queue_.push_back(std::make_pair(diag, stamp)); +} + +void controlEvaluatorNode::updateDiagnosticQueue( + const DiagnosticArray & input_diagnostics, const std::string & function, + const rclcpp::Time & stamp) +{ + const auto it = std::find_if( + input_diagnostics.status.begin(), input_diagnostics.status.end(), + [&function](const diagnostic_msgs::msg::DiagnosticStatus & diag) { + return diag.name.find(function) != std::string::npos; + }); + if (it != input_diagnostics.status.end()) { + removeDiagnosticsByName(it->name); + addDiagnostic(*it, input_diagnostics.header.stamp); + } + + removeOldDiagnostics(stamp); +} + +void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) +{ + // add target diagnostics to the queue and remove old ones + for (const auto & function : target_functions_) { + updateDiagnosticQueue(*diag_msg, function, now()); + } +} + +DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag) +{ + DiagnosticStatus status; + status.level = status.OK; + status.name = diag.name; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "decision"; + const bool is_emergency_brake = (diag.level == DiagnosticStatus::ERROR); + key_value.value = (is_emergency_brake) ? "stop" : "none"; + status.values.push_back(key_value); + + return status; +} + +DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus( + const Trajectory & traj, const Point & ego_point) +{ + const double lateral_deviation = metrics::calcLateralDeviation(traj, ego_point); + + DiagnosticStatus status; + status.level = status.OK; + status.name = "lateral_deviation"; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metric_value"; + key_value.value = std::to_string(lateral_deviation); + status.values.push_back(key_value); + + return status; +} + +DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus( + const Trajectory & traj, const Pose & ego_pose) +{ + const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose); + + DiagnosticStatus status; + status.level = status.OK; + status.name = "yaw_deviation"; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metric_value"; + key_value.value = std::to_string(yaw_deviation); + status.values.push_back(key_value); + + return status; +} + +void controlEvaluatorNode::onTimer() +{ + DiagnosticArray metrics_msg; + const auto traj = traj_sub_.takeData(); + const auto odom = odometry_sub_.takeData(); + + // generate decision diagnostics from input diagnostics + for (const auto & function : target_functions_) { + const auto it = std::find_if( + diag_queue_.begin(), diag_queue_.end(), + [&function](const std::pair & p) { + return p.first.name.find(function) != std::string::npos; + }); + if (it == diag_queue_.end()) { + continue; + } + // generate each decision diagnostics + // - AEB decision + if (it->first.name.find("autonomous_emergency_braking") != std::string::npos) { + metrics_msg.status.push_back(generateAEBDiagnosticStatus(it->first)); + } + } + + // calculate deviation metrics + if (odom && traj && !traj->points.empty()) { + const Pose ego_pose = odom->pose.pose; + metrics_msg.status.push_back( + generateLateralDeviationDiagnosticStatus(*traj, ego_pose.position)); + metrics_msg.status.push_back(generateYawDeviationDiagnosticStatus(*traj, ego_pose)); + } + + metrics_msg.header.stamp = now(); + metrics_pub_->publish(metrics_msg); +} +} // namespace control_diagnostics + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::controlEvaluatorNode) diff --git a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp new file mode 100644 index 0000000000000..689291da09f6d --- /dev/null +++ b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp @@ -0,0 +1,42 @@ +// Copyright 2024 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 "autoware/control_evaluator/metrics/deviation_metrics.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" + +namespace control_diagnostics +{ +namespace metrics +{ +using autoware_planning_msgs::msg::Trajectory; + +double calcLateralDeviation(const Trajectory & traj, const Point & point) +{ + const size_t nearest_index = autoware::motion_utils::findNearestIndex(traj.points, point); + return std::abs( + autoware::universe_utils::calcLateralDeviation(traj.points[nearest_index].pose, point)); +} + +double calcYawDeviation(const Trajectory & traj, const Pose & pose) +{ + const size_t nearest_index = autoware::motion_utils::findNearestIndex(traj.points, pose.position); + return std::abs( + autoware::universe_utils::calcYawDeviation(traj.points[nearest_index].pose, pose)); +} + +} // namespace metrics +} // namespace control_diagnostics diff --git a/evaluator/autoware_planning_evaluator/CMakeLists.txt b/evaluator/autoware_planning_evaluator/CMakeLists.txt new file mode 100644 index 0000000000000..848b10e392316 --- /dev/null +++ b/evaluator/autoware_planning_evaluator/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_planning_evaluator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(pluginlib REQUIRED) + +ament_auto_add_library(planning_evaluator_node SHARED + DIRECTORY src +) + +rclcpp_components_register_node(planning_evaluator_node + PLUGIN "planning_diagnostics::PlanningEvaluatorNode" + EXECUTABLE planning_evaluator +) + +rclcpp_components_register_node(planning_evaluator_node + PLUGIN "planning_diagnostics::MotionEvaluatorNode" + EXECUTABLE motion_evaluator +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_planning_evaluator + test/test_planning_evaluator_node.cpp + ) + target_link_libraries(test_planning_evaluator + planning_evaluator_node + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) diff --git a/evaluator/planning_evaluator/README.md b/evaluator/autoware_planning_evaluator/README.md similarity index 100% rename from evaluator/planning_evaluator/README.md rename to evaluator/autoware_planning_evaluator/README.md diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp similarity index 91% rename from evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp rename to evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp index 02464207801cb..8d7cfbd30d604 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#define PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#include "planning_evaluator/stat.hpp" +#include "autoware/planning_evaluator/stat.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -80,4 +80,4 @@ Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose); } // namespace metrics } // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#endif // AUTOWARE__PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp similarity index 96% rename from evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp rename to evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp index e1ec69dbaef5c..c0313ed0727dd 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_EVALUATOR__METRICS__METRIC_HPP_ -#define PLANNING_EVALUATOR__METRICS__METRIC_HPP_ +#ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__METRIC_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__METRICS__METRIC_HPP_ #include #include @@ -131,4 +131,4 @@ static struct CheckCorrectMaps } // namespace details } // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__METRICS__METRIC_HPP_ +#endif // AUTOWARE__PLANNING_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp similarity index 83% rename from evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp rename to evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp index e3280187ccef7..2ade316ba7ad5 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ -#define PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ +#ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ -#include "planning_evaluator/stat.hpp" +#include "autoware/planning_evaluator/stat.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -40,4 +40,4 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons } // namespace utils } // namespace metrics } // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ +#endif // AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp similarity index 86% rename from evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp rename to evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp index 062ace8747253..c4d468b4dacd5 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ -#define PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ +#ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ -#include "planning_evaluator/stat.hpp" +#include "autoware/planning_evaluator/stat.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -48,4 +48,4 @@ Stat calcTimeToCollision( } // namespace metrics } // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ +#endif // AUTOWARE__PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp similarity index 83% rename from evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp rename to evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp index e6ae9222d4bb9..f1e93380d34d6 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ -#define PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ +#ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ -#include "planning_evaluator/stat.hpp" +#include "autoware/planning_evaluator/stat.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -44,4 +44,4 @@ Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & tr } // namespace metrics } // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ +#endif // AUTOWARE__PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp similarity index 90% rename from evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp rename to evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp index e88864016ca84..14a6a5c451619 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ -#define PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ +#ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ -#include "planning_evaluator/stat.hpp" +#include "autoware/planning_evaluator/stat.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -87,4 +87,4 @@ Stat calcTrajectoryJerk(const Trajectory & traj); } // namespace metrics } // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ +#endif // AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp similarity index 90% rename from evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp rename to evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp index 99ec5b787706b..f500a435edba1 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ -#define PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ -#include "planning_evaluator/metrics/metric.hpp" -#include "planning_evaluator/parameters.hpp" -#include "planning_evaluator/stat.hpp" +#ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ +#include "autoware/planning_evaluator/metrics/metric.hpp" +#include "autoware/planning_evaluator/parameters.hpp" +#include "autoware/planning_evaluator/stat.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" @@ -104,4 +104,4 @@ class MetricsCalculator } // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ +#endif // AUTOWARE__PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp similarity index 86% rename from evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp rename to evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp index 8e2561e3eaf0f..ed9c975436ba5 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ -#define PLANNING_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ +#ifndef AUTOWARE__PLANNING_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ -#include "planning_evaluator/metrics_calculator.hpp" -#include "planning_evaluator/stat.hpp" +#include "autoware/planning_evaluator/metrics_calculator.hpp" +#include "autoware/planning_evaluator/stat.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -71,4 +71,4 @@ class MotionEvaluatorNode : public rclcpp::Node }; } // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ +#endif // AUTOWARE__PLANNING_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp similarity index 83% rename from evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp rename to evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp index fb4acb53888f2..cd894fecc2331 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_EVALUATOR__PARAMETERS_HPP_ -#define PLANNING_EVALUATOR__PARAMETERS_HPP_ +#ifndef AUTOWARE__PLANNING_EVALUATOR__PARAMETERS_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__PARAMETERS_HPP_ -#include "planning_evaluator/metrics/metric.hpp" +#include "autoware/planning_evaluator/metrics/metric.hpp" #include @@ -46,4 +46,4 @@ struct Parameters } // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__PARAMETERS_HPP_ +#endif // AUTOWARE__PLANNING_EVALUATOR__PARAMETERS_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp similarity index 92% rename from evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp rename to evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index e02f833a67291..0121425c15d1d 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ -#define PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ +#ifndef AUTOWARE__PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ -#include "planning_evaluator/metrics_calculator.hpp" -#include "planning_evaluator/stat.hpp" +#include "autoware/planning_evaluator/metrics_calculator.hpp" +#include "autoware/planning_evaluator/stat.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -119,4 +119,4 @@ class PlanningEvaluatorNode : public rclcpp::Node }; } // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ +#endif // AUTOWARE__PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/stat.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/stat.hpp similarity index 93% rename from evaluator/planning_evaluator/include/planning_evaluator/stat.hpp rename to evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/stat.hpp index ce03aea7669b4..5f63f29a96f26 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/stat.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/stat.hpp @@ -15,8 +15,8 @@ #include #include -#ifndef PLANNING_EVALUATOR__STAT_HPP_ -#define PLANNING_EVALUATOR__STAT_HPP_ +#ifndef AUTOWARE__PLANNING_EVALUATOR__STAT_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__STAT_HPP_ namespace planning_diagnostics { @@ -90,4 +90,4 @@ std::ostream & operator<<(std::ostream & os, const Stat & stat) } // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__STAT_HPP_ +#endif // AUTOWARE__PLANNING_EVALUATOR__STAT_HPP_ diff --git a/evaluator/planning_evaluator/launch/motion_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/motion_evaluator.launch.xml similarity index 57% rename from evaluator/planning_evaluator/launch/motion_evaluator.launch.xml rename to evaluator/autoware_planning_evaluator/launch/motion_evaluator.launch.xml index d6cf76da860dc..6558d0b568c94 100644 --- a/evaluator/planning_evaluator/launch/motion_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/motion_evaluator.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml similarity index 88% rename from evaluator/planning_evaluator/launch/planning_evaluator.launch.xml rename to evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml index 4f36a614b5d09..2e358f6272379 100644 --- a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -7,8 +7,8 @@ - - + + diff --git a/evaluator/planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml similarity index 90% rename from evaluator/planning_evaluator/package.xml rename to evaluator/autoware_planning_evaluator/package.xml index cf91d7077d609..5bd500f200eac 100644 --- a/evaluator/planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -1,7 +1,7 @@ - planning_evaluator + autoware_planning_evaluator 0.1.0 ROS 2 node for evaluating planners Maxime CLEMENT @@ -13,19 +13,19 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs + autoware_universe_utils diagnostic_msgs eigen geometry_msgs - motion_utils nav_msgs pluginlib rclcpp rclcpp_components tf2 tf2_ros - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml b/evaluator/autoware_planning_evaluator/param/planning_evaluator.defaults.yaml similarity index 100% rename from evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml rename to evaluator/autoware_planning_evaluator/param/planning_evaluator.defaults.yaml diff --git a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp similarity index 68% rename from evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp rename to evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index e403b8415938d..8ce8a009652d8 100644 --- a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/planning_evaluator/metrics/deviation_metrics.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" namespace planning_diagnostics { @@ -37,9 +37,10 @@ Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & tra * need more precise calculation, e.g., lateral distance from spline of the reference traj */ for (TrajectoryPoint p : traj.points) { - const size_t nearest_index = motion_utils::findNearestIndex(ref.points, p.pose.position); - stat.add( - tier4_autoware_utils::calcLateralDeviation(ref.points[nearest_index].pose, p.pose.position)); + const size_t nearest_index = + autoware::motion_utils::findNearestIndex(ref.points, p.pose.position); + stat.add(autoware::universe_utils::calcLateralDeviation( + ref.points[nearest_index].pose, p.pose.position)); } return stat; } @@ -56,8 +57,9 @@ Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) * need more precise calculation, e.g., yaw distance from spline of the reference traj */ for (TrajectoryPoint p : traj.points) { - const size_t nearest_index = motion_utils::findNearestIndex(ref.points, p.pose.position); - stat.add(tier4_autoware_utils::calcYawDeviation(ref.points[nearest_index].pose, p.pose)); + const size_t nearest_index = + autoware::motion_utils::findNearestIndex(ref.points, p.pose.position); + stat.add(autoware::universe_utils::calcYawDeviation(ref.points[nearest_index].pose, p.pose)); } return stat; } @@ -72,7 +74,8 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr // TODO(Maxime CLEMENT) need more precise calculation for (TrajectoryPoint p : traj.points) { - const size_t nearest_index = motion_utils::findNearestIndex(ref.points, p.pose.position); + const size_t nearest_index = + autoware::motion_utils::findNearestIndex(ref.points, p.pose.position); stat.add(p.longitudinal_velocity_mps - ref.points[nearest_index].longitudinal_velocity_mps); } return stat; @@ -81,21 +84,21 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) { Stat stat; - stat.add(std::abs(tier4_autoware_utils::calcLongitudinalDeviation(base_pose, target_point))); + stat.add(std::abs(autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point))); return stat; } Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point) { Stat stat; - stat.add(std::abs(tier4_autoware_utils::calcLateralDeviation(base_pose, target_point))); + stat.add(std::abs(autoware::universe_utils::calcLateralDeviation(base_pose, target_point))); return stat; } Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose) { Stat stat; - stat.add(std::abs(tier4_autoware_utils::calcYawDeviation(base_pose, target_pose))); + stat.add(std::abs(autoware::universe_utils::calcYawDeviation(base_pose, target_pose))); return stat; } } // namespace metrics diff --git a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp similarity index 88% rename from evaluator/planning_evaluator/src/metrics/metrics_utils.cpp rename to evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp index 974f30223bff8..e17cfd98b27da 100644 --- a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_evaluator/metrics/trajectory_metrics.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" namespace planning_diagnostics { namespace metrics { namespace utils { +using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::calcDistance2d; size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance) { diff --git a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp similarity index 93% rename from evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp rename to evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp index af4508a370ab6..3cdaf3d7eaaae 100644 --- a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_evaluator/metrics/obstacle_metrics.hpp" +#include "autoware/planning_evaluator/metrics/obstacle_metrics.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -27,8 +27,8 @@ namespace planning_diagnostics { namespace metrics { +using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::calcDistance2d; Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj) { diff --git a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp similarity index 68% rename from evaluator/planning_evaluator/src/metrics/stability_metrics.cpp rename to evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index c963c5e46aa43..4d1c02faa406f 100644 --- a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_evaluator/metrics/stability_metrics.hpp" +#include "autoware/planning_evaluator/metrics/stability_metrics.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -41,7 +41,8 @@ Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & tr for (size_t i = 0; i < traj1.points.size(); ++i) { for (size_t j = 0; j < traj2.points.size(); ++j) { - const double dist = tier4_autoware_utils::calcDistance2d(traj1.points[i], traj2.points[j]); + const double dist = + autoware::universe_utils::calcDistance2d(traj1.points[i], traj2.points[j]); if (i > 0 && j > 0) { ca(i, j) = std::max(std::min(ca(i - 1, j), std::min(ca(i - 1, j - 1), ca(i, j - 1))), dist); } else if (i > 0 /*&& j == 0*/) { @@ -64,27 +65,29 @@ Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & tr return stat; } for (const auto & point : traj2.points) { - const auto p0 = tier4_autoware_utils::getPoint(point); + const auto p0 = autoware::universe_utils::getPoint(point); // find nearest segment - const size_t nearest_segment_idx = motion_utils::findNearestSegmentIndex(traj1.points, p0); + const size_t nearest_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(traj1.points, p0); double dist; // distance to segment if ( nearest_segment_idx == traj1.points.size() - 2 && - motion_utils::calcLongitudinalOffsetToSegment(traj1.points, nearest_segment_idx, p0) > - tier4_autoware_utils::calcDistance2d( + autoware::motion_utils::calcLongitudinalOffsetToSegment( + traj1.points, nearest_segment_idx, p0) > + autoware::universe_utils::calcDistance2d( traj1.points[nearest_segment_idx], traj1.points[nearest_segment_idx + 1])) { // distance to last point - dist = tier4_autoware_utils::calcDistance2d(traj1.points.back(), p0); + dist = autoware::universe_utils::calcDistance2d(traj1.points.back(), p0); } else if ( // NOLINT - nearest_segment_idx == 0 && - motion_utils::calcLongitudinalOffsetToSegment(traj1.points, nearest_segment_idx, p0) <= 0) { + nearest_segment_idx == 0 && autoware::motion_utils::calcLongitudinalOffsetToSegment( + traj1.points, nearest_segment_idx, p0) <= 0) { // distance to first point - dist = tier4_autoware_utils::calcDistance2d(traj1.points.front(), p0); + dist = autoware::universe_utils::calcDistance2d(traj1.points.front(), p0); } else { // orthogonal distance - const auto p1 = tier4_autoware_utils::getPoint(traj1.points[nearest_segment_idx]); - const auto p2 = tier4_autoware_utils::getPoint(traj1.points[nearest_segment_idx + 1]); + const auto p1 = autoware::universe_utils::getPoint(traj1.points[nearest_segment_idx]); + const auto p2 = autoware::universe_utils::getPoint(traj1.points[nearest_segment_idx + 1]); dist = std::abs((p2.x - p1.x) * (p1.y - p0.y) - (p1.x - p0.x) * (p2.y - p1.y)) / std::sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y)); } diff --git a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp similarity index 94% rename from evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp rename to evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp index 53265872bffa8..4526865ced97d 100644 --- a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_evaluator/metrics/trajectory_metrics.hpp" +#include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" -#include "planning_evaluator/metrics/metrics_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/planning_evaluator/metrics/metrics_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" namespace planning_diagnostics { namespace metrics { -using tier4_autoware_utils::calcCurvature; -using tier4_autoware_utils::calcDistance2d; +using autoware::universe_utils::calcCurvature; +using autoware::universe_utils::calcDistance2d; Stat calcTrajectoryInterval(const Trajectory & traj) { diff --git a/evaluator/planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp similarity index 88% rename from evaluator/planning_evaluator/src/metrics_calculator.cpp rename to evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 38113513dbad2..8658a666b4976 100644 --- a/evaluator/planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_evaluator/metrics_calculator.hpp" +#include "autoware/planning_evaluator/metrics_calculator.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "planning_evaluator/metrics/deviation_metrics.hpp" -#include "planning_evaluator/metrics/obstacle_metrics.hpp" -#include "planning_evaluator/metrics/stability_metrics.hpp" -#include "planning_evaluator/metrics/trajectory_metrics.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/planning_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/planning_evaluator/metrics/obstacle_metrics.hpp" +#include "autoware/planning_evaluator/metrics/stability_metrics.hpp" +#include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" namespace planning_diagnostics { std::optional> MetricsCalculator::calculate( @@ -122,7 +122,8 @@ Trajectory MetricsCalculator::getLookaheadTrajectory( return traj; } - const auto ego_index = motion_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); + const auto ego_index = + autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); Trajectory lookahead_traj; lookahead_traj.header = traj.header; double dist = 0.0; @@ -131,7 +132,7 @@ Trajectory MetricsCalculator::getLookaheadTrajectory( auto prev_point_it = curr_point_it; while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) { lookahead_traj.points.push_back(*curr_point_it); - const auto d = tier4_autoware_utils::calcDistance2d( + const auto d = autoware::universe_utils::calcDistance2d( prev_point_it->pose.position, curr_point_it->pose.position); dist += d; if (prev_point_it->longitudinal_velocity_mps != 0.0) { diff --git a/evaluator/planning_evaluator/src/motion_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp similarity index 98% rename from evaluator/planning_evaluator/src/motion_evaluator_node.cpp rename to evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp index 9a103890d53ac..a5320ef28cbec 100644 --- a/evaluator/planning_evaluator/src/motion_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_evaluator/motion_evaluator_node.hpp" +#include "autoware/planning_evaluator/motion_evaluator_node.hpp" #include #include diff --git a/evaluator/planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp similarity index 99% rename from evaluator/planning_evaluator/src/planning_evaluator_node.cpp rename to evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 039991d54a846..103e14c73f26d 100644 --- a/evaluator/planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_evaluator/planning_evaluator_node.hpp" +#include "autoware/planning_evaluator/planning_evaluator_node.hpp" #include "boost/lexical_cast.hpp" diff --git a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp similarity index 98% rename from evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp rename to evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp index e87b5c8ad35b7..14f8338acfed4 100644 --- a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp @@ -18,7 +18,7 @@ #include "rclcpp/time.hpp" #include "tf2_ros/transform_broadcaster.h" -#include +#include #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -50,7 +50,8 @@ class EvalTest : public ::testing::Test rclcpp::init(0, nullptr); rclcpp::NodeOptions options; - const auto share_dir = ament_index_cpp::get_package_share_directory("planning_evaluator"); + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_planning_evaluator"); options.arguments( {"--ros-args", "--params-file", share_dir + "/param/planning_evaluator.defaults.yaml"}); diff --git a/evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp b/evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp deleted file mode 100644 index 3a3ba47d88e03..0000000000000 --- a/evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2024 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 CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ -#define CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "diagnostic_msgs/msg/diagnostic_array.hpp" - -#include -#include -#include -#include -#include - -namespace control_diagnostics -{ - -using diagnostic_msgs::msg::DiagnosticArray; -using diagnostic_msgs::msg::DiagnosticStatus; - -/** - * @brief Node for control evaluation - */ -class controlEvaluatorNode : public rclcpp::Node -{ -public: - explicit controlEvaluatorNode(const rclcpp::NodeOptions & node_options); - - /** - * @brief publish the given metric statistic - */ - DiagnosticStatus generateDiagnosticStatus(const bool is_emergency_brake) const; - void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); - void onTimer(); - -private: - rclcpp::Subscription::SharedPtr control_diag_sub_; - rclcpp::Publisher::SharedPtr metrics_pub_; - - // Calculator - // Metrics - std::deque stamps_; - DiagnosticArray metrics_msg_; - rclcpp::TimerBase::SharedPtr timer_; -}; -} // namespace control_diagnostics - -#endif // CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ diff --git a/evaluator/control_evaluator/launch/control_evaluator.launch.xml b/evaluator/control_evaluator/launch/control_evaluator.launch.xml deleted file mode 100644 index 75012791888a6..0000000000000 --- a/evaluator/control_evaluator/launch/control_evaluator.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/evaluator/control_evaluator/src/control_evaluator_node.cpp b/evaluator/control_evaluator/src/control_evaluator_node.cpp deleted file mode 100644 index d575a35a2389f..0000000000000 --- a/evaluator/control_evaluator/src/control_evaluator_node.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright 2024 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 "control_evaluator/control_evaluator_node.hpp" - -#include -#include -#include -#include -#include -#include -#include - -namespace control_diagnostics -{ -controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_options) -: Node("control_evaluator", node_options) -{ - using std::placeholders::_1; - - control_diag_sub_ = create_subscription( - "~/input/diagnostics", 1, std::bind(&controlEvaluatorNode::onDiagnostics, this, _1)); - - // Publisher - metrics_pub_ = create_publisher("~/metrics", 1); - - // Timer callback to publish evaluator diagnostics - using namespace std::literals::chrono_literals; - timer_ = - rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this)); -} - -DiagnosticStatus controlEvaluatorNode::generateDiagnosticStatus(const bool is_emergency_brake) const -{ - DiagnosticStatus status; - status.level = status.OK; - status.name = "autonomous_emergency_braking"; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "decision"; - key_value.value = (is_emergency_brake) ? "stop" : "none"; - status.values.push_back(key_value); - return status; -} - -void controlEvaluatorNode::onTimer() -{ - if (!metrics_msg_.status.empty()) { - metrics_pub_->publish(metrics_msg_); - metrics_msg_.status.clear(); - } -} - -void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) -{ - const auto start = now(); - const auto aeb_status = - std::find_if(diag_msg->status.begin(), diag_msg->status.end(), [](const auto & status) { - const bool aeb_found = status.name.find("autonomous_emergency_braking") != std::string::npos; - return aeb_found; - }); - - if (aeb_status == diag_msg->status.end()) return; - - const bool is_emergency_brake = (aeb_status->level == DiagnosticStatus::ERROR); - metrics_msg_.header.stamp = now(); - metrics_msg_.status.emplace_back(generateDiagnosticStatus(is_emergency_brake)); - - const auto runtime = (now() - start).seconds(); - RCLCPP_DEBUG(get_logger(), "control evaluation calculation time: %2.2f ms", runtime * 1e3); -} - -} // namespace control_diagnostics - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::controlEvaluatorNode) diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/parameters.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/parameters.hpp index a2de980336e5d..dada46f44292f 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/parameters.hpp +++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/parameters.hpp @@ -27,7 +27,7 @@ namespace kinematic_diagnostics struct Parameters { std::array(Metric::SIZE)> metrics{}; // default values to false -}; // struct Parameters +}; // struct Parameters } // namespace kinematic_diagnostics diff --git a/evaluator/kinematic_evaluator/package.xml b/evaluator/kinematic_evaluator/package.xml index 1b62b06e57c9f..3642e0cf94604 100644 --- a/evaluator/kinematic_evaluator/package.xml +++ b/evaluator/kinematic_evaluator/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_planning_msgs + autoware_universe_utils diagnostic_msgs eigen geometry_msgs @@ -26,7 +27,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils ament_cmake_gtest ament_lint_auto diff --git a/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp b/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp index 314bace43eb59..51e6113aea999 100644 --- a/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp +++ b/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp @@ -67,9 +67,7 @@ class EvalTest : public ::testing::Test tf_broadcaster_ = std::make_unique(dummy_node); } - ~EvalTest() override - { /*rclcpp::shutdown();*/ - } + ~EvalTest() override { /*rclcpp::shutdown();*/ } void setTargetMetric(kinematic_diagnostics::Metric metric) { diff --git a/evaluator/localization_evaluator/include/localization_evaluator/parameters.hpp b/evaluator/localization_evaluator/include/localization_evaluator/parameters.hpp index 46cdf2e77bf11..d8328fe62b3ed 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/parameters.hpp +++ b/evaluator/localization_evaluator/include/localization_evaluator/parameters.hpp @@ -27,7 +27,7 @@ namespace localization_diagnostics struct Parameters { std::array(Metric::SIZE)> metrics{}; // default values to false -}; // struct Parameters +}; // struct Parameters } // namespace localization_diagnostics diff --git a/evaluator/localization_evaluator/package.xml b/evaluator/localization_evaluator/package.xml index ac52f9fe94d5e..8d3b788a2b8d9 100644 --- a/evaluator/localization_evaluator/package.xml +++ b/evaluator/localization_evaluator/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_planning_msgs + autoware_universe_utils diagnostic_msgs eigen geometry_msgs @@ -22,7 +23,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils ament_cmake_gtest ament_lint_auto diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp index b05bb5e1d8ad3..9c49605d944b5 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -30,11 +30,11 @@ namespace marker_utils { +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index d64d3a5ec4957..082a9bc2d1366 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -17,14 +17,15 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_perception_msgs + autoware_universe_utils autoware_vehicle_info_utils diagnostic_msgs eigen geometry_msgs glog lanelet2_extension - motion_utils nav_msgs object_recognition_utils pluginlib @@ -32,7 +33,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp index 047d278334eb1..5859bf055d4aa 100644 --- a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp @@ -14,17 +14,17 @@ #include "perception_online_evaluator/metrics/detection_count.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include "perception_online_evaluator/utils/objects_filtering.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include +#include namespace perception_diagnostics { namespace metrics { -using tier4_autoware_utils::toHexString; +using autoware::universe_utils::toHexString; bool isCountObject( const std::uint8_t classification, const std::unordered_map & params) diff --git a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp index 7a9435444c065..d5ac88613fa83 100644 --- a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp @@ -14,10 +14,10 @@ #include "perception_online_evaluator/metrics/deviation_metrics.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" -#include +#include namespace perception_diagnostics { @@ -30,9 +30,10 @@ double calcLateralDeviation(const std::vector & ref_path, const Pose & tar return 0.0; } - const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); + const size_t nearest_index = + autoware::motion_utils::findNearestIndex(ref_path, target_pose.position); return std::abs( - tier4_autoware_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position)); + autoware::universe_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position)); } double calcYawDeviation(const std::vector & ref_path, const Pose & target_pose) @@ -41,8 +42,9 @@ double calcYawDeviation(const std::vector & ref_path, const Pose & target_ return 0.0; } - const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); - return std::abs(tier4_autoware_utils::calcYawDeviation(ref_path[nearest_index], target_pose)); + const size_t nearest_index = + autoware::motion_utils::findNearestIndex(ref_path, target_pose.position); + return std::abs(autoware::universe_utils::calcYawDeviation(ref_path[nearest_index], target_pose)); } std::vector calcPredictedPathDeviation( @@ -54,9 +56,9 @@ std::vector calcPredictedPathDeviation( return {}; } for (const Pose & p : pred_path.path) { - const size_t nearest_index = motion_utils::findNearestIndex(ref_path, p.position); + const size_t nearest_index = autoware::motion_utils::findNearestIndex(ref_path, p.position); deviations.push_back( - tier4_autoware_utils::calcDistance2d(ref_path[nearest_index].position, p.position)); + autoware::universe_utils::calcDistance2d(ref_path[nearest_index].position, p.position)); } return deviations; diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index cc455445ca9f8..212af8711a62e 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -14,17 +14,17 @@ #include "perception_online_evaluator/metrics_calculator.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_recognition_utils/object_classification.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include +#include namespace perception_diagnostics { +using autoware::universe_utils::inverseTransformPoint; using object_recognition_utils::convertLabelToString; -using tier4_autoware_utils::inverseTransformPoint; std::optional MetricsCalculator::calculate(const Metric & metric) const { @@ -238,7 +238,7 @@ MetricStatMap MetricsCalculator::calcLateralDeviationMetrics( Stat stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + const auto uuid = autoware::universe_utils::toHexString(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -267,7 +267,7 @@ MetricStatMap MetricsCalculator::calcYawDeviationMetrics( Stat stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + const auto uuid = autoware::universe_utils::toHexString(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -326,7 +326,7 @@ PredictedPathDeviationMetrics MetricsCalculator::calcPredictedPathDeviationMetri const auto stamp = objects.header.stamp; for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + const auto uuid = autoware::universe_utils::toHexString(object.object_id); const auto predicted_paths = object.kinematics.predicted_paths; for (size_t i = 0; i < predicted_paths.size(); i++) { const auto predicted_path = predicted_paths[i]; @@ -350,7 +350,7 @@ PredictedPathDeviationMetrics MetricsCalculator::calcPredictedPathDeviationMetri const auto history_pose = history_object.kinematics.initial_pose_with_covariance.pose; const Pose & p = predicted_path.path[j]; const double distance = - tier4_autoware_utils::calcDistance2d(p.position, history_pose.position); + autoware::universe_utils::calcDistance2d(p.position, history_pose.position); deviation_map_for_paths[uuid][i].push_back(distance); // Save debug information @@ -424,7 +424,7 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + const auto uuid = autoware::universe_utils::toHexString(object.object_id); if (!hasPassedTime(uuid, stamp)) { continue; } @@ -444,7 +444,7 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas tf2::getYaw(previous_object.kinematics.initial_pose_with_covariance.pose.orientation); // Calculate the absolute difference between current_yaw and previous_yaw const double yaw_diff = - std::abs(tier4_autoware_utils::normalizeRadian(current_yaw - previous_yaw)); + std::abs(autoware::universe_utils::normalizeRadian(current_yaw - previous_yaw)); // The yaw difference is flipped if the angle is larger than 90 degrees const double yaw_diff_flip_fixed = std::min(yaw_diff, M_PI - yaw_diff); const double yaw_rate = yaw_diff_flip_fixed / time_diff; @@ -494,7 +494,7 @@ void MetricsCalculator::setPredictedObjects( // store objects to check deviation { - using tier4_autoware_utils::toHexString; + using autoware::universe_utils::toHexString; for (const auto & object : objects.objects) { std::string uuid = toHexString(object.object_id); updateObjects(uuid, current_stamp_, object); @@ -557,7 +557,7 @@ void MetricsCalculator::updateHistoryPath() const auto current_pose = object.kinematics.initial_pose_with_covariance.pose; const auto prev_pose = prev_object.kinematics.initial_pose_with_covariance.pose; const auto velocity = - tier4_autoware_utils::calcDistance2d(current_pose.position, prev_pose.position) / + autoware::universe_utils::calcDistance2d(current_pose.position, prev_pose.position) / time_diff; if (velocity < parameters_->stopped_velocity_threshold) { continue; @@ -603,9 +603,9 @@ std::vector MetricsCalculator::generateHistoryPathWithPrev( std::vector MetricsCalculator::averageFilterPath( const std::vector & path, const size_t window_size) const { - using tier4_autoware_utils::calcAzimuthAngle; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createQuaternionFromYaw; + using autoware::universe_utils::calcAzimuthAngle; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::createQuaternionFromYaw; std::vector filtered_path; filtered_path.reserve(path.size()); // Reserve space to avoid reallocations @@ -645,7 +645,7 @@ std::vector MetricsCalculator::averageFilterPath( if (i > 0) { const double azimuth = calcAzimuthAngle(path.at(i - 1).position, path.at(i).position); const double yaw = tf2::getYaw(path.at(i).orientation); - if (tier4_autoware_utils::normalizeRadian(yaw - azimuth) > M_PI_2) { + if (autoware::universe_utils::normalizeRadian(yaw - azimuth) > M_PI_2) { continue; } } @@ -662,7 +662,7 @@ std::vector MetricsCalculator::averageFilterPath( const double azimuth_to_prev = calcAzimuthAngle((it - 2)->position, (it - 1)->position); const double azimuth_to_current = calcAzimuthAngle((it - 1)->position, it->position); if ( - std::abs(tier4_autoware_utils::normalizeRadian(azimuth_to_prev - azimuth_to_current)) > + std::abs(autoware::universe_utils::normalizeRadian(azimuth_to_prev - azimuth_to_current)) > M_PI_2) { it = filtered_path.erase(it); continue; diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 2933afdab3d08..63d635af4ffdf 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -14,12 +14,12 @@ #include "perception_online_evaluator/perception_online_evaluator_node.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "perception_online_evaluator/utils/marker_utils.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include +#include #include "boost/lexical_cast.hpp" @@ -156,7 +156,7 @@ void PerceptionOnlineEvaluatorNode::publishDebugMarker() for (auto & marker : added.markers) { marker.lifetime = rclcpp::Duration::from_seconds(1.5); } - tier4_autoware_utils::appendMarkerArray(added, &marker); + autoware::universe_utils::appendMarkerArray(added, &marker); }; const auto & p = parameters_->debug_marker_parameters; @@ -236,7 +236,7 @@ void PerceptionOnlineEvaluatorNode::publishDebugMarker() rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParameter( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; auto & p = parameters_; @@ -305,8 +305,8 @@ rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParame void PerceptionOnlineEvaluatorNode::initParameter() { - using tier4_autoware_utils::getOrDeclareParameter; - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::getOrDeclareParameter; + using autoware::universe_utils::updateParam; auto & p = parameters_; diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp index eddeadc331101..a7a697c4efff6 100644 --- a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -14,11 +14,11 @@ #include "perception_online_evaluator/utils/marker_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" -#include -#include +#include +#include #include #include @@ -28,13 +28,13 @@ namespace marker_utils { +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; void addFootprintMarker( @@ -46,13 +46,13 @@ void addFootprintMarker( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); marker.points.push_back(marker.points.front()); } @@ -167,7 +167,7 @@ std_msgs::msg::ColorRGBA createColorFromString(const std::string & str) const auto r = (hash & 0xFF) / 255.0; const auto g = ((hash >> 8) & 0xFF) / 255.0; const auto b = ((hash >> 16) & 0xFF) / 255.0; - return tier4_autoware_utils::createMarkerColor(r, g, b, 0.5); + return autoware::universe_utils::createMarkerColor(r, g, b, 0.5); } MarkerArray createObjectPolygonMarkerArray( @@ -182,7 +182,7 @@ MarkerArray createObjectPolygonMarkerArray( const double z = object.kinematics.initial_pose_with_covariance.pose.position.z; const double height = object.shape.dimensions.z; - const auto polygon = tier4_autoware_utils::toPolygon2d( + const auto polygon = autoware::universe_utils::toPolygon2d( object.kinematics.initial_pose_with_covariance.pose, object.shape); for (const auto & p : polygon.outer()) { marker.points.push_back(createPoint(p.x(), p.y(), z - height / 2)); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 3db487f3167ae..e2ab22be2b61b 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -16,8 +16,8 @@ #include "rclcpp/time.hpp" #include +#include #include -#include #include #include @@ -43,7 +43,7 @@ using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification using nav_msgs::msg::Odometry; using TFMessage = tf2_msgs::msg::TFMessage; -using tier4_autoware_utils::generateUUID; +using autoware::universe_utils::generateUUID; constexpr double epsilon = 1e-6; diff --git a/evaluator/planning_evaluator/CMakeLists.txt b/evaluator/planning_evaluator/CMakeLists.txt deleted file mode 100644 index 4b643b3c85e44..0000000000000 --- a/evaluator/planning_evaluator/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(planning_evaluator) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(pluginlib REQUIRED) - -ament_auto_add_library(${PROJECT_NAME}_node SHARED - src/metrics_calculator.cpp - src/${PROJECT_NAME}_node.cpp - src/motion_evaluator_node.cpp - src/metrics/deviation_metrics.cpp - src/metrics/metrics_utils.cpp - src/metrics/obstacle_metrics.cpp - src/metrics/stability_metrics.cpp - src/metrics/trajectory_metrics.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "planning_diagnostics::PlanningEvaluatorNode" - EXECUTABLE ${PROJECT_NAME} -) - -rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "planning_diagnostics::MotionEvaluatorNode" - EXECUTABLE motion_evaluator -) - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_planning_evaluator_node.cpp - ) - target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME}_node - ) -endif() - -ament_auto_package( - INSTALL_TO_SHARE - param - launch -) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 02ad160164c32..1cf3cb5b44656 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -148,6 +148,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/velocity", "/vehicle/status/velocity_status"), ("~/input/imu", "/sensing/imu/imu_data"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/objects", "/perception/object_recognition/objects"), ( "~/input/predicted_trajectory", "/control/trajectory_follower/lateral/predicted_trajectory", @@ -374,12 +375,14 @@ def launch_setup(context, *args, **kwargs): # control evaluator control_evaluator_component = ComposableNode( - package="control_evaluator", + package="autoware_control_evaluator", plugin="control_diagnostics::controlEvaluatorNode", name="control_evaluator", remappings=[ ("~/input/diagnostics", "/diagnostics"), - ("~/output/metrics", "~/metrics"), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/trajectory", "/planning/scenario_planning/trajectory"), + ("~/metrics", "/diagnostic/control_evaluator/metrics"), ], ) diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index 699c39f8afa63..cf94e37955516 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -11,13 +11,13 @@ ament_cmake_auto autoware_cmake + autoware_control_evaluator autoware_external_cmd_converter autoware_external_cmd_selector autoware_lane_departure_checker autoware_shift_decider autoware_trajectory_follower_node autoware_vehicle_cmd_gate - control_evaluator ament_lint_auto autoware_lint_common diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index 225c7ab9d1146..e3ee69fb02b20 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index d0046acf294b5..150744bbae4a7 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -18,8 +18,8 @@ ament_cmake_auto autoware_cmake - ar_tag_based_localizer automatic_pose_initializer + autoware_ar_tag_based_localizer eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 7e389f5790051..228b4c89f95d3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -1,200 +1,280 @@ - - + + + + + + + + + + - + - - - + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - + + + - + + + + - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + @@ -204,38 +284,19 @@ - - - - - - - - - - - - - - + + + + + + - - - - - - - - - - - - - - + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 6294df64279a5..3c8480038f320 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -2,104 +2,105 @@ - - + + + - - - + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - + - + - + @@ -111,7 +112,7 @@ - + @@ -122,32 +123,32 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -178,32 +179,32 @@ - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - @@ -234,12 +235,4 @@ - - - - - - - - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index 274b39be681b8..ab34cc49dd2f6 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -2,6 +2,7 @@ + @@ -27,7 +28,7 @@ - + @@ -48,7 +49,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml index 0fabd5e98e45f..819a6337138a1 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -1,17 +1,19 @@ - - + + + + - + - + @@ -21,12 +23,12 @@ - + - + @@ -45,12 +47,4 @@ - - - - - - - - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml new file mode 100644 index 0000000000000..0efe7444fcba5 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_filter.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_filter.launch.xml new file mode 100644 index 0000000000000..d7bbcb01472eb --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_filter.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_validator.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_validator.launch.xml new file mode 100644 index 0000000000000..9a09ad2871e85 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_validator.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py similarity index 100% rename from launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py rename to launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml similarity index 84% rename from launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml rename to launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml index b09684281d33a..fe7ab70e9331e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml @@ -1,14 +1,17 @@ - - + - + - + + + + + @@ -33,7 +36,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml index 2f62e83ae0ef5..85b404ba89b1f 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml @@ -1,124 +1,152 @@ - - - - - + - - - - + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - + + - - - - - - - - - + + + - + - - - - - - - - - + + + + - - - - - + + + - - - - - - - - - - - - - - - - - - + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml index b4d19c9052a63..16ec8f8fbd573 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -1,150 +1,186 @@ - - - - - + - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - + + - - - - - - - - - + + + - + - - - - - - - - - + + + + - - - - - + + + - - - - - - - - - - - - - - - - - - + + + + + + - - - + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml index 0ecc89c8743e8..c82de41a28b50 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml @@ -1,89 +1,90 @@ - - - - - + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - + + - - - - - - - - - + + + - + - - - - - - - - + + + - + - - - - - + + + - - - - - - - - - - - - - - - - - - + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index cf11c65ac805c..b8172daa616fa 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -1,41 +1,50 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - + + @@ -43,9 +52,9 @@ - - - + + + diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index a307b192d7caa..25fb1e6ac6c4d 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -501,9 +501,11 @@ def launch_setup(context, *args, **kwargs): components.extend( pipeline.create_single_frame_obstacle_segmentation_components( input_topic=LaunchConfiguration("input/pointcloud"), - output_topic=pipeline.single_frame_obstacle_seg_output - if pipeline.use_single_frame_filter or pipeline.use_time_series_filter - else pipeline.output_topic, + output_topic=( + pipeline.single_frame_obstacle_seg_output + if pipeline.use_single_frame_filter or pipeline.use_time_series_filter + else pipeline.output_topic + ), ) ) @@ -512,18 +514,20 @@ def launch_setup(context, *args, **kwargs): components.extend( pipeline.create_single_frame_outlier_filter_components( input_topic=pipeline.single_frame_obstacle_seg_output, - output_topic=relay_topic - if pipeline.use_time_series_filter - else pipeline.output_topic, + output_topic=( + relay_topic if pipeline.use_time_series_filter else pipeline.output_topic + ), context=context, ) ) if pipeline.use_time_series_filter: components.extend( pipeline.create_time_series_outlier_filter_components( - input_topic=relay_topic - if pipeline.use_single_frame_filter - else pipeline.single_frame_obstacle_seg_output, + input_topic=( + relay_topic + if pipeline.use_single_frame_filter + else pipeline.single_frame_obstacle_seg_output + ), output_topic=pipeline.output_topic, ) ) diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 62404a7deb43a..8f0513bcd5446 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -77,6 +77,7 @@ + - - + + - + @@ -115,12 +116,13 @@ - + + @@ -186,25 +188,35 @@ - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -217,10 +229,15 @@ + + + + + - + @@ -238,6 +255,7 @@ + diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 1d01732454daf..e71253c8a978b 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -41,7 +41,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 9121be27825aa..3deff32d6e297 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -33,7 +33,6 @@ - @@ -168,11 +167,6 @@ value="$(eval "'$(var behavior_velocity_planner_launch_modules)' + 'autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin, '")" if="$(var launch_no_drivable_lane_module)" /> - @@ -256,7 +250,6 @@ - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 94f641200868e..ae43c2d81efb6 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -2,9 +2,9 @@ + - @@ -19,11 +19,11 @@ value="$(eval "'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::ObstacleVelocityLimiterModule, '")" if="$(var launch_obstacle_velocity_limiter_module)" /> - - - - - + @@ -140,6 +140,7 @@ + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 03f12b43a6f85..1c65c975bc5bf 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -66,6 +66,7 @@ autoware_mission_planner autoware_obstacle_cruise_planner autoware_path_optimizer + autoware_planning_evaluator autoware_planning_topic_converter autoware_planning_validator autoware_remaining_distance_time_calculator @@ -74,7 +75,6 @@ autoware_velocity_smoother glog_component obstacle_stop_planner - planning_evaluator ament_lint_auto autoware_lint_common diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 2ffd2a701fb65..97b07410d108a 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -147,7 +147,7 @@ - + diff --git a/localization/landmark_based_localizer/README.md b/localization/autoware_landmark_based_localizer/README.md similarity index 100% rename from localization/landmark_based_localizer/README.md rename to localization/autoware_landmark_based_localizer/README.md diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CMakeLists.txt similarity index 96% rename from localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CMakeLists.txt index a8435fa056847..8b2487ef79ea9 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(ar_tag_based_localizer) +project(autoware_ar_tag_based_localizer) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/README.md rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/ar_tag_image.png similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/ar_tag_image.png diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/sample_result.png similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/sample_result.png diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/sample_result_in_awsim.png similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/sample_result_in_awsim.png diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml similarity index 82% rename from localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 34602ca70daf4..dd9956acce748 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -1,5 +1,5 @@ - + @@ -14,7 +14,7 @@ - + diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml similarity index 88% rename from localization/landmark_based_localizer/ar_tag_based_localizer/package.xml rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml index 90fdd2fee31f4..bf40719d7a5e6 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml @@ -1,9 +1,9 @@ - ar_tag_based_localizer + autoware_ar_tag_based_localizer 0.0.0 - The ar_tag_based_localizer package + The autoware_ar_tag_based_localizer package Shintaro Sakoda Masahiro Sakamoto Yamato Ando @@ -18,9 +18,9 @@ ament_index_cpp aruco + autoware_landmark_manager cv_bridge diagnostic_msgs - landmark_manager lanelet2_extension localization_util rclcpp diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp similarity index 98% rename from localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index 0ea40690d9f4d..c39969fb3cc5f 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -62,7 +62,7 @@ #include #endif -#include +#include ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) : rclcpp::Node("ar_tag_based_localizer", options), cam_info_received_(false) @@ -185,7 +185,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) pose_array_msg.header.frame_id = "map"; for (const Landmark & landmark : landmarks) { const Pose detected_marker_on_map = - tier4_autoware_utils::transformPose(landmark.pose, self_pose); + autoware::universe_utils::transformPose(landmark.pose, self_pose); pose_array_msg.poses.push_back(detected_marker_on_map); } detected_tag_pose_pub_->publish(pose_array_msg); @@ -194,7 +194,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) // calc new_self_pose const Pose new_self_pose = landmark_manager_.calculate_new_self_pose(landmarks, self_pose, consider_orientation_); - const Pose diff_pose = tier4_autoware_utils::inverseTransformPose(new_self_pose, self_pose); + const Pose diff_pose = autoware::universe_utils::inverseTransformPose(new_self_pose, self_pose); const double distance = std::hypot(diff_pose.position.x, diff_pose.position.y, diff_pose.position.z); diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp similarity index 98% rename from localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index f00621b3c520a..9826c04e3a86f 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -45,7 +45,7 @@ #ifndef AR_TAG_BASED_LOCALIZER_HPP_ #define AR_TAG_BASED_LOCALIZER_HPP_ -#include "landmark_manager/landmark_manager.hpp" +#include "autoware/landmark_manager/landmark_manager.hpp" #include "localization_util/smart_pose_buffer.hpp" #include diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/test/test.cpp similarity index 96% rename from localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/test/test.cpp index d62c2b38b6cd1..295737ed723a5 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/test/test.cpp @@ -30,7 +30,7 @@ class TestArTagBasedLocalizer : public ::testing::Test void SetUp() override { const std::string yaml_path = - ament_index_cpp::get_package_share_directory("ar_tag_based_localizer") + + ament_index_cpp::get_package_share_directory("autoware_ar_tag_based_localizer") + "/config/ar_tag_based_localizer.param.yaml"; rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); diff --git a/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/CMakeLists.txt similarity index 92% rename from localization/landmark_based_localizer/landmark_manager/CMakeLists.txt rename to localization/autoware_landmark_based_localizer/autoware_landmark_manager/CMakeLists.txt index 1b57e5cc89018..6c67ff7a320e8 100644 --- a/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(landmark_manager) +project(autoware_landmark_manager) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/include/autoware/landmark_manager/landmark_manager.hpp similarity index 91% rename from localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp rename to localization/autoware_landmark_based_localizer/autoware_landmark_manager/include/autoware/landmark_manager/landmark_manager.hpp index 7c1a3f656dd66..3fc6505e72c31 100644 --- a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/include/autoware/landmark_manager/landmark_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ -#define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#ifndef AUTOWARE__LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#define AUTOWARE__LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ #include "lanelet2_extension/localization/landmark.hpp" @@ -61,4 +61,4 @@ class LandmarkManager } // namespace landmark_manager -#endif // LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#endif // AUTOWARE__LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ diff --git a/localization/landmark_based_localizer/landmark_manager/package.xml b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml similarity index 91% rename from localization/landmark_based_localizer/landmark_manager/package.xml rename to localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml index 4e080c0362c07..1471b5182def8 100644 --- a/localization/landmark_based_localizer/landmark_manager/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml @@ -1,9 +1,9 @@ - landmark_manager + autoware_landmark_manager 0.0.0 - The landmark_manager package + The autoware_landmark_manager package Shintaro Sakoda Masahiro Sakamoto Yamato Ando diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp similarity index 96% rename from localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp rename to localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp index a808a6428f682..2fa0fdbdb315d 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "landmark_manager/landmark_manager.hpp" +#include "autoware/landmark_manager/landmark_manager.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "localization_util/util_func.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -161,7 +161,7 @@ geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( // convert base_link to map const Pose detected_landmark_on_map = - tier4_autoware_utils::transformPose(detected_landmark_on_base_link, self_pose); + autoware::universe_utils::transformPose(detected_landmark_on_base_link, self_pose); // match to map if (landmarks_map_.count(landmark.id) == 0) { @@ -171,7 +171,7 @@ geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( // check all poses for (const Pose mapped_landmark_on_map : landmarks_map_.at(landmark.id)) { // check distance - const double curr_distance = tier4_autoware_utils::calcDistance3d( + const double curr_distance = autoware::universe_utils::calcDistance3d( mapped_landmark_on_map.position, detected_landmark_on_map.position); if (curr_distance > min_distance) { continue; diff --git a/localization/landmark_based_localizer/doc_image/consider_orientation.drawio.svg b/localization/autoware_landmark_based_localizer/doc_image/consider_orientation.drawio.svg similarity index 100% rename from localization/landmark_based_localizer/doc_image/consider_orientation.drawio.svg rename to localization/autoware_landmark_based_localizer/doc_image/consider_orientation.drawio.svg diff --git a/localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg b/localization/autoware_landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg similarity index 100% rename from localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg rename to localization/autoware_landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg diff --git a/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg b/localization/autoware_landmark_based_localizer/doc_image/node_diagram.drawio.svg similarity index 100% rename from localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg rename to localization/autoware_landmark_based_localizer/doc_image/node_diagram.drawio.svg diff --git a/localization/landmark_based_localizer/doc_image/principle.png b/localization/autoware_landmark_based_localizer/doc_image/principle.png similarity index 100% rename from localization/landmark_based_localizer/doc_image/principle.png rename to localization/autoware_landmark_based_localizer/doc_image/principle.png diff --git a/localization/autoware_pose_covariance_modifier/README.md b/localization/autoware_pose_covariance_modifier/README.md index 392df9caca6eb..b834bc88116a3 100644 --- a/localization/autoware_pose_covariance_modifier/README.md +++ b/localization/autoware_pose_covariance_modifier/README.md @@ -156,8 +156,8 @@ the [pose_twist_estimator.launch.xml](../../launch/tier4_localization_launch/lau The parameters are set in [config/pose_covariance_modifier.param.yaml](config/pose_covariance_modifier.param.yaml) . -{{ json_to_markdown(" -localization/autoware_pose_covariance_modifier/schema/pose_covariance_modifier.schema.json") }} +{{ json_to_markdown( + "localization/autoware_pose_covariance_modifier/schema/pose_covariance_modifier.schema.json") }} ## FAQ diff --git a/localization/autoware_pose_covariance_modifier/schema/pose_covariance_modifier.schema.json b/localization/autoware_pose_covariance_modifier/schema/pose_covariance_modifier.schema.json index 02cf301d731b1..53fc040adc026 100644 --- a/localization/autoware_pose_covariance_modifier/schema/pose_covariance_modifier.schema.json +++ b/localization/autoware_pose_covariance_modifier/schema/pose_covariance_modifier.schema.json @@ -2,66 +2,68 @@ "$schema": "http://json-schema.org/draft-07/schema#", "title": "Pose Covariance Modifier Node Parameters", "type": "object", - "actual_parameters": { - "type": "object", - "properties": { - "threshold_gnss_stddev_yaw_deg_max": { - "type": "number", - "default": 0.3, - "description": "If GNSS yaw standard deviation values are larger than this, trust only NDT" - }, - "threshold_gnss_stddev_z_max": { - "type": "number", - "default": 0.1, - "description": "If GNSS position Z standard deviation values are larger than this, trust only NDT" - }, - "threshold_gnss_stddev_xy_bound_lower": { - "type": "number", - "default": 0.1, - "description": "If GNSS position XY standard deviation values are lower than this, trust only GNSS" - }, - "threshold_gnss_stddev_xy_bound_upper": { - "type": "number", - "default": 0.25, - "description": "If GNSS position XY standard deviation values are higher than this, trust only NDT" - }, - "ndt_std_dev_bound_lower": { - "type": "number", - "default": 0.15, - "description": "Lower bound value for standard deviation of NDT positions (x, y, z) when used with GNSS" - }, - "ndt_std_dev_bound_upper": { - "type": "number", - "default": 0.3, - "description": "Upper bound value for standard deviation of NDT positions (x, y, z) when used with GNSS" - }, - "gnss_pose_timeout_sec": { - "type": "number", - "default": 1.0, - "description": "If GNSS data is not received for this duration, trust only NDT" + "definitions": { + "pose_covariance_modifier": { + "type": "object", + "properties": { + "threshold_gnss_stddev_yaw_deg_max": { + "type": "number", + "default": 0.3, + "description": "If GNSS yaw standard deviation values are larger than this, trust only NDT" + }, + "threshold_gnss_stddev_z_max": { + "type": "number", + "default": 0.1, + "description": "If GNSS position Z standard deviation values are larger than this, trust only NDT" + }, + "threshold_gnss_stddev_xy_bound_lower": { + "type": "number", + "default": 0.1, + "description": "If GNSS position XY standard deviation values are lower than this, trust only GNSS" + }, + "threshold_gnss_stddev_xy_bound_upper": { + "type": "number", + "default": 0.25, + "description": "If GNSS position XY standard deviation values are higher than this, trust only NDT" + }, + "ndt_std_dev_bound_lower": { + "type": "number", + "default": 0.15, + "description": "Lower bound value for standard deviation of NDT positions (x, y, z) when used with GNSS" + }, + "ndt_std_dev_bound_upper": { + "type": "number", + "default": 0.3, + "description": "Upper bound value for standard deviation of NDT positions (x, y, z) when used with GNSS" + }, + "gnss_pose_timeout_sec": { + "type": "number", + "default": 1.0, + "description": "If GNSS data is not received for this duration, trust only NDT" + }, + "enable_debug_topics": { + "type": "boolean", + "default": true, + "description": "Publish additional debug topics" + } }, - "enable_debug_topics": { - "type": "boolean", - "default": true, - "description": "Publish additional debug topics" - } - }, - "required": [ - "threshold_gnss_stddev_yaw_deg_max", - "threshold_gnss_stddev_z_max", - "threshold_gnss_stddev_xy_bound_lower", - "threshold_gnss_stddev_xy_bound_upper", - "gnss_pose_timeout_sec", - "enable_debug_topics" - ], - "additionalProperties": false + "required": [ + "threshold_gnss_stddev_yaw_deg_max", + "threshold_gnss_stddev_z_max", + "threshold_gnss_stddev_xy_bound_lower", + "threshold_gnss_stddev_xy_bound_upper", + "gnss_pose_timeout_sec", + "enable_debug_topics" + ], + "additionalProperties": false + } }, "properties": { "/**": { "type": "object", "properties": { "ros__parameters": { - "$ref": "#/actual_parameters" + "$ref": "#/definitions/pose_covariance_modifier" } }, "required": ["ros__parameters"], diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index b78e9b1ee0469..65003f5fe9864 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -20,10 +20,10 @@ #include "ekf_localizer/hyper_parameters.hpp" #include "ekf_localizer/warning.hpp" +#include +#include +#include #include -#include -#include -#include #include #include @@ -142,7 +142,7 @@ class EKFLocalizer : public rclcpp::Node std::shared_ptr tf_br_; //!< @brief logger configure module - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; //!< @brief extended kalman filter instance. std::unique_ptr ekf_module_; @@ -236,7 +236,7 @@ class EKFLocalizer : public rclcpp::Node const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); - tier4_autoware_utils::StopWatch stop_watch_; + autoware::universe_utils::StopWatch stop_watch_; friend class EKFLocalizerTestSuite; // for test code }; diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index dad1b0e730711..07efca6339b98 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -23,6 +23,7 @@ eigen + autoware_universe_utils diagnostic_msgs fmt geometry_msgs @@ -33,7 +34,6 @@ std_srvs tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_ros diff --git a/localization/ekf_localizer/src/covariance.cpp b/localization/ekf_localizer/src/covariance.cpp index fc16abf429d93..4655ea8a89855 100644 --- a/localization/ekf_localizer/src/covariance.cpp +++ b/localization/ekf_localizer/src/covariance.cpp @@ -14,10 +14,10 @@ #include "ekf_localizer/covariance.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "ekf_localizer/state_index.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" -using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P) { diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 8e7121ec73a12..11fa2b6713a52 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -18,11 +18,11 @@ #include "ekf_localizer/string.hpp" #include "ekf_localizer/warning_message.hpp" +#include +#include +#include #include #include -#include -#include -#include #include @@ -96,7 +96,7 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) std::shared_ptr(this, [](auto) {})); ekf_module_ = std::make_unique(warning_, params_); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); z_filter_.set_proc_dev(params_.z_filter_proc_dev); roll_filter_.set_proc_dev(params_.roll_filter_proc_dev); @@ -264,7 +264,7 @@ void EKFLocalizer::timer_tf_callback() const rclcpp::Time current_time = this->now(); geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped = tier4_autoware_utils::pose2transform( + transform_stamped = autoware::universe_utils::pose2transform( ekf_module_->get_current_pose(current_time, z, roll, pitch, false), "base_link"); transform_stamped.header.stamp = current_time; tf_br_->sendTransform(transform_stamped); @@ -436,9 +436,9 @@ void EKFLocalizer::update_simple_1d_filters( { double z = pose.pose.pose.position.z; - const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); + const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; double z_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); double pitch_dev = @@ -454,9 +454,9 @@ void EKFLocalizer::init_simple_1d_filters( { double z = pose.pose.pose.position.z; - const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); + const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; double z_dev = pose.pose.covariance[COV_IDX::Z_Z]; double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL]; double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH]; diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 8703d8754eaaf..ba6b7dedca82c 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -22,8 +22,8 @@ #include "ekf_localizer/state_transition.hpp" #include "ekf_localizer/warning_message.hpp" -#include -#include +#include +#include #include #include @@ -65,7 +65,7 @@ void EKFModule::initialize( x(IDX::VX) = 0.0; x(IDX::WZ) = 0.0; - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; p(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X]; p(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y]; p(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW]; @@ -97,13 +97,13 @@ geometry_msgs::msg::PoseStamped EKFModule::get_current_pose( Pose current_ekf_pose; current_ekf_pose.header.frame_id = params_.pose_frame_id; current_ekf_pose.header.stamp = current_time; - current_ekf_pose.pose.position = tier4_autoware_utils::createPoint(x, y, z); + current_ekf_pose.pose.position = autoware::universe_utils::createPoint(x, y, z); if (get_biased_yaw) { current_ekf_pose.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, biased_yaw); + autoware::universe_utils::createQuaternionFromRPY(roll, pitch, biased_yaw); } else { current_ekf_pose.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); + autoware::universe_utils::createQuaternionFromRPY(roll, pitch, yaw); } return current_ekf_pose; } @@ -285,7 +285,7 @@ bool EKFModule::measurement_update_pose( geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_z_delay( const PoseWithCovariance & pose, const double delay_time) { - const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); + const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); const double dz_delay = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y); PoseWithCovariance pose_with_z_delay; pose_with_z_delay = pose; diff --git a/localization/ekf_localizer/src/measurement.cpp b/localization/ekf_localizer/src/measurement.cpp index 4533bacbee991..faf8768938b7d 100644 --- a/localization/ekf_localizer/src/measurement.cpp +++ b/localization/ekf_localizer/src/measurement.cpp @@ -14,8 +14,8 @@ #include "ekf_localizer/measurement.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "ekf_localizer/state_index.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" Eigen::Matrix pose_measurement_matrix() { @@ -38,7 +38,7 @@ Eigen::Matrix3d pose_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { Eigen::Matrix3d r; - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::Y_X), covariance.at(COV_IDX::Y_Y), covariance.at(COV_IDX::Y_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_Y), covariance.at(COV_IDX::YAW_YAW); @@ -49,7 +49,7 @@ Eigen::Matrix2d twist_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { Eigen::Matrix2d r; - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_YAW); return r * static_cast(smoothing_step); diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index f85640ab2c523..f1f7214c3b848 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,10 +15,10 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "gyro_odometer/diagnostics_module.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include @@ -43,7 +43,7 @@ namespace autoware::gyro_odometer class GyroOdometerNode : public rclcpp::Node { private: - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; public: explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); @@ -67,8 +67,8 @@ class GyroOdometerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr twist_with_covariance_pub_; - std::shared_ptr transform_listener_; - std::unique_ptr logger_configure_; + std::shared_ptr transform_listener_; + std::unique_ptr logger_configure_; std::string output_frame_; double message_timeout_sec_; diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index 575f6a2d74837..faa64a1313ebb 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils diagnostic_msgs fmt geometry_msgs @@ -25,7 +26,6 @@ sensor_msgs tf2 tf2_geometry_msgs - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 0ec479770740f..c661f63d91535 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -33,7 +33,7 @@ namespace autoware::gyro_odometer std::array transform_covariance(const std::array & cov) { - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); @@ -52,8 +52,8 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) vehicle_twist_arrived_(false), imu_arrived_(false) { - transform_listener_ = std::make_shared(this); - logger_configure_ = std::make_unique(this); + transform_listener_ = std::make_shared(this); + logger_configure_ = std::make_unique(this); vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, @@ -208,8 +208,8 @@ void GyroOdometerNode::concat_gyro_and_odometer() gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance); } - using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; - using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using COV_IDX_XYZ = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX_XYZRPY = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; // calc mean, covariance double vx_mean = 0; diff --git a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp index 53f243520742f..a178a305f6d12 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp @@ -16,8 +16,8 @@ #define LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ #include +#include #include -#include #include #include @@ -43,7 +43,7 @@ class LocalizationErrorMonitor : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; double scale_; double error_ellipse_size_; diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 0138451b069e4..681da7e57c6c5 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -20,13 +20,13 @@ autoware_cmake eigen + autoware_universe_utils diagnostic_msgs diagnostic_updater nav_msgs rclcpp_components tf2 tf2_geometry_msgs - tier4_autoware_utils visualization_msgs ament_cmake_ros diff --git a/localization/localization_error_monitor/src/localization_error_monitor.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp index 04bb37a85e41b..204afa7bdec2f 100644 --- a/localization/localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -55,7 +55,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o diag_pub_ = this->create_publisher("/diagnostics", 10); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } visualization_msgs::msg::Marker LocalizationErrorMonitor::create_ellipse_marker( diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt index fe65077d89649..9a9086314f42e 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/localization_util/CMakeLists.txt @@ -7,6 +7,7 @@ autoware_package() ament_auto_add_library(localization_util SHARED src/util_func.cpp src/smart_pose_buffer.cpp + src/tree_structured_parzen_estimator.cpp ) if(BUILD_TESTING) @@ -15,6 +16,11 @@ if(BUILD_TESTING) test/test_smart_pose_buffer.cpp src/smart_pose_buffer.cpp ) + + ament_auto_add_gtest(test_tpe + test/test_tpe.cpp + src/tree_structured_parzen_estimator.cpp + ) endif() ament_auto_package( diff --git a/localization/localization_util/README.md b/localization/localization_util/README.md index a3e980464d0c6..a7daea33e0273 100644 --- a/localization/localization_util/README.md +++ b/localization/localization_util/README.md @@ -1,5 +1,5 @@ # localization_util -`localization_util`` is a localization utility package. +`localization_util` is a localization utility package. This package does not have a node, it is just a library. diff --git a/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp b/localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp similarity index 71% rename from localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp rename to localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp index 30d36e7150113..ee25ac175c0b4 100644 --- a/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp +++ b/localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ -#define TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#ifndef LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#define LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ /* A implementation of tree-structured parzen estimator (TPE) @@ -56,19 +56,20 @@ class TreeStructuredParzenEstimator TreeStructuredParzenEstimator() = delete; TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, - const std::vector & sample_mean, const std::vector & sample_stddev); + const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, + std::vector sample_stddev); void add_trial(const Trial & trial); - Input get_next_input() const; + [[nodiscard]] Input get_next_input() const; private: - static constexpr double MAX_GOOD_RATE = 0.10; - static constexpr int64_t N_EI_CANDIDATES = 100; + static constexpr double max_good_rate = 0.10; + static constexpr int64_t n_ei_candidates = 100; static std::mt19937_64 engine; - double compute_log_likelihood_ratio(const Input & input) const; - double log_gaussian_pdf(const Input & input, const Input & mu, const Input & sigma) const; + [[nodiscard]] double compute_log_likelihood_ratio(const Input & input) const; + [[nodiscard]] static double log_gaussian_pdf( + const Input & input, const Input & mu, const Input & sigma); std::vector trials_; int64_t above_num_; @@ -80,4 +81,4 @@ class TreeStructuredParzenEstimator Input base_stddev_; }; -#endif // TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#endif // LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/localization_util/package.xml b/localization/localization_util/package.xml index fa8da2aa1231a..bb1ca3123ad27 100644 --- a/localization/localization_util/package.xml +++ b/localization/localization_util/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils fmt geometry_msgs nav_msgs @@ -29,7 +30,6 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - tier4_autoware_utils tier4_debug_msgs tier4_localization_msgs visualization_msgs diff --git a/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp b/localization/localization_util/src/tree_structured_parzen_estimator.cpp similarity index 86% rename from localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp rename to localization/localization_util/src/tree_structured_parzen_estimator.cpp index c81962c14f61c..aab78f0aaec71 100644 --- a/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp +++ b/localization/localization_util/src/tree_structured_parzen_estimator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" +#include "localization_util/tree_structured_parzen_estimator.hpp" #include #include @@ -23,14 +23,14 @@ std::mt19937_64 TreeStructuredParzenEstimator::engine(std::random_device{}()); TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, - const std::vector & sample_mean, const std::vector & sample_stddev) + const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, + std::vector sample_stddev) : above_num_(0), direction_(direction), n_startup_trials_(n_startup_trials), input_dimension_(INDEX_NUM), - sample_mean_(sample_mean), - sample_stddev_(sample_stddev) + sample_mean_(std::move(sample_mean)), + sample_stddev_(std::move(sample_stddev)) { if (sample_mean_.size() != ANGLE_Z) { std::cerr << "sample_mean size is invalid" << std::endl; @@ -56,8 +56,9 @@ void TreeStructuredParzenEstimator::add_trial(const Trial & trial) std::sort(trials_.begin(), trials_.end(), [this](const Trial & lhs, const Trial & rhs) { return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); }); - above_num_ = - std::min({static_cast(10), static_cast(trials_.size() * MAX_GOOD_RATE)}); + above_num_ = std::min( + {static_cast(10), + static_cast(static_cast(trials_.size()) * max_good_rate)}); } TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const @@ -88,7 +89,7 @@ TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_inp Input best_input; double best_log_likelihood_ratio = std::numeric_limits::lowest(); - for (int64_t i = 0; i < N_EI_CANDIDATES; i++) { + for (int64_t i = 0; i < n_ei_candidates; i++) { Input input(input_dimension_); input[TRANS_X] = dist_normal_trans_x(engine); input[TRANS_Y] = dist_normal_trans_y(engine); @@ -107,7 +108,7 @@ TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_inp double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & input) const { - const int64_t n = trials_.size(); + const auto n = static_cast(trials_.size()); // The above KDE and the below KDE are calculated respectively, and the ratio is the criteria to // select best sample. @@ -117,11 +118,11 @@ double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & for (int64_t i = 0; i < n; i++) { const double log_p = log_gaussian_pdf(input, trials_[i].input, base_stddev_); if (i < above_num_) { - const double w = 1.0 / above_num_; + const double w = 1.0 / static_cast(above_num_); const double log_w = std::log(w); above_logs.push_back(log_p + log_w); } else { - const double w = 1.0 / (n - above_num_); + const double w = 1.0 / static_cast(n - above_num_); const double log_w = std::log(w); below_logs.push_back(log_p + log_w); } @@ -129,10 +130,9 @@ double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & auto log_sum_exp = [](const std::vector & log_vec) { const double max = *std::max_element(log_vec.begin(), log_vec.end()); - double sum = 0.0; - for (const double log_v : log_vec) { - sum += std::exp(log_v - max); - } + double sum = std::accumulate( + log_vec.begin(), log_vec.end(), 0.0, + [max](double total, double log_v) { return total + std::exp(log_v - max); }); return max + std::log(sum); }; @@ -147,14 +147,14 @@ double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & } double TreeStructuredParzenEstimator::log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma) const + const Input & input, const Input & mu, const Input & sigma) { const double log_2pi = std::log(2.0 * M_PI); auto log_gaussian_pdf_1d = [&](const double diff, const double sigma) { return -0.5 * log_2pi - std::log(sigma) - (diff * diff) / (2.0 * sigma * sigma); }; - const int64_t n = input.size(); + const auto n = static_cast(input.size()); double result = 0.0; for (int64_t i = 0; i < n; i++) { diff --git a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp b/localization/localization_util/test/test_tpe.cpp similarity index 64% rename from localization/tree_structured_parzen_estimator/test/test_tpe.cpp rename to localization/localization_util/test/test_tpe.cpp index f8a697878d6a3..fd9afe8b2a75f 100644 --- a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp +++ b/localization/localization_util/test/test_tpe.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" +#include "localization_util/tree_structured_parzen_estimator.hpp" #include @@ -20,7 +20,7 @@ TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphe { auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { double value = 0.0; - const int64_t n = input.size(); + const auto n = static_cast(input.size()); for (int64_t i = 0; i < n; i++) { const double v = input[i] * 10; value += v * v; @@ -28,26 +28,23 @@ TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphe return value; }; - constexpr int64_t kOuterTrialsNum = 20; - constexpr int64_t kInnerTrialsNum = 200; + constexpr int64_t k_outer_trials_num = 20; + constexpr int64_t k_inner_trials_num = 200; std::cout << std::fixed; std::vector mean_scores; - const int64_t n_startup_trials = kInnerTrialsNum / 10; - const std::string method = ((n_startup_trials == kInnerTrialsNum) ? "Random" : "TPE"); + std::vector sample_mean(5, 0.0); + std::vector sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1}; - const std::vector sample_mean(5, 0.0); - const std::vector sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1}; - - for (const int64_t n_startup_trials : {kInnerTrialsNum, kInnerTrialsNum / 2}) { - const std::string method = ((n_startup_trials == kInnerTrialsNum) ? "Random" : "TPE"); + for (const int64_t n_startup_trials : {k_inner_trials_num, k_inner_trials_num / 2}) { + const std::string method = ((n_startup_trials == k_inner_trials_num) ? "Random" : "TPE"); std::vector scores; - for (int64_t i = 0; i < kOuterTrialsNum; i++) { + for (int64_t i = 0; i < k_outer_trials_num; i++) { double best_score = std::numeric_limits::lowest(); TreeStructuredParzenEstimator estimator( TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, sample_mean, sample_stddev); - for (int64_t trial = 0; trial < kInnerTrialsNum; trial++) { + for (int64_t trial = 0; trial < k_inner_trials_num; trial++) { const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); const double score = -sphere_function(input); estimator.add_trial({input, score}); @@ -57,13 +54,12 @@ TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphe } const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); - const double mean = sum / scores.size(); + const double mean = sum / static_cast(scores.size()); mean_scores.push_back(mean); - double sq_sum = 0.0; - for (const double score : scores) { - sq_sum += (score - mean) * (score - mean); - } - const double stddev = std::sqrt(sq_sum / scores.size()); + double sq_sum = std::accumulate( + scores.begin(), scores.end(), 0.0, + [mean](double total, double score) { return total + (score - mean) * (score - mean); }); + const double stddev = std::sqrt(sq_sum / static_cast(scores.size())); std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; } diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 8049104e8f2f5..48ce49d2bcaa9 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -266,7 +266,7 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num | ------------------------------------------------ | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- | | `topic_time_stamp` | the time stamp of input topic | none | none | no | | `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | -| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | +| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `sensor_points.timeout_sec` | none | yes | | `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | | `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | | `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. | @@ -280,9 +280,9 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num | `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | | `nearest_voxel_transformation_likelihood_diff` | the nvtl score difference for the current ndt optimization | none | none | no | | `nearest_voxel_transformation_likelihood_before` | the nvtl score before the current ndt optimization | none | none | no | -| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | +| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than `validation.initial_to_result_distance_tolerance_m` | none | no | | `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | -| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - | +| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is `validation.skipping_publish_num` or more | none | - | ※The `sensor_points_callback` shares the same callback group as the `trigger_node_service` and `ndt_align_service`. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale. diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index ec80a0ef79c69..f62329b8bdb6d 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -12,6 +12,9 @@ sensor_points: + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] + timeout_sec: 1.0 + # Required distance of input sensor points. [m] # If the max distance of input sensor points is lower than this value, the scan matching will not be performed. required_distance: 10.0 @@ -52,18 +55,21 @@ validation: - # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] - lidar_topic_timeout_sec: 1.0 - # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] initial_pose_timeout_sec: 1.0 # Tolerance of distance difference between two initial poses used for linear interpolation. [m] initial_pose_distance_tolerance_m: 10.0 + # Tolerance of distance difference from initial pose to result pose. [m] + initial_to_result_distance_tolerance_m: 3.0 + # The execution time which means probably NDT cannot matches scans properly. [ms] critical_upper_bound_exe_time_ms: 100.0 + # Tolerance for the number of times rejected estimation results consecutively + skipping_publish_num: 5 + score_estimation: # Converged param type diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index 7002b4bf43a73..e488b49393d48 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -40,6 +40,7 @@ struct HyperParameters struct SensorPoints { + double timeout_sec{}; double required_distance{}; } sensor_points{}; @@ -54,10 +55,11 @@ struct HyperParameters struct Validation { - double lidar_topic_timeout_sec{}; double initial_pose_timeout_sec{}; double initial_pose_distance_tolerance_m{}; + double initial_to_result_distance_tolerance_m{}; double critical_upper_bound_exe_time_ms{}; + int64_t skipping_publish_num{}; } validation{}; struct ScoreEstimation @@ -97,6 +99,7 @@ struct HyperParameters frame.ndt_base_frame = node->declare_parameter("frame.ndt_base_frame"); frame.map_frame = node->declare_parameter("frame.map_frame"); + sensor_points.timeout_sec = node->declare_parameter("sensor_points.timeout_sec"); sensor_points.required_distance = node->declare_parameter("sensor_points.required_distance"); @@ -115,14 +118,16 @@ struct HyperParameters initial_pose_estimation.n_startup_trials = node->declare_parameter("initial_pose_estimation.n_startup_trials"); - validation.lidar_topic_timeout_sec = - node->declare_parameter("validation.lidar_topic_timeout_sec"); validation.initial_pose_timeout_sec = node->declare_parameter("validation.initial_pose_timeout_sec"); validation.initial_pose_distance_tolerance_m = node->declare_parameter("validation.initial_pose_distance_tolerance_m"); + validation.initial_to_result_distance_tolerance_m = + node->declare_parameter("validation.initial_to_result_distance_tolerance_m"); validation.critical_upper_bound_exe_time_ms = node->declare_parameter("validation.critical_upper_bound_exe_time_ms"); + validation.skipping_publish_num = + node->declare_parameter("validation.skipping_publish_num"); const int64_t converged_param_type_tmp = node->declare_parameter("score_estimation.converged_param_type"); 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 2bcb6f4cf6fb8..ac913b128b5f1 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 @@ -20,9 +20,9 @@ #include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/particle.hpp" +#include +#include #include -#include -#include #include #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index cbd2797c57922..2f3eb82b1c217 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -22,8 +22,8 @@ #include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/map_update_module.hpp" +#include #include -#include #include #include @@ -203,7 +203,7 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr diagnostics_ndt_align_; std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; HyperParameters param_; }; diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index b62e810926331..c69836d77c825 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -19,6 +19,7 @@ autoware_cmake autoware_map_msgs + autoware_universe_utils diagnostic_msgs fmt geometry_msgs @@ -37,10 +38,8 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - tier4_autoware_utils tier4_debug_msgs tier4_localization_msgs - tree_structured_parzen_estimator visualization_msgs ament_cmake_cppcheck diff --git a/localization/ndt_scan_matcher/schema/sub/sensor_points.json b/localization/ndt_scan_matcher/schema/sub/sensor_points.json index 68a0b40f8f94e..12bda6fb38d24 100644 --- a/localization/ndt_scan_matcher/schema/sub/sensor_points.json +++ b/localization/ndt_scan_matcher/schema/sub/sensor_points.json @@ -5,13 +5,19 @@ "sensor_points": { "type": "object", "properties": { + "timeout_sec": { + "type": "number", + "description": "Tolerance of timestamp difference between current time and sensor pointcloud. [sec]", + "default": 1.0, + "minimum": 0.0 + }, "required_distance": { "type": "number", "description": "Required distance of input sensor points [m]. If the max distance of input sensor points is lower than this value, the scan matching will not be performed.", "default": "10.0" } }, - "required": ["required_distance"], + "required": ["timeout_sec", "required_distance"], "additionalProperties": false } } diff --git a/localization/ndt_scan_matcher/schema/sub/validation.json b/localization/ndt_scan_matcher/schema/sub/validation.json index 5ad40ceb99577..c1168d733ff9d 100644 --- a/localization/ndt_scan_matcher/schema/sub/validation.json +++ b/localization/ndt_scan_matcher/schema/sub/validation.json @@ -5,12 +5,6 @@ "validation": { "type": "object", "properties": { - "lidar_topic_timeout_sec": { - "type": "number", - "description": "Tolerance of timestamp difference between current time and sensor pointcloud. [sec]", - "default": 1.0, - "minimum": 0.0 - }, "initial_pose_timeout_sec": { "type": "number", "description": "Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]", @@ -23,18 +17,31 @@ "default": 10.0, "minimum": 0.0 }, + "initial_to_result_distance_tolerance_m": { + "type": "number", + "description": "Tolerance of distance difference from initial pose to result pose. [m]", + "default": 3.0, + "minimum": 0.0 + }, "critical_upper_bound_exe_time_ms": { "type": "number", "description": "The execution time which means probably NDT cannot matches scans properly. [ms]", "default": 100.0, "minimum": 0.0 + }, + "skipping_publish_num": { + "type": "number", + "description": "Tolerance for the number for times rejected estimation results consecutively.", + "default": 5, + "minimum": 1 } }, "required": [ - "lidar_topic_timeout_sec", "initial_pose_timeout_sec", "initial_pose_distance_tolerance_m", - "critical_upper_bound_exe_time_ms" + "initial_to_result_distance_tolerance_m", + "critical_upper_bound_exe_time_ms", + "skipping_publish_num" ], "additionalProperties": false } diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index e84d6709c3151..ca486a320d9dc 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -15,12 +15,12 @@ #include "ndt_scan_matcher/ndt_scan_matcher_core.hpp" #include "localization_util/matrix_type.hpp" +#include "localization_util/tree_structured_parzen_estimator.hpp" #include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.hpp" -#include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" -#include -#include +#include +#include #include @@ -210,7 +210,7 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) diagnostics_trigger_node_ = std::make_unique(this, "trigger_node_service_status"); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void NDTScanMatcher::callback_timer() @@ -300,12 +300,11 @@ void NDTScanMatcher::callback_sensor_points( callback_sensor_points_main(sensor_points_msg_in_sensor_frame); // check skipping_publish_num - static size_t skipping_publish_num = 0; - const size_t error_skipping_publish_num = 5; + static int64_t skipping_publish_num = 0; skipping_publish_num = ((is_succeed_scan_matching || !is_activated_) ? 0 : (skipping_publish_num + 1)); diagnostics_scan_points_->add_key_value("skipping_publish_num", skipping_publish_num); - if (skipping_publish_num >= error_skipping_publish_num) { + if (skipping_publish_num >= param_.validation.skipping_publish_num) { std::stringstream message; message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times)."; diagnostics_scan_points_->update_level_and_message( @@ -340,11 +339,11 @@ bool NDTScanMatcher::callback_sensor_points_main( (this->now() - sensor_points_msg_in_sensor_frame->header.stamp).seconds(); diagnostics_scan_points_->add_key_value( "sensor_points_delay_time_sec", sensor_points_delay_time_sec); - if (sensor_points_delay_time_sec > param_.validation.lidar_topic_timeout_sec) { + if (sensor_points_delay_time_sec > param_.sensor_points.timeout_sec) { std::stringstream message; message << "sensor points is experiencing latency." << "The delay time is " << sensor_points_delay_time_sec << "[sec] " - << "(the tolerance is " << param_.validation.lidar_topic_timeout_sec << "[sec])."; + << "(the tolerance is " << param_.sensor_points.timeout_sec << "[sec])."; diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); @@ -578,8 +577,7 @@ bool NDTScanMatcher::callback_sensor_points_main( const auto distance_initial_to_result = static_cast( norm(interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position)); diagnostics_scan_points_->add_key_value("distance_initial_to_result", distance_initial_to_result); - const double warn_distance_initial_to_result = 3.0; - if (distance_initial_to_result > warn_distance_initial_to_result) { + if (distance_initial_to_result > param_.validation.initial_to_result_distance_tolerance_m) { std::stringstream message; message << "distance_initial_to_result is too large (" << distance_initial_to_result << " [m])."; @@ -617,7 +615,7 @@ bool NDTScanMatcher::callback_sensor_points_main( pcl::shared_ptr> sensor_points_in_map_ptr( new pcl::PointCloud); - tier4_autoware_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( *sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud(sensor_ros_time, param_.frame.map_frame, sensor_points_in_map_ptr); @@ -673,10 +671,10 @@ void NDTScanMatcher::transform_sensor_measurement( } const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = - tier4_autoware_utils::transform2pose(transform); + autoware::universe_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = pose_to_matrix4f(target_to_source_pose_stamped.pose); - tier4_autoware_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( *sensor_points_input_ptr, *sensor_points_output_ptr, base_to_sensor_matrix); } @@ -688,7 +686,7 @@ void NDTScanMatcher::publish_tf( result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; tf2_broadcaster_.sendTransform( - tier4_autoware_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); + autoware::universe_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); } void NDTScanMatcher::publish_pose( @@ -732,7 +730,7 @@ void NDTScanMatcher::publish_marker( marker.header.frame_id = param_.frame.map_frame; marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1); + marker.scale = autoware::universe_utils::createMarkerScale(0.3, 0.1, 0.1); int i = 0; marker.ns = "result_pose_matrix_array"; marker.action = visualization_msgs::msg::Marker::ADD; @@ -761,7 +759,7 @@ void NDTScanMatcher::publish_initial_to_result( { geometry_msgs::msg::PoseStamped initial_to_result_relative_pose_stamped; initial_to_result_relative_pose_stamped.pose = - tier4_autoware_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); + autoware::universe_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); initial_to_result_relative_pose_stamped.header.stamp = sensor_ros_time; initial_to_result_relative_pose_stamped.header.frame_id = param_.frame.map_frame; initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); @@ -1070,8 +1068,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); Particle particle( - initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, - ndt_result.iteration_num); + initial_pose, matrix4f_to_pose(ndt_result.pose), + ndt_result.nearest_voxel_transformation_likelihood, ndt_result.iteration_num); particle_array.push_back(particle); push_debug_markers(marker_array, get_clock()->now(), param_.frame.map_frame, particle, i); if ( @@ -1093,7 +1091,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability}); auto sensor_points_in_map_ptr = std::make_shared>(); - tier4_autoware_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud( initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr); @@ -1103,6 +1101,13 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( std::begin(particle_array), std::end(particle_array), [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; }); + if ( + best_particle_ptr->score < + param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood) + RCLCPP_WARN_STREAM( + this->get_logger(), + "Initial Pose Estimation is Unstable. Score is " << best_particle_ptr->score); + geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp index c366a7f02f4fe..b4ad7111e4ba0 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp @@ -30,7 +30,7 @@ struct GridKey GridKey() : x(0), y(0) {} GridKey(float x, float y) : x(std::floor(x / unit_length)), y(std::floor(y / unit_length)) {} - pcl::PointXYZ get_center_point() const + [[nodiscard]] pcl::PointXYZ get_center_point() const { pcl::PointXYZ xyz; xyz.x = unit_length * (static_cast(x) + 0.5f); diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp index 72071b23b467f..4b0188a1638f6 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp @@ -20,6 +20,11 @@ #include +#include +#include +#include +#include + namespace pose_estimator_arbiter::rule_helper { PcdOccupancy::PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold) diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp index e5eb4ff091a31..098704e11aba9 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp @@ -24,6 +24,8 @@ #include #include +#include + namespace pose_estimator_arbiter::rule_helper { class PcdOccupancy @@ -34,7 +36,7 @@ class PcdOccupancy public: explicit PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold); - MarkerArray debug_marker_array() const; + [[nodiscard]] MarkerArray debug_marker_array() const; void init(PointCloud2::ConstSharedPtr msg); bool ndt_can_operate( const geometry_msgs::msg::Point & position, std::string * optional_message = nullptr) const; diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp index 4eea34f9ae1ee..df7f9c6c1035f 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp @@ -33,14 +33,14 @@ using BoostPolygon = boost::geometry::model::polygon; struct PoseEstimatorArea::Impl { - explicit Impl(rclcpp::Logger logger) : logger_(logger) {} + explicit Impl(const rclcpp::Logger & logger) : logger_(logger) {} std::multimap bounding_boxes_; void init(HADMapBin::ConstSharedPtr msg); - bool within( + [[nodiscard]] bool within( const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const; - MarkerArray debug_marker_array() const { return marker_array_; } + [[nodiscard]] MarkerArray debug_marker_array() const { return marker_array_; } private: rclcpp::Logger logger_; @@ -105,7 +105,7 @@ void PoseEstimatorArea::Impl::init(HADMapBin::ConstSharedPtr msg) marker.color.set__r(1.0f).set__g(1.0f).set__b(0.0f).set__a(1.0f); marker.ns = subtype; marker.header.frame_id = "map"; - marker.id = marker_array_.markers.size(); + marker.id = static_cast(marker_array_.markers.size()); // Enqueue the vertices of the polygon to bounding box and visualization marker BoostPolygon poly; diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp index 392219b2281e1..74843c66a4eba 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp @@ -40,10 +40,10 @@ class PoseEstimatorArea // This is assumed to be called only once in the vector map callback. void init(const HADMapBin::ConstSharedPtr msg); - bool within( + [[nodiscard]] bool within( const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const; - MarkerArray debug_marker_array() const; + [[nodiscard]] MarkerArray debug_marker_array() const; private: struct Impl; diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp index 4c3829316230b..3a565e7f2e4df 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp @@ -14,19 +14,23 @@ #include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" -#include +#include + +#include namespace pose_estimator_arbiter::switch_rule { PcdMapBasedRule::PcdMapBasedRule( - rclcpp::Node & node, const std::unordered_set & running_estimator_list, - const std::shared_ptr shared_data) -: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data) + rclcpp::Node & node, std::unordered_set running_estimator_list, + std::shared_ptr shared_data) +: BaseSwitchRule(node), + running_estimator_list_(std::move(running_estimator_list)), + shared_data_(std::move(shared_data)) { const int pcd_density_upper_threshold = - tier4_autoware_utils::getOrDeclareParameter(node, "pcd_density_upper_threshold"); + autoware::universe_utils::getOrDeclareParameter(node, "pcd_density_upper_threshold"); const int pcd_density_lower_threshold = - tier4_autoware_utils::getOrDeclareParameter(node, "pcd_density_lower_threshold"); + autoware::universe_utils::getOrDeclareParameter(node, "pcd_density_lower_threshold"); pcd_occupancy_ = std::make_unique( pcd_density_upper_threshold, pcd_density_lower_threshold); @@ -63,14 +67,14 @@ std::unordered_map PcdMapBasedRule::update() {PoseEstimatorType::eagleye, false}, {PoseEstimatorType::artag, false}, }; - } else { - debug_string_ = "Enable yabloc: "; - return { - {PoseEstimatorType::ndt, false}, - {PoseEstimatorType::yabloc, true}, - {PoseEstimatorType::eagleye, false}, - {PoseEstimatorType::artag, false}}; } + + debug_string_ = "Enable yabloc: "; + return { + {PoseEstimatorType::ndt, false}, + {PoseEstimatorType::yabloc, true}, + {PoseEstimatorType::eagleye, false}, + {PoseEstimatorType::artag, false}}; } } // namespace pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp index 23fd0f812f700..ab4d18dcad66a 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp @@ -31,7 +31,7 @@ class PcdMapBasedRule : public BaseSwitchRule { public: PcdMapBasedRule( - rclcpp::Node & node, const std::unordered_set & running_estimator_list, + rclcpp::Node & node, std::unordered_set running_estimator_list, const std::shared_ptr shared_data); std::unordered_map update() override; diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp index 675842662acf8..dffb738e87685 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp @@ -16,12 +16,16 @@ #include +#include + namespace pose_estimator_arbiter::switch_rule { VectorMapBasedRule::VectorMapBasedRule( - rclcpp::Node & node, const std::unordered_set & running_estimator_list, - const std::shared_ptr shared_data) -: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data) + rclcpp::Node & node, std::unordered_set running_estimator_list, + std::shared_ptr shared_data) +: BaseSwitchRule(node), + running_estimator_list_(std::move(running_estimator_list)), + shared_data_(std::move(shared_data)) { pose_estimator_area_ = std::make_unique(node.get_logger()); diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp index e3360f9692f86..83723a346054b 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp @@ -32,8 +32,8 @@ class VectorMapBasedRule : public BaseSwitchRule { public: VectorMapBasedRule( - rclcpp::Node & node, const std::unordered_set & running_estimator_list, - const std::shared_ptr shared_data); + rclcpp::Node & node, std::unordered_set running_estimator_list, + std::shared_ptr shared_data); std::unordered_map update() override; diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp index 5febfa403363e..f53e01dc9c548 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp @@ -19,10 +19,10 @@ #include -class MockNode : public ::testing::Test +class PcdMapBasedRuleMockNode : public ::testing::Test { protected: - virtual void SetUp() + void SetUp() override { rclcpp::init(0, nullptr); node = std::make_shared("test_node"); @@ -46,10 +46,10 @@ class MockNode : public ::testing::Test std::shared_ptr shared_data_; std::shared_ptr rule_; - virtual void TearDown() { rclcpp::shutdown(); } + void TearDown() override { rclcpp::shutdown(); } }; -TEST_F(MockNode, pcdMapBasedRule) +TEST_F(PcdMapBasedRuleMockNode, pcdMapBasedRule) { // Create dummy pcd and set { diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp index beda4d720b7f6..0001ce674a2e8 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp +++ b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp @@ -28,10 +28,10 @@ #include -class MockNode : public ::testing::Test +class RuleHelperMockNode : public ::testing::Test { protected: - virtual void SetUp() + void SetUp() override { rclcpp::init(0, nullptr); node = std::make_shared("test_node"); @@ -39,10 +39,10 @@ class MockNode : public ::testing::Test std::shared_ptr node{nullptr}; - virtual void TearDown() { rclcpp::shutdown(); } + void TearDown() override { rclcpp::shutdown(); } }; -TEST_F(MockNode, poseEstimatorArea) +TEST_F(RuleHelperMockNode, poseEstimatorArea) { auto create_polygon3d = []() -> lanelet::Polygon3d { lanelet::Polygon3d polygon; @@ -73,7 +73,7 @@ TEST_F(MockNode, poseEstimatorArea) EXPECT_FALSE(pose_estimator_area.within(Point().set__x(-5).set__y(-5).set__z(0), "yabloc")); } -TEST_F(MockNode, pcdOccupancy) +TEST_F(RuleHelperMockNode, pcdOccupancy) { using pose_estimator_arbiter::rule_helper::PcdOccupancy; const int pcd_density_upper_threshold = 20; @@ -89,7 +89,7 @@ TEST_F(MockNode, pcdOccupancy) EXPECT_FALSE(pcd_occupancy.ndt_can_operate(point, &message)); } -TEST_F(MockNode, gridKey) +TEST_F(RuleHelperMockNode, gridKey) { using pose_estimator_arbiter::rule_helper::GridKey; EXPECT_TRUE(GridKey(10., -5.) == GridKey(10., -10.)); diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp index 66d867883272b..a4fd91365f433 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp @@ -24,10 +24,10 @@ #include -class MockNode : public ::testing::Test +class VectorMapBasedRuleMockNode : public ::testing::Test { protected: - virtual void SetUp() + void SetUp() override { rclcpp::init(0, nullptr); node = std::make_shared("test_node"); @@ -49,10 +49,10 @@ class MockNode : public ::testing::Test std::shared_ptr shared_data_; std::shared_ptr rule_; - virtual void TearDown() { rclcpp::shutdown(); } + void TearDown() override { rclcpp::shutdown(); } }; -TEST_F(MockNode, vectorMapBasedRule) +TEST_F(VectorMapBasedRuleMockNode, vectorMapBasedRule) { // Create dummy lanelet2 and set { diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml index 70896d93d9cb8..4880e048ea392 100644 --- a/localization/pose_estimator_arbiter/package.xml +++ b/localization/pose_estimator_arbiter/package.xml @@ -4,7 +4,13 @@ pose_estimator_arbiter 0.1.0 The arbiter of multiple pose estimators + Yamato Ando Kento Yabuuchi + Masahiro Sakamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Kento Yabuuchi @@ -13,6 +19,7 @@ autoware_adapi_v1_msgs autoware_map_msgs + autoware_universe_utils diagnostic_msgs geometry_msgs lanelet2_extension @@ -24,7 +31,6 @@ sensor_msgs std_msgs std_srvs - tier4_autoware_utils ublox_msgs visualization_msgs yabloc_particle_filter diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp index fda77ec877482..013b4b38f9ef6 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp @@ -19,8 +19,8 @@ #include "pose_estimator_arbiter/stopper/base_stopper.hpp" #include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" +#include #include -#include #include #include @@ -53,7 +53,7 @@ class PoseEstimatorArbiter : public rclcpp::Node // Set of running pose estimators specified by ros param `pose_sources` const std::unordered_set running_estimator_list_; // Configuration to allow changing the log level by service - const std::unique_ptr logger_configure_; + const std::unique_ptr logger_configure_; // This is passed to several modules (stoppers & rule) so that all modules can access common data // without passing them as arguments. Also, modules can register subscriber callbacks through diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp index 67c555227976d..8e25628d6e0fc 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp @@ -46,7 +46,7 @@ PoseEstimatorArbiter::PoseEstimatorArbiter(const rclcpp::NodeOptions & options) : rclcpp::Node("pose_estimator_arbiter", options), running_estimator_list_(parse_estimator_name_args( declare_parameter>("pose_sources"), get_logger())), - logger_configure_(std::make_unique(this)) + logger_configure_(std::make_unique(this)) { // Shared data shared_data_ = std::make_shared(); diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp index edf12537198a3..d78bfeb05b4f0 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp @@ -18,6 +18,6 @@ namespace pose_estimator_arbiter { enum class PoseEstimatorType : int { ndt = 1, yabloc = 2, eagleye = 4, artag = 8 }; -} +} // namespace pose_estimator_arbiter #endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp index a72dc5585877b..0d3dbfab11cbe 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp @@ -30,29 +30,29 @@ namespace pose_estimator_arbiter template struct CallbackInvokingVariable { - CallbackInvokingVariable() {} + CallbackInvokingVariable() = default; - explicit CallbackInvokingVariable(T initial_data) : value(initial_data) {} + explicit CallbackInvokingVariable(T initial_data) : value_(initial_data) {} // Set data and invoke all callbacks void set_and_invoke(T new_value) { - value = new_value; + value_ = new_value; // Call all callbacks with the new value - for (const auto & callback : callbacks) { - callback(value.value()); + for (const auto & callback : callbacks_) { + callback(value_.value()); } } // Same as std::optional::has_value() - bool has_value() const { return value.has_value(); } + bool has_value() const { return value_.has_value(); } // Same as std::optional::value() - const T operator()() const { return value.value(); } + T operator()() const { return value_.value(); } // Register callback function which is invoked when set_and_invoke() is called - void register_callback(std::function callback) const { callbacks.push_back(callback); } + void register_callback(std::function callback) const { callbacks_.push_back(callback); } // Create subscription callback function which is used as below: // auto subscriber = create_subscription("topic_name", 10, @@ -64,10 +64,10 @@ struct CallbackInvokingVariable private: // The latest data - std::optional value{std::nullopt}; + std::optional value_{std::nullopt}; // These functions are expected not to change the value variable - mutable std::vector> callbacks; + mutable std::vector> callbacks_; }; // This structure is handed to several modules as shared_ptr so that all modules can access data. @@ -80,7 +80,7 @@ struct SharedData using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; - SharedData() {} + SharedData() = default; // Used for stoppers CallbackInvokingVariable eagleye_output_pose_cov; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp index fc2242a0b27e9..d64592a12308e 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp @@ -20,6 +20,7 @@ #include #include +#include namespace pose_estimator_arbiter::stopper { @@ -28,11 +29,16 @@ class BaseStopper public: using SharedPtr = std::shared_ptr; - explicit BaseStopper(rclcpp::Node * node, const std::shared_ptr shared_data) - : logger_(node->get_logger()), shared_data_(shared_data) + explicit BaseStopper(rclcpp::Node * node, std::shared_ptr shared_data) + : logger_(node->get_logger()), shared_data_(std::move(shared_data)) { } + virtual ~BaseStopper() = default; + BaseStopper(const BaseStopper & other) = default; + BaseStopper(BaseStopper && other) noexcept = default; + BaseStopper & operator=(const BaseStopper & other) = default; + BaseStopper & operator=(BaseStopper && other) noexcept = default; void enable() { set_enable(true); } void disable() { set_enable(false); } diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp index 72a3f5412add0..f334983f33aad 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp @@ -29,7 +29,7 @@ class StopperArTag : public BaseStopper using SetBool = std_srvs::srv::SetBool; public: - explicit StopperArTag(rclcpp::Node * node, const std::shared_ptr shared_data) + explicit StopperArTag(rclcpp::Node * node, const std::shared_ptr & shared_data) : BaseStopper(node, shared_data) { ar_tag_is_enabled_ = true; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp index 12bbe8c9769a1..cc800ad6bdee4 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp @@ -27,7 +27,8 @@ class StopperEagleye : public BaseStopper using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; public: - explicit StopperEagleye(rclcpp::Node * node, const std::shared_ptr shared_data) + explicit StopperEagleye( + rclcpp::Node * node, const std::shared_ptr & shared_data) : BaseStopper(node, shared_data) { eagleye_is_enabled_ = true; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp index dacea02f77bde..3b7c083ea31e3 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp @@ -27,7 +27,7 @@ class StopperNdt : public BaseStopper using PointCloud2 = sensor_msgs::msg::PointCloud2; public: - explicit StopperNdt(rclcpp::Node * node, const std::shared_ptr shared_data) + explicit StopperNdt(rclcpp::Node * node, const std::shared_ptr & shared_data) : BaseStopper(node, shared_data) { ndt_is_enabled_ = true; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp index 2cd8b66f4ffd4..808a5bf9407ca 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp @@ -29,7 +29,7 @@ class StopperYabLoc : public BaseStopper using SetBool = std_srvs::srv::SetBool; public: - explicit StopperYabLoc(rclcpp::Node * node, const std::shared_ptr shared_data) + explicit StopperYabLoc(rclcpp::Node * node, const std::shared_ptr & shared_data) : BaseStopper(node, shared_data) { yabloc_is_enabled_ = true; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp index 5770f18efa32c..95d10c2b741a8 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp @@ -40,12 +40,16 @@ class BaseSwitchRule } virtual ~BaseSwitchRule() = default; + BaseSwitchRule(const BaseSwitchRule & other) = default; + BaseSwitchRule(BaseSwitchRule && other) noexcept = default; + BaseSwitchRule & operator=(const BaseSwitchRule & other) = default; + BaseSwitchRule & operator=(BaseSwitchRule && other) noexcept = default; virtual std::unordered_map update() = 0; virtual std::string debug_string() { return std::string{}; } virtual MarkerArray debug_marker_array() { return MarkerArray{}; } protected: - rclcpp::Logger get_logger() const { return *logger_ptr_; } + [[nodiscard]] rclcpp::Logger get_logger() const { return *logger_ptr_; } std::shared_ptr logger_ptr_{nullptr}; }; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp index 2b2e325c3d94b..aebad094a0eca 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp @@ -19,14 +19,15 @@ #include #include #include +#include #include namespace pose_estimator_arbiter::switch_rule { EnableAllRule::EnableAllRule( - rclcpp::Node & node, const std::unordered_set & running_estimator_list, - const std::shared_ptr) -: BaseSwitchRule(node), running_estimator_list_(running_estimator_list) + rclcpp::Node & node, std::unordered_set running_estimator_list, + const std::shared_ptr &) +: BaseSwitchRule(node), running_estimator_list_(std::move(running_estimator_list)) { } diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp index 568226985b2ff..63142b0e662e1 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp @@ -29,10 +29,8 @@ class EnableAllRule : public BaseSwitchRule { public: EnableAllRule( - rclcpp::Node & node, const std::unordered_set & running_estimator_list, - const std::shared_ptr shared_data); - - virtual ~EnableAllRule() = default; + rclcpp::Node & node, std::unordered_set running_estimator_list, + const std::shared_ptr & shared_data); std::unordered_map update() override; diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 58968828bc2fa..5f3ea72f7d400 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -19,15 +19,15 @@ ament_cmake autoware_cmake + autoware_motion_utils + autoware_universe_utils component_interface_specs component_interface_utils geometry_msgs map_height_fitter - motion_utils rclcpp rclcpp_components std_srvs - tier4_autoware_utils tier4_localization_msgs ament_cmake_cppcheck diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 1111547b393e7..7d75a1fc80f2e 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -56,7 +56,7 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) stop_check_duration_ = declare_parameter("stop_check_duration"); stop_check_ = std::make_unique(this, stop_check_duration_ + 1.0); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); change_state(State::Message::UNINITIALIZED); diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index d80fb26fdd41f..a813ec6459810 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -15,10 +15,10 @@ #ifndef POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ #define POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ +#include #include #include #include -#include #include @@ -55,7 +55,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr stop_check_; std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); diff --git a/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp b/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp index 86c68b4a2dbb4..31f04029bd00e 100644 --- a/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp @@ -15,13 +15,13 @@ #ifndef POSE_INITIALIZER__STOP_CHECK_MODULE_HPP_ #define POSE_INITIALIZER__STOP_CHECK_MODULE_HPP_ -#include +#include #include #include #include -class StopCheckModule : public motion_utils::VehicleStopCheckerBase +class StopCheckModule : public autoware::motion_utils::VehicleStopCheckerBase { public: StopCheckModule(rclcpp::Node * node, double buffer_duration); diff --git a/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp index 0a55a5005dde1..febff8c2ee244 100644 --- a/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp +++ b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp @@ -50,8 +50,8 @@ class PoseInstabilityDetector : public rclcpp::Node }; explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - ThresholdValues calculate_threshold(double interval_sec); - void dead_reckon( + ThresholdValues calculate_threshold(double interval_sec) const; + static void dead_reckon( PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, const std::deque & twist_deque, Pose::SharedPtr & estimated_pose); @@ -60,7 +60,7 @@ class PoseInstabilityDetector : public rclcpp::Node void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); void callback_timer(); - std::deque clip_out_necessary_twist( + static std::deque clip_out_necessary_twist( const std::deque & twist_buffer, const rclcpp::Time & start_time, const rclcpp::Time & end_time); @@ -76,8 +76,6 @@ class PoseInstabilityDetector : public rclcpp::Node // parameters const double timer_period_; // [sec] - ThresholdValues threshold_values_; - const double heading_velocity_maximum_; // [m/s] const double heading_velocity_scale_factor_tolerance_; // [%] diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml index d658d1c2d437f..04e9747d3723b 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/pose_instability_detector/package.xml @@ -18,6 +18,7 @@ autoware_cmake ament_index_cpp + autoware_universe_utils diagnostic_msgs geometry_msgs nav_msgs @@ -25,7 +26,6 @@ rclcpp_components tf2 tf2_geometry_msgs - tier4_autoware_utils tier4_debug_msgs ament_cmake_cppcheck diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index fa7b2ecf16562..28398588809eb 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -14,7 +14,7 @@ #include "autoware/pose_instability_detector/pose_instability_detector.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -118,21 +118,21 @@ void PoseInstabilityDetector::callback_timer() prev_pose->header = prev_odometry_->header; prev_pose->pose = prev_odometry_->pose.pose; - Pose::SharedPtr DR_pose = std::make_shared(); - dead_reckon(prev_pose, latest_odometry_time, twist_buffer_, DR_pose); + Pose::SharedPtr dr_pose = std::make_shared(); + dead_reckon(prev_pose, latest_odometry_time, twist_buffer_, dr_pose); // compare dead reckoning pose and latest_odometry_ const Pose latest_ekf_pose = latest_odometry_->pose.pose; - const Pose ekf_to_DR = tier4_autoware_utils::inverseTransformPose(*DR_pose, latest_ekf_pose); - const geometry_msgs::msg::Point pos = ekf_to_DR.position; - const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_DR.orientation); + const Pose ekf_to_dr = autoware::universe_utils::inverseTransformPose(*dr_pose, latest_ekf_pose); + const geometry_msgs::msg::Point pos = ekf_to_dr.position; + const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_dr.orientation); const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; // publish diff_pose for debug PoseStamped diff_pose; diff_pose.header.stamp = latest_odometry_time; diff_pose.header.frame_id = "base_link"; - diff_pose.pose = ekf_to_DR; + diff_pose.pose = ekf_to_dr; diff_pose_pub_->publish(diff_pose); // publish diagnostics @@ -178,7 +178,7 @@ void PoseInstabilityDetector::callback_timer() } PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_threshold( - double interval_sec) + double interval_sec) const { // Calculate maximum longitudinal difference const double longitudinal_difference = @@ -229,7 +229,7 @@ PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_thre const double yaw_difference = roll_difference; // Set thresholds - ThresholdValues result_values; + ThresholdValues result_values{}; result_values.position_x = longitudinal_difference + pose_estimator_longitudinal_tolerance_; result_values.position_y = lateral_difference + pose_estimator_lateral_tolerance_; result_values.position_z = vertical_difference + pose_estimator_vertical_tolerance_; diff --git a/localization/tree_structured_parzen_estimator/CMakeLists.txt b/localization/tree_structured_parzen_estimator/CMakeLists.txt deleted file mode 100644 index 7b687a12a003c..0000000000000 --- a/localization/tree_structured_parzen_estimator/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tree_structured_parzen_estimator) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(tree_structured_parzen_estimator SHARED - src/tree_structured_parzen_estimator.cpp -) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_tpe - test/test_tpe.cpp - src/tree_structured_parzen_estimator.cpp - ) - target_include_directories(test_tpe PRIVATE include) - target_link_libraries(test_tpe) -endif() - -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/localization/tree_structured_parzen_estimator/README.md b/localization/tree_structured_parzen_estimator/README.md deleted file mode 100644 index 2b21e65929485..0000000000000 --- a/localization/tree_structured_parzen_estimator/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# tree_structured_parzen_estimator - -`tree_structured_parzen_estimator`` is a package for black-box optimization. - -This package does not have a node, it is just a library. diff --git a/localization/tree_structured_parzen_estimator/package.xml b/localization/tree_structured_parzen_estimator/package.xml deleted file mode 100644 index 96d2b9ecf54cc..0000000000000 --- a/localization/tree_structured_parzen_estimator/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - tree_structured_parzen_estimator - 0.1.0 - The tree_structured_parzen_estimator package - Yamato Ando - Masahiro Sakamoto - Shintaro Sakoda - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Ryu Yamamoto - Apache License 2.0 - Shintaro Sakoda - - ament_cmake_auto - autoware_cmake - - ament_cmake_cppcheck - ament_lint_auto - - - ament_cmake - - diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/camera_info_subscriber.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/camera_info_subscriber.hpp index f26a63ebd6317..e8bcdf98025cc 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/camera_info_subscriber.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/camera_info_subscriber.hpp @@ -31,17 +31,17 @@ class CameraInfoSubscriber explicit CameraInfoSubscriber(rclcpp::Node * node); - bool is_camera_info_ready() const; + [[nodiscard]] bool is_camera_info_ready() const; - bool is_camera_info_nullopt() const; + [[nodiscard]] bool is_camera_info_nullopt() const; - Eigen::Vector2i size() const; + [[nodiscard]] Eigen::Vector2i size() const; - Eigen::Matrix3f intrinsic() const; + [[nodiscard]] Eigen::Matrix3f intrinsic() const; // This member function DOES NOT check isCameraInfoReady() // If it it not ready, throw bad optional access - std::string get_frame_id() const; + [[nodiscard]] std::string get_frame_id() const; private: rclcpp::Subscription::SharedPtr sub_info_; diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp index 219c50f387cb5..447c101fd520a 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp @@ -31,25 +31,28 @@ struct Color } explicit Color(const cv::Scalar & rgb, float a = 1.0f) - : r(rgb[2] / 255.f), g(rgb[1] / 255.f), b(rgb[0] / 255.f), a(a) + : r(static_cast(rgb[2]) / 255.f), + g(static_cast(rgb[1]) / 255.f), + b(static_cast(rgb[0]) / 255.f), + a(a) { } - operator uint32_t() const + explicit operator uint32_t() const { union uchar4_uint32 { uint8_t rgba[4]; uint32_t u32; }; - uchar4_uint32 tmp; + uchar4_uint32 tmp{}; tmp.rgba[0] = static_cast(r * 255); tmp.rgba[1] = static_cast(g * 255); tmp.rgba[2] = static_cast(b * 255); tmp.rgba[3] = static_cast(a * 255); return tmp.u32; } - operator const cv::Scalar() const { return cv::Scalar(255 * b, 255 * g, 255 * r); } - operator const std_msgs::msg::ColorRGBA() const + explicit operator cv::Scalar() const { return {255 * b, 255 * g, 255 * r}; } + explicit operator std_msgs::msg::ColorRGBA() const { std_msgs::msg::ColorRGBA rgba; rgba.a = a; diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/gamma_converter.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/gamma_converter.hpp index 98c082e125547..9802c0abe4acd 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/gamma_converter.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/gamma_converter.hpp @@ -27,7 +27,8 @@ class GammaConverter { lut_ = cv::Mat(1, 256, CV_8UC1); for (int i = 0; i < 256; i++) { - lut_.at(0, i) = 256 * std::pow(i / 256.f, gamma); + lut_.at(0, i) = + static_cast(256 * std::pow(static_cast(i) / 256.f, gamma)); } } diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_plane.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_plane.hpp index 62a3695a9852f..61798ae6a4f04 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_plane.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_plane.hpp @@ -46,39 +46,39 @@ struct GroundPlane } } - float height() const { return xyz.z(); } + [[nodiscard]] float height() const { return xyz.z(); } - Eigen::Quaternionf align_with_slope(const Eigen::Quaternionf & q) const + [[nodiscard]] Eigen::Quaternionf align_with_slope(const Eigen::Quaternionf & q) const { return Eigen::Quaternionf{align_with_slope(q.toRotationMatrix())}; } - Eigen::Matrix3f align_with_slope(const Eigen::Matrix3f & R) const + [[nodiscard]] Eigen::Matrix3f align_with_slope(const Eigen::Matrix3f & R) const { - Eigen::Matrix3f R_; + Eigen::Matrix3f r; Eigen::Vector3f rz = this->normal; Eigen::Vector3f azimuth = R * Eigen::Vector3f::UnitX(); Eigen::Vector3f ry = (rz.cross(azimuth)).normalized(); Eigen::Vector3f rx = ry.cross(rz); - R_.col(0) = rx; - R_.col(1) = ry; - R_.col(2) = rz; - return R_; + r.col(0) = rx; + r.col(1) = ry; + r.col(2) = rz; + return r; } - Sophus::SE3f align_with_slope(const Sophus::SE3f & pose) const + [[nodiscard]] Sophus::SE3f align_with_slope(const Sophus::SE3f & pose) const { return {align_with_slope(pose.rotationMatrix()), pose.translation()}; } - Eigen::Affine3f align_with_slope(const Eigen::Affine3f & pose) const + [[nodiscard]] Eigen::Affine3f align_with_slope(const Eigen::Affine3f & pose) const { - Eigen::Matrix3f R = pose.rotation(); + Eigen::Matrix3f r = pose.rotation(); Eigen::Vector3f t = pose.translation(); - return Eigen::Translation3f(t) * align_with_slope(R); + return Eigen::Translation3f(t) * align_with_slope(r); } - Float32Array msg() const + [[nodiscard]] Float32Array msg() const { Float32Array array; for (int i = 0; i < 3; i++) array.data.push_back(xyz(i)); diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/filter/moving_averaging.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/filter/moving_averaging.hpp index 5b62c8fb79c33..aa90e52920ea5 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/filter/moving_averaging.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/filter/moving_averaging.hpp @@ -32,7 +32,7 @@ class MovingAveraging Eigen::Vector3f mean = Eigen::Vector3f::Zero(); for (const Eigen::Vector3f & v : buffer_) mean += v; - mean /= buffer_.size(); + mean /= static_cast(buffer_.size()); return mean.normalized(); } diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp index 92e5d939d0b7e..e8f7e6e081e43 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp @@ -61,8 +61,8 @@ class GroundServer : public rclcpp::Node private: const bool force_zero_tilt_; - const float R; - const int K; + const float R_; + const int K_; // Subscriber rclcpp::Subscription::SharedPtr sub_map_; diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/util.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/util.hpp index fe3285fb08e8a..8d9dc06447d62 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/util.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/util.hpp @@ -26,12 +26,14 @@ namespace yabloc::ground_server { -void upsample_line_string( +void inline upsample_line_string( const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to, pcl::PointCloud::Ptr cloud) { - Eigen::Vector3f f(from.x(), from.y(), from.z()); - Eigen::Vector3f t(to.x(), to.y(), to.z()); + Eigen::Vector3f f( + static_cast(from.x()), static_cast(from.y()), static_cast(from.z())); + Eigen::Vector3f t( + static_cast(to.x()), static_cast(to.y()), static_cast(to.z())); float length = (t - f).norm(); Eigen::Vector3f d = (t - f).normalized(); for (float l = 0; l < length; l += 0.5f) { @@ -41,7 +43,8 @@ void upsample_line_string( } }; -std::vector merge_indices(const std::vector & indices1, const std::vector & indices2) +std::vector inline merge_indices( + const std::vector & indices1, const std::vector & indices2) { std::unordered_set set; for (int i : indices1) set.insert(i); diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp index b044ae985c6c0..6956a7a7b04bb 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp @@ -54,17 +54,18 @@ class Ll2Decomposer : public rclcpp::Node void on_map(const LaneletMapBin & msg); - pcl::PointNormal to_point_normal( - const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) const; + static pcl::PointNormal to_point_normal( + const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to); - pcl::PointCloud split_line_strings( + static pcl::PointCloud split_line_strings( const lanelet::ConstLineStrings3d & line_strings); pcl::PointCloud load_bounding_boxes(const lanelet::PolygonLayer & polygons) const; - lanelet::ConstLineStrings3d extract_specified_line_string( - const lanelet::LineStringLayer & line_strings, const std::set & visible_labels); - lanelet::ConstPolygons3d extract_specified_polygon( + static lanelet::ConstLineStrings3d extract_specified_line_string( + const lanelet::LineStringLayer & line_string_layer, + const std::set & visible_labels); + static lanelet::ConstPolygons3d extract_specified_polygon( const lanelet::PolygonLayer & polygon_layer, const std::set & visible_labels); MarkerArray make_sign_marker_msg( diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index d4c8b71fa6b66..18038d80fbfca 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -17,6 +17,7 @@ rosidl_default_generators autoware_map_msgs + autoware_universe_utils cv_bridge geometry_msgs lanelet2_core @@ -29,7 +30,6 @@ sophus std_msgs tf2_ros - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp b/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp index 33a2279767107..222389c32ef31 100644 --- a/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp +++ b/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp @@ -51,8 +51,8 @@ Eigen::Matrix3f CameraInfoSubscriber::intrinsic() const if (!opt_info_.has_value()) { throw std::runtime_error("camera_info is not ready but it's accessed"); } - const Eigen::Matrix3d Kd_t = Eigen::Map>(opt_info_->k.data()); - return Kd_t.cast().transpose(); + const Eigen::Matrix3d kd_t = Eigen::Map>(opt_info_->k.data()); + return kd_t.cast().transpose(); } } // namespace yabloc::common diff --git a/localization/yabloc/yabloc_common/src/color.cpp b/localization/yabloc/yabloc_common/src/color.cpp index 485f655bd4151..6889f7238fb93 100644 --- a/localization/yabloc/yabloc_common/src/color.cpp +++ b/localization/yabloc/yabloc_common/src/color.cpp @@ -19,7 +19,9 @@ namespace yabloc::common::color_scale Color rainbow(float value) { // clang-format off - float r = 1.0f, g = 1.0f, b = 1.0f; + float r = 1.0f; + float g = 1.0f; + float b = 1.0f; value = std::clamp(value, 0.0f, 1.0f); if (value < 0.25f) { r = 0; g = 4 * (value); @@ -40,18 +42,12 @@ Color hsv_to_rgb(float h_, float s_, float v_) const float max = v_; const float min = max * (1.0f - s_); - if (h < 60) - return {max, h / 60 * (max - min) + min, min}; - else if (h < 120) - return {(120 - h) / 60 * (max - min) + min, max, min}; - else if (h < 180) - return {min, max, (h - 120) / 60 * (max - min) + min}; - else if (h < 240) - return {min, (240 - h) / 60 * (max - min) + min, max}; - else if (h < 300) - return {(h - 240) / 60 * (max - min) + min, min, max}; - else - return {max, min, (360 - h) / 60 * (max - min) + min}; + if (h < 60) return {max, h / 60 * (max - min) + min, min}; + if (h < 120) return {(120 - h) / 60 * (max - min) + min, max, min}; + if (h < 180) return {min, max, (h - 120) / 60 * (max - min) + min}; + if (h < 240) return {min, (240 - h) / 60 * (max - min) + min, max}; + if (h < 300) return {(h - 240) / 60 * (max - min) + min, min, max}; + return {max, min, (360 - h) / 60 * (max - min) + min}; } Color blue_red(float value) diff --git a/localization/yabloc/yabloc_common/src/cv_decompress.cpp b/localization/yabloc/yabloc_common/src/cv_decompress.cpp index dc232488e147c..bf26908c9f0b0 100644 --- a/localization/yabloc/yabloc_common/src/cv_decompress.cpp +++ b/localization/yabloc/yabloc_common/src/cv_decompress.cpp @@ -28,16 +28,16 @@ cv::Mat decompress_image(const sensor_msgs::msg::CompressedImage & compressed_im cv::Mat raw_image; const std::string & format = compressed_img.format; - const std::string encoding = format.substr(0, format.find(";")); + const std::string encoding = format.substr(0, format.find(';')); - constexpr int DECODE_GRAY = 0; - constexpr int DECODE_RGB = 1; + constexpr int decode_gray = 0; + constexpr int decode_rgb = 1; bool encoding_is_bayer = encoding.find("bayer") != std::string::npos; if (!encoding_is_bayer) { - return cv::imdecode(cv::Mat(compressed_img.data), DECODE_RGB); + return cv::imdecode(cv::Mat(compressed_img.data), decode_rgb); } - raw_image = cv::imdecode(cv::Mat(compressed_img.data), DECODE_GRAY); + raw_image = cv::imdecode(cv::Mat(compressed_img.data), decode_gray); // TODO(KYabuuchi) integrate with implementation in the sensing/perception component if (encoding == "bayer_rggb8") { diff --git a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp index 9940dca1fd62b..f440c5da683a3 100644 --- a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp +++ b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp @@ -20,8 +20,8 @@ pcl::PointCloud extract_near_line_segments( const pcl::PointCloud & line_segments, const Sophus::SE3f & transform, const float max_range) { - constexpr double sqrt_two = 1.41421356237309504880; - const Eigen::Vector3f pose_vector = transform.translation(); + const double sqrt_two = 1.41421356237309504880; + const Eigen::Vector3f & pose_vector = transform.translation(); // All line segments contained in a square with max_range on one side must be taken out, // so pick up those that are closer than the **diagonals** of the square. @@ -42,11 +42,9 @@ pcl::PointCloud extract_near_line_segments( }; pcl::PointCloud dst; - for (const pcl::PointNormal & pn : line_segments) { - if (check_intersection(pn)) { - dst.push_back(pn); - } - } + std::copy_if( + line_segments.begin(), line_segments.end(), std::back_inserter(dst), + [check_intersection](const auto & pn) { return check_intersection(pn); }); return dst; } diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp index d2d1929991442..1366ecaebccf1 100644 --- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp +++ b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp @@ -28,13 +28,15 @@ #include #include +#include + namespace yabloc::ground_server { GroundServer::GroundServer(const rclcpp::NodeOptions & options) : Node("ground_server", options), force_zero_tilt_(declare_parameter("force_zero_tilt")), - R(declare_parameter("R")), - K(declare_parameter("K")) + R_(static_cast(declare_parameter("R"))), + K_(static_cast(declare_parameter("K"))) { using std::placeholders::_1; using std::placeholders::_2; @@ -147,8 +149,8 @@ float GroundServer::estimate_height_simply(const geometry_msgs::msg::Point & poi { // NOTE: Sometimes it might give not-accurate height constexpr float sq_radius = 3.0 * 3.0; - const float x = point.x; - const float y = point.y; + const auto x = static_cast(point.x); + const auto y = static_cast(point.y); float height = std::numeric_limits::infinity(); for (const auto & p : cloud_->points) { @@ -186,12 +188,12 @@ std::vector GroundServer::estimate_inliers_by_ransac(const std::vector GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point) { // Because height_filter_ is always initialized, getValue does not return nullopt - const float predicted_z = height_filter_.getValue().value(); - const pcl::PointXYZ xyz(point.x, point.y, predicted_z); + const float predicted_z = static_cast(height_filter_.getValue().value()); + const pcl::PointXYZ xyz(static_cast(point.x), static_cast(point.y), predicted_z); std::vector raw_indices; std::vector distances; - kdtree_->nearestKSearch(xyz, K, raw_indices, distances); + kdtree_->nearestKSearch(xyz, K_, raw_indices, distances); std::vector indices = estimate_inliers_by_ransac(raw_indices); @@ -206,7 +208,7 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point) // NOTE: I forgot why I don't use coefficients computed by SACSegmentation Eigen::Vector4f plane_parameter; - float curvature; + float curvature = NAN; pcl::solvePlaneParameters(covariance, centroid, plane_parameter, curvature); Eigen::Vector3f normal = plane_parameter.topRows(3).normalized(); @@ -229,15 +231,16 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point) const Eigen::Vector3f filt_normal = normal_filter_.update(normal); GroundPlane plane; - plane.xyz = Eigen::Vector3f(point.x, point.y, predicted_z); + plane.xyz = + Eigen::Vector3f(static_cast(point.x), static_cast(point.y), predicted_z); plane.normal = filt_normal; // Compute z value by intersection of estimated plane and orthogonal line { Eigen::Vector3f center = centroid.topRows(3); float inner = center.dot(plane.normal); - float px_nx = point.x * plane.normal.x(); - float py_ny = point.y * plane.normal.y(); + float px_nx = static_cast(point.x) * plane.normal.x(); + float py_ny = static_cast(point.y) * plane.normal.y(); plane.xyz.z() = (inner - px_nx - py_ny) / plane.normal.z(); } diff --git a/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp b/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp index d4b134cf3a203..5ce6804c7bdc6 100644 --- a/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp +++ b/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp @@ -31,9 +31,9 @@ pcl::PointCloud sample_from_polygons(const lanelet::PolygonLayer for (const lanelet::ConstPolygon3d & polygon : polygons) { for (const lanelet::ConstPoint3d & p : polygon) { pcl::PointXYZ xyz; - xyz.x = p.x(); - xyz.y = p.y(); - xyz.z = p.z(); + xyz.x = static_cast(p.x()); + xyz.y = static_cast(p.y()); + xyz.z = static_cast(p.z()); raw_cloud.push_back(xyz); } } @@ -45,9 +45,9 @@ void push_back_line( { Eigen::Vector3f f = from.getVector3fMap(); Eigen::Vector3f t = to.getVector3fMap(); - const float L = (f - t).norm(); + const float vec_len = (f - t).norm(); - for (float l = 0.f; l < L; l += 0.25f) { + for (float l = 0.f; l < vec_len; l += 0.25f) { Eigen::Vector3f xyz = f + (t - f) * l; dst_cloud.emplace_back(xyz.x(), xyz.y(), xyz.z()); } @@ -56,19 +56,20 @@ void push_back_line( void push_back_contour( pcl::PointCloud & dst_cloud, const pcl::PointCloud & vertices) { - const int N = vertices.size(); - for (int i = 0; i < N - 1; ++i) { + const int n = static_cast(vertices.size()); + for (int i = 0; i < n - 1; ++i) { push_back_line(dst_cloud, vertices.at(i), vertices.at(i + 1)); } - push_back_line(dst_cloud, vertices.at(0), vertices.at(N - 1)); + push_back_line(dst_cloud, vertices.at(0), vertices.at(n - 1)); } pcl::PointCloud shrink_vertices( const pcl::PointCloud & vertices, float rate) { - Eigen::Vector3f center = Eigen::Vector3f::Zero(); - for (const pcl::PointXYZ p : vertices) center += p.getVector3fMap(); - center /= vertices.size(); + Eigen::Vector3f center = std::accumulate( + vertices.begin(), vertices.end(), Eigen::Vector3f::Zero().eval(), + [](const Eigen::Vector3f & acc, const pcl::PointXYZ & p) { return acc + p.getVector3fMap(); }); + center /= static_cast(vertices.size()); pcl::PointCloud dst_cloud; for (const pcl::PointXYZ p : vertices) { diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index 1126d6260b911..3ea23ded92efa 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -14,8 +14,8 @@ #include "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" +#include #include -#include #include #include @@ -44,7 +44,7 @@ Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to [this](const std::string & param_name, std::set & labels) -> void { this->template declare_parameter>(param_name); auto label_array = get_parameter(param_name).as_string_array(); - for (auto l : label_array) labels.insert(l); + for (const auto & l : label_array) labels.insert(l); }; load_lanelet2_labels("road_marking_labels", road_marking_labels_); @@ -86,14 +86,14 @@ pcl::PointCloud Ll2Decomposer::load_bounding_boxes( for (const lanelet::ConstPolygon3d & polygon : polygons) { if (!polygon.hasAttribute(lanelet::AttributeName::Type)) continue; - lanelet::Attribute attr = polygon.attribute(lanelet::AttributeName::Type); + const lanelet::Attribute & attr = polygon.attribute(lanelet::AttributeName::Type); if (bounding_box_labels_.count(attr.value()) == 0) continue; for (const lanelet::ConstPoint3d & p : polygon) { pcl::PointXYZL xyzl; - xyzl.x = p.x(); - xyzl.y = p.y(); - xyzl.z = p.z(); + xyzl.x = static_cast(p.x()); + xyzl.y = static_cast(p.y()); + xyzl.z = static_cast(p.z()); xyzl.label = index; cloud.push_back(xyzl); } @@ -151,7 +151,7 @@ lanelet::ConstLineStrings3d Ll2Decomposer::extract_specified_line_string( lanelet::ConstLineStrings3d line_strings; for (const lanelet::ConstLineString3d & line : line_string_layer) { if (!line.hasAttribute(lanelet::AttributeName::Type)) continue; - lanelet::Attribute attr = line.attribute(lanelet::AttributeName::Type); + const lanelet::Attribute & attr = line.attribute(lanelet::AttributeName::Type); if (visible_labels.count(attr.value()) == 0) continue; line_strings.push_back(line); } @@ -164,7 +164,7 @@ lanelet::ConstPolygons3d Ll2Decomposer::extract_specified_polygon( lanelet::ConstPolygons3d polygons; for (const lanelet::ConstPolygon3d & polygon : polygon_layer) { if (!polygon.hasAttribute(lanelet::AttributeName::Type)) continue; - lanelet::Attribute attr = polygon.attribute(lanelet::AttributeName::Type); + const lanelet::Attribute & attr = polygon.attribute(lanelet::AttributeName::Type); if (visible_labels.count(attr.value()) == 0) continue; polygons.push_back(polygon); } @@ -172,15 +172,15 @@ lanelet::ConstPolygons3d Ll2Decomposer::extract_specified_polygon( } pcl::PointNormal Ll2Decomposer::to_point_normal( - const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) const + const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) { pcl::PointNormal pn; - pn.x = from.x(); - pn.y = from.y(); - pn.z = from.z(); - pn.normal_x = to.x(); - pn.normal_y = to.y(); - pn.normal_z = to.z(); + pn.x = static_cast(from.x()); + pn.y = static_cast(from.y()); + pn.z = static_cast(from.z()); + pn.normal_x = static_cast(to.x()); + pn.normal_y = static_cast(to.y()); + pn.normal_z = static_cast(to.z()); return pn; } @@ -198,7 +198,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_sign_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = tier4_autoware_utils::createMarkerColor(0.6f, 0.6f, 0.6f, 0.999f); + marker.color = autoware::universe_utils::createMarkerColor(0.6f, 0.6f, 0.6f, 0.999f); marker.scale.x = 0.1; marker.ns = ns; marker.id = id++; @@ -228,7 +228,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_polygon_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = tier4_autoware_utils::createMarkerColor(0.4f, 0.4f, 0.8f, 0.999f); + marker.color = autoware::universe_utils::createMarkerColor(0.4f, 0.4f, 0.8f, 0.999f); marker.scale.x = 0.2; marker.ns = ns; marker.id = id++; diff --git a/localization/yabloc/yabloc_common/src/pose_conversions.cpp b/localization/yabloc/yabloc_common/src/pose_conversions.cpp index 6ad914eeaf916..374871bc96f0c 100644 --- a/localization/yabloc/yabloc_common/src/pose_conversions.cpp +++ b/localization/yabloc/yabloc_common/src/pose_conversions.cpp @@ -37,8 +37,11 @@ Eigen::Affine3f pose_to_affine(const geometry_msgs::msg::Pose & pose) { const auto pos = pose.position; const auto ori = pose.orientation; - Eigen::Translation3f t(pos.x, pos.y, pos.z); - Eigen::Quaternionf q(ori.w, ori.x, ori.y, ori.z); + Eigen::Translation3f t( + static_cast(pos.x), static_cast(pos.y), static_cast(pos.z)); + Eigen::Quaternionf q( + static_cast(ori.w), static_cast(ori.x), static_cast(ori.y), + static_cast(ori.z)); return t * q; } @@ -46,8 +49,11 @@ Sophus::SE3f pose_to_se3(const geometry_msgs::msg::Pose & pose) { auto ori = pose.orientation; auto pos = pose.position; - Eigen::Quaternionf q(ori.w, ori.x, ori.y, ori.z); - Eigen::Vector3f t(pos.x, pos.y, pos.z); + Eigen::Quaternionf q( + static_cast(ori.w), static_cast(ori.x), static_cast(ori.y), + static_cast(ori.z)); + Eigen::Vector3f t( + static_cast(pos.x), static_cast(pos.y), static_cast(pos.z)); return {q, t}; } diff --git a/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp b/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp index baf93ad295e9c..3aeaefad12629 100644 --- a/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp +++ b/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp @@ -35,26 +35,26 @@ std::optional StaticTfSubscriber::se3f( std::optional StaticTfSubscriber::operator()( const std::string & frame_id, const std::string & parent_frame_id) { - std::optional extrinsic_{std::nullopt}; + std::optional extrinsic{std::nullopt}; try { geometry_msgs::msg::TransformStamped ts = tf_buffer_->lookupTransform(parent_frame_id, frame_id, tf2::TimePointZero); Eigen::Vector3f p; - p.x() = ts.transform.translation.x; - p.y() = ts.transform.translation.y; - p.z() = ts.transform.translation.z; + p.x() = static_cast(ts.transform.translation.x); + p.y() = static_cast(ts.transform.translation.y); + p.z() = static_cast(ts.transform.translation.z); Eigen::Quaternionf q; - q.w() = ts.transform.rotation.w; - q.x() = ts.transform.rotation.x; - q.y() = ts.transform.rotation.y; - q.z() = ts.transform.rotation.z; - extrinsic_ = Eigen::Affine3f::Identity(); - extrinsic_->translation() = p; - extrinsic_->matrix().topLeftCorner(3, 3) = q.toRotationMatrix(); - } catch (tf2::TransformException & ex) { + q.w() = static_cast(ts.transform.rotation.w); + q.x() = static_cast(ts.transform.rotation.x); + q.y() = static_cast(ts.transform.rotation.y); + q.z() = static_cast(ts.transform.rotation.z); + extrinsic = Eigen::Affine3f::Identity(); + extrinsic->translation() = p; + extrinsic->matrix().topLeftCorner(3, 3) = q.toRotationMatrix(); + } catch (const tf2::TransformException & ex) { } - return extrinsic_; + return extrinsic; } } // namespace yabloc::common diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp index c3ac2475f7e08..18b06b08cb55e 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp @@ -27,7 +27,7 @@ struct Histogram EIGEN_MAKE_ALIGNED_OPERATOR_NEW explicit Histogram(int bin = 10) : bin(bin) { data = Eigen::MatrixXf::Zero(3, bin); } - Eigen::MatrixXf eval() const + [[nodiscard]] Eigen::MatrixXf eval() const { float sum = data.sum(); if (sum < 1e-6f) throw std::runtime_error("invalid division"); @@ -37,7 +37,9 @@ struct Histogram void add(const cv::Vec3b & rgb) { for (int ch = 0; ch < 3; ++ch) { - int index = std::clamp(static_cast(rgb[ch] * bin / 255.f), 0, bin - 1); + int index = std::clamp( + static_cast(static_cast(rgb[ch]) * static_cast(bin) / 255.f), 0, + bin - 1); data(ch, index) += 1.0f; } } diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp index 7d644376ba591..d6d737a2808c4 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp @@ -76,7 +76,7 @@ class Lanelet2Overlay : public rclcpp::Node void draw_overlay( const cv::Mat & image, const std::optional & pose, const rclcpp::Time & stamp); void draw_overlay_line_segments( - cv::Mat & image, const Pose & pose, const LineSegments & line_segments); + cv::Mat & image, const Pose & pose, const LineSegments & near_segments); void make_vis_marker(const LineSegments & ls, const Pose & pose, const rclcpp::Time & stamp); }; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp index 761c581200369..7da51cc5daf35 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp @@ -51,8 +51,8 @@ class LineSegmentDetector : public rclcpp::Node cv::Ptr line_segment_detector_; - std::vector remove_too_outer_elements( - const cv::Mat & lines, const cv::Size & size) const; + static std::vector remove_too_outer_elements( + const cv::Mat & lines, const cv::Size & size); void on_image(const sensor_msgs::msg::Image & msg); void execute(const cv::Mat & image, const rclcpp::Time & stamp); }; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp index c8f036f2b12de..6b76db6751d26 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp @@ -63,14 +63,14 @@ class SegmentFilter : public rclcpp::Node bool define_project_func(); pcl::PointCloud project_lines( - const pcl::PointCloud & lines, const std::set & indices, + const pcl::PointCloud & points, const std::set & indices, bool negative = false) const; - std::set filter_by_mask( + static std::set filter_by_mask( const cv::Mat & mask, const pcl::PointCloud & edges); cv::Point2i to_cv_point(const Eigen::Vector3f & v) const; - void execute(const PointCloud2 & msg1, const Image & msg2); + void execute(const PointCloud2 & line_segments_msg, const Image & segment_msg); bool is_near_element(const pcl::PointNormal & pn, pcl::PointNormal & truncated_pn) const; }; diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 416acfdc76a16..453ae58b9b33b 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -16,13 +16,13 @@ ament_cmake autoware_cmake + autoware_universe_utils cv_bridge pcl_conversions rclcpp rclcpp_components sensor_msgs std_msgs - tier4_autoware_utils visualization_msgs yabloc_common diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp index f8d4c74dd5bf8..9539e44f61276 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp @@ -15,9 +15,9 @@ #include "yabloc_image_processing/graph_segment/graph_segment.hpp" #include "yabloc_image_processing/graph_segment/histogram.hpp" +#include #include #include -#include #include #include @@ -25,8 +25,9 @@ namespace yabloc::graph_segment { GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) : Node("graph_segment", options), - target_height_ratio_(declare_parameter("target_height_ratio")), - target_candidate_box_width_(declare_parameter("target_candidate_box_width")) + target_height_ratio_(static_cast(declare_parameter("target_height_ratio"))), + target_candidate_box_width_( + static_cast(declare_parameter("target_candidate_box_width"))) { using std::placeholders::_1; @@ -38,8 +39,8 @@ GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) pub_debug_image_ = create_publisher("~/debug/segmented_image", 10); const double sigma = declare_parameter("sigma"); - const float k = declare_parameter("k"); - const int min_size = declare_parameter("min_size"); + const float k = static_cast(declare_parameter("k")); + const int min_size = static_cast(declare_parameter("min_size")); segmentation_ = cv::ximgproc::segmentation::createGraphSegmentation(sigma, k, min_size); // additional area pickup module @@ -52,16 +53,20 @@ GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) cv::Vec3b random_hsv(int index) { // It generates colors that are not too bright or too vivid, but rich in hues. - double base = static_cast(index); - return cv::Vec3b(std::fmod(base * 0.7, 1.0) * 180, 0.7 * 255, 0.5 * 255); + auto base = static_cast(index); + return { + static_cast(std::fmod(base * 0.7, 1.0) * 180), + static_cast(0.7 * 255), static_cast(0.5 * 255)}; }; int GraphSegment::search_most_road_like_class(const cv::Mat & segmented) const { - const int W = target_candidate_box_width_; - const float R = target_height_ratio_; - cv::Point2i target_px(segmented.cols * 0.5, segmented.rows * R); - cv::Rect2i rect(target_px + cv::Point2i(-W, -W), target_px + cv::Point2i(W, W)); + const int bw = target_candidate_box_width_; + const float r = target_height_ratio_; + cv::Point2i target_px( + static_cast(static_cast(segmented.cols) * 0.5), + static_cast(static_cast(segmented.rows) * r)); + cv::Rect2i rect(target_px + cv::Point2i(-bw, -bw), target_px + cv::Point2i(bw, bw)); std::unordered_map areas; std::unordered_set candidates; @@ -69,7 +74,7 @@ int GraphSegment::search_most_road_like_class(const cv::Mat & segmented) const const int * seg_ptr = segmented.ptr(h); for (int w = 0; w < segmented.cols; w++) { int key = seg_ptr[w]; - if (areas.count(key) == 0) areas[key] = 0; + areas.try_emplace(key, 0); areas[key]++; if (rect.contains(cv::Point2i{w, h})) candidates.insert(key); } @@ -93,7 +98,7 @@ void GraphSegment::on_image(const Image & msg) cv::resize(image, resized, cv::Size(), 0.5, 0.5); // Execute graph-based segmentation - tier4_autoware_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; cv::Mat segmented; segmentation_->processImage(resized, segmented); RCLCPP_INFO_STREAM(get_logger(), "segmentation time: " << stop_watch.toc() * 1000 << "[ms]"); @@ -111,8 +116,8 @@ void GraphSegment::on_image(const Image & msg) cv::Mat debug_image = cv::Mat::zeros(resized.size(), CV_8UC3); for (int h = 0; h < resized.rows; h++) { // NOTE: Accessing through ptr() is faster than at() - cv::Vec3b * const debug_image_ptr = debug_image.ptr(h); - uchar * const output_image_ptr = output_image.ptr(h); + auto * const debug_image_ptr = debug_image.ptr(h); + auto * const output_image_ptr = output_image.ptr(h); const int * const segmented_image_ptr = segmented.ptr(h); for (int w = 0; w < resized.cols; w++) { @@ -148,10 +153,10 @@ void GraphSegment::draw_and_publish_image( // Draw target rectangle { - const int W = target_candidate_box_width_; - const float R = target_height_ratio_; - cv::Point2i target(size.width / 2, size.height * R); - cv::Rect2i rect(target + cv::Point2i(-W, -W), target + cv::Point2i(W, W)); + const int w = target_candidate_box_width_; + const float r = target_height_ratio_; + cv::Point2i target(size.width / 2, static_cast(static_cast(size.height) * r)); + cv::Rect2i rect(target + cv::Point2i(-w, -w), target + cv::Point2i(w, w)); cv::rectangle(show_image, rect, cv::Scalar::all(0), 2); } diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp index 85ce9e354362a..c7bf41bff459b 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp @@ -37,7 +37,7 @@ std::set SimilarAreaSearcher::search( for (int h = 0; h < rgb_image.rows; h++) { const int * seg_ptr = segmented.ptr(h); - const cv::Vec3b * rgb_ptr = rgb_image.ptr(h); + const auto * rgb_ptr = rgb_image.ptr(h); for (int w = 0; w < rgb_image.cols; w++) { int key = seg_ptr[w]; diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp index 107d861364038..d31b19406e320 100644 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp @@ -72,11 +72,11 @@ void Lanelet2Overlay::on_image(const sensor_msgs::msg::Image & msg) // Search synchronized pose float min_abs_dt = std::numeric_limits::max(); std::optional synched_pose{std::nullopt}; - for (auto pose : pose_buffer_) { + for (const auto & pose : pose_buffer_) { auto dt = (rclcpp::Time(pose.header.stamp) - stamp); auto abs_dt = std::abs(dt.seconds()); if (abs_dt < min_abs_dt) { - min_abs_dt = abs_dt; + min_abs_dt = static_cast(abs_dt); synched_pose = pose.pose; } } @@ -93,19 +93,17 @@ void Lanelet2Overlay::on_line_segments(const PointCloud2 & msg) // Search synchronized pose float min_dt = std::numeric_limits::max(); geometry_msgs::msg::PoseStamped synched_pose; - for (auto pose : pose_buffer_) { + for (const auto & pose : pose_buffer_) { auto dt = (rclcpp::Time(pose.header.stamp) - stamp); auto abs_dt = std::abs(dt.seconds()); if (abs_dt < min_dt) { - min_dt = abs_dt; + min_dt = static_cast(abs_dt); synched_pose = pose; } } if (min_dt > 0.1) return; auto latest_pose_stamp = rclcpp::Time(pose_buffer_.back().header.stamp); - std::vector a; - LineSegments line_segments_cloud; pcl::fromROSMsg(msg, line_segments_cloud); make_vis_marker(line_segments_cloud, synched_pose.pose, stamp); @@ -139,32 +137,35 @@ void Lanelet2Overlay::draw_overlay_line_segments( if (!camera_extrinsic_.has_value()) return; if (!info_.has_value()) return; - Eigen::Matrix3f K = + Eigen::Matrix3f k = Eigen::Map >(info_->k.data()).cast().transpose(); - Eigen::Affine3f T = camera_extrinsic_.value(); + Eigen::Affine3f t = camera_extrinsic_.value(); Eigen::Affine3f transform = ground_plane_.align_with_slope(common::pose_to_affine(pose)); - auto projectLineSegment = - [K, T, transform]( + auto project_line_segment = + [k, t, transform]( const Eigen::Vector3f & p1, const Eigen::Vector3f & p2) -> std::tuple { - Eigen::Vector3f from_camera1 = K * T.inverse() * transform.inverse() * p1; - Eigen::Vector3f from_camera2 = K * T.inverse() * transform.inverse() * p2; - constexpr float EPSILON = 0.1f; - bool p1_is_visible = from_camera1.z() > EPSILON; - bool p2_is_visible = from_camera2.z() > EPSILON; + Eigen::Vector3f from_camera1 = k * t.inverse() * transform.inverse() * p1; + Eigen::Vector3f from_camera2 = k * t.inverse() * transform.inverse() * p2; + constexpr float epsilon = 0.1f; + bool p1_is_visible = from_camera1.z() > epsilon; + bool p2_is_visible = from_camera2.z() > epsilon; if ((!p1_is_visible) && (!p2_is_visible)) return {false, cv::Point2i{}, cv::Point2i{}}; - Eigen::Vector3f uv1, uv2; + Eigen::Vector3f uv1; + Eigen::Vector3f uv2; if (p1_is_visible) uv1 = from_camera1 / from_camera1.z(); if (p2_is_visible) uv2 = from_camera2 / from_camera2.z(); if ((p1_is_visible) && (p2_is_visible)) - return {true, cv::Point2i(uv1.x(), uv1.y()), cv::Point2i(uv2.x(), uv2.y())}; + return { + true, cv::Point2i(static_cast(uv1.x()), static_cast(uv1.y())), + cv::Point2i(static_cast(uv2.x()), static_cast(uv2.y()))}; Eigen::Vector3f tangent = from_camera2 - from_camera1; - float mu = (EPSILON - from_camera1.z()) / (tangent.z()); + float mu = (epsilon - from_camera1.z()) / (tangent.z()); if (!p1_is_visible) { from_camera1 = from_camera1 + mu * tangent; uv1 = from_camera1 / from_camera1.z(); @@ -173,11 +174,13 @@ void Lanelet2Overlay::draw_overlay_line_segments( from_camera2 = from_camera1 + mu * tangent; uv2 = from_camera2 / from_camera2.z(); } - return {true, cv::Point2i(uv1.x(), uv1.y()), cv::Point2i(uv2.x(), uv2.y())}; + return { + true, cv::Point2i(static_cast(uv1.x()), static_cast(uv1.y())), + cv::Point2i(static_cast(uv2.x()), static_cast(uv2.y()))}; }; for (const pcl::PointNormal & pn : near_segments) { - auto [success, u1, u2] = projectLineSegment(pn.getVector3fMap(), pn.getNormalVector3fMap()); + auto [success, u1, u2] = project_line_segment(pn.getVector3fMap(), pn.getNormalVector3fMap()); if (success) cv::line(image, u1, u2, cv::Scalar(0, 255, 255), 2); } } @@ -197,7 +200,8 @@ void Lanelet2Overlay::make_vis_marker( marker.color.a = 0.7f; for (const auto pn : ls) { - geometry_msgs::msg::Point p1, p2; + geometry_msgs::msg::Point p1; + geometry_msgs::msg::Point p2; p1.x = pn.x; p1.y = pn.y; p1.z = pn.z; diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index c613642628499..c5cec31e24844 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -14,8 +14,8 @@ #include "yabloc_image_processing/line_segment_detector/line_segment_detector.hpp" +#include #include -#include #include #include @@ -53,7 +53,7 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st cv::Mat lines; { - tier4_autoware_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; line_segment_detector_->detect(gray_image, lines); if (lines.size().width != 0) { line_segment_detector_->drawSegments(gray_image, lines); @@ -67,7 +67,8 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st std::vector filtered_lines = remove_too_outer_elements(lines, image.size()); for (const cv::Mat & xy_xy : filtered_lines) { - Eigen::Vector3f xy1, xy2; + Eigen::Vector3f xy1; + Eigen::Vector3f xy2; xy1 << xy_xy.at(0), xy_xy.at(1), 0; xy2 << xy_xy.at(2), xy_xy.at(3), 0; pcl::PointNormal pn; @@ -79,7 +80,7 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st } std::vector LineSegmentDetector::remove_too_outer_elements( - const cv::Mat & lines, const cv::Size & size) const + const cv::Mat & lines, const cv::Size & size) { std::vector rect_vector; rect_vector.emplace_back(0, 0, size.width, 3); diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp index 7e06de81fbd18..70e1bf2d6e6e2 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp @@ -30,11 +30,11 @@ LineSegmentsOverlay::LineSegmentsOverlay(const rclcpp::NodeOptions & options) using std::placeholders::_1; auto cb_image = std::bind(&LineSegmentsOverlay::on_image, this, _1); - auto cb_line_segments_ = std::bind(&LineSegmentsOverlay::on_line_segments, this, _1); + auto cb_line_segments = std::bind(&LineSegmentsOverlay::on_line_segments, this, _1); sub_image_ = create_subscription("~/input/image_raw", 10, cb_image); sub_line_segments_ = - create_subscription("~/input/line_segments", 10, cb_line_segments_); + create_subscription("~/input/line_segments", 10, cb_line_segments); pub_debug_image_ = create_publisher("~/debug/image_with_colored_line_segments", 10); } @@ -73,8 +73,7 @@ void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & l LineSegments line_segments_cloud; pcl::fromROSMsg(*line_segments_msg, line_segments_cloud); - for (size_t index = 0; index < line_segments_cloud.size(); ++index) { - const LineSegment & pn = line_segments_cloud.at(index); + for (auto & pn : line_segments_cloud) { Eigen::Vector3f xy1 = pn.getVector3fMap(); Eigen::Vector3f xy2 = pn.getNormalVector3fMap(); @@ -83,7 +82,9 @@ void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & l color = cv::Scalar(0, 0, 255); // Red } - cv::line(image, cv::Point(xy1(0), xy1(1)), cv::Point(xy2(0), xy2(1)), color, 2); + cv::line( + image, cv::Point(static_cast(xy1(0)), static_cast(xy1(1))), + cv::Point(static_cast(xy2(0)), static_cast(xy2(1))), color, 2); } common::publish_image(*pub_debug_image_, image, stamp); diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp index df0aa7d65c617..b844fb69285b5 100644 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp @@ -25,11 +25,11 @@ namespace yabloc::segment_filter { SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) : Node("segment_filter", options), - image_size_(declare_parameter("image_size")), - max_range_(declare_parameter("max_range")), - min_segment_length_(declare_parameter("min_segment_length")), - max_segment_distance_(declare_parameter("max_segment_distance")), - max_lateral_distance_(declare_parameter("max_lateral_distance")), + image_size_(static_cast(declare_parameter("image_size"))), + max_range_(static_cast(declare_parameter("max_range"))), + min_segment_length_(static_cast(declare_parameter("min_segment_length"))), + max_segment_distance_(static_cast(declare_parameter("max_segment_distance"))), + max_lateral_distance_(static_cast(declare_parameter("max_lateral_distance"))), info_(this), synchro_subscriber_(this, "~/input/line_segments_cloud", "~/input/graph_segmented"), tf_subscriber_(this->get_clock()) @@ -47,9 +47,12 @@ SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) cv::Point2i SegmentFilter::to_cv_point(const Eigen::Vector3f & v) const { - cv::Point pt; - pt.x = -v.y() / max_range_ * image_size_ * 0.5f + image_size_ / 2; - pt.y = -v.x() / max_range_ * image_size_ * 0.5f + image_size_; + cv::Point2i pt; + pt.x = static_cast( + -v.y() / max_range_ * static_cast(image_size_) * 0.5f + + static_cast(image_size_) / 2.0); + pt.y = static_cast( + -v.x() / max_range_ * static_cast(image_size_) * 0.5f + static_cast(image_size_)); return pt; } @@ -149,7 +152,7 @@ void SegmentFilter::execute(const PointCloud2 & line_segments_msg, const Image & pcl::PointXYZLNormal pln; pln.getVector3fMap() = pn.getVector3fMap(); pln.getNormalVector3fMap() = pn.getNormalVector3fMap(); - if (indices.count(index) > 0) + if (indices.count(static_cast(index)) > 0) pln.label = 255; else pln.label = 0; @@ -194,7 +197,7 @@ std::set get_unique_pixel_value(cv::Mat & image) auto last = std::unique(begin, image.end()); std::sort(begin, last); last = std::unique(begin, last); - return std::set(begin, last); + return {begin, last}; } pcl::PointCloud SegmentFilter::project_lines( @@ -204,9 +207,9 @@ pcl::PointCloud SegmentFilter::project_lines( pcl::PointCloud projected_points; for (size_t index = 0; index < points.size(); ++index) { if (negative) { - if (indices.count(index) > 0) continue; + if (indices.count(static_cast(index)) > 0) continue; } else { - if (indices.count(index) == 0) continue; + if (indices.count(static_cast(index)) == 0) continue; } pcl::PointNormal truncated_pn = points.at(index); @@ -254,9 +257,10 @@ std::set SegmentFilter::filter_by_mask( auto & pn = edges.at(i); Eigen::Vector3f p1 = pn.getVector3fMap(); Eigen::Vector3f p2 = pn.getNormalVector3fMap(); - cv::Scalar color = cv::Scalar::all(i + 1); + cv::Scalar color = cv::Scalar::all(static_cast(i + 1)); cv::line( - line_image, cv::Point2i(p1.x(), p1.y()), cv::Point2i(p2.x(), p2.y()), color, 1, + line_image, cv::Point2i(static_cast(p1.x()), static_cast(p1.y())), + cv::Point2i(static_cast(p2.x()), static_cast(p2.y())), color, 1, cv::LineTypes::LINE_4); } @@ -275,7 +279,7 @@ std::set SegmentFilter::filter_by_mask( // Extract edges within masks std::set reliable_indices; for (size_t i = 0; i < edges.size(); i++) { - if (pixel_values.count(i + 1) != 0) reliable_indices.insert(i); + if (pixel_values.count(i + 1) != 0) reliable_indices.insert(static_cast(i)); } return reliable_indices; diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 0714b6c8091c8..5590387e5ba26 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include #include -#include #include #include @@ -39,8 +39,8 @@ class UndistortNode : public rclcpp::Node explicit UndistortNode(const rclcpp::NodeOptions & options) : Node("undistort", options), - OUTPUT_WIDTH(declare_parameter("width")), - OVERRIDE_FRAME_ID(declare_parameter("override_frame_id")) + OUTPUT_WIDTH_(static_cast(declare_parameter("width"))), + OVERRIDE_FRAME_ID_(declare_parameter("override_frame_id")) { using std::placeholders::_1; @@ -63,8 +63,8 @@ class UndistortNode : public rclcpp::Node } private: - const int OUTPUT_WIDTH; - const std::string OVERRIDE_FRAME_ID; + const int OUTPUT_WIDTH_; + const std::string OVERRIDE_FRAME_ID_; rclcpp::Subscription::SharedPtr sub_image_; rclcpp::Subscription::SharedPtr sub_compressed_image_; @@ -74,29 +74,33 @@ class UndistortNode : public rclcpp::Node std::optional info_{std::nullopt}; std::optional scaled_info_{std::nullopt}; - cv::Mat undistort_map_x, undistort_map_y; + cv::Mat undistort_map_x_, undistort_map_y_; void make_remap_lut() { if (!info_.has_value()) return; - cv::Mat K = cv::Mat(cv::Size(3, 3), CV_64FC1, reinterpret_cast(info_->k.data())); - cv::Mat D = cv::Mat(cv::Size(5, 1), CV_64FC1, reinterpret_cast(info_->d.data())); - cv::Size size(info_->width, info_->height); + cv::Mat k = cv::Mat(cv::Size(3, 3), CV_64FC1, info_->k.data()); + cv::Mat d = cv::Mat(cv::Size(5, 1), CV_64FC1, info_->d.data()); + cv::Size size(static_cast(info_->width), static_cast(info_->height)); cv::Size new_size = size; - if (OUTPUT_WIDTH > 0) - new_size = cv::Size(OUTPUT_WIDTH, 1.0f * OUTPUT_WIDTH / size.width * size.height); + if (OUTPUT_WIDTH_ > 0) + // set new_size along with aspect ratio + new_size = cv::Size( + OUTPUT_WIDTH_, static_cast( + static_cast(OUTPUT_WIDTH_) * + (static_cast(size.height) / static_cast(size.width)))); - cv::Mat new_K = cv::getOptimalNewCameraMatrix(K, D, size, 0, new_size); + cv::Mat new_k = cv::getOptimalNewCameraMatrix(k, d, size, 0, new_size); cv::initUndistortRectifyMap( - K, D, cv::Mat(), new_K, new_size, CV_32FC1, undistort_map_x, undistort_map_y); + k, d, cv::Mat(), new_k, new_size, CV_32FC1, undistort_map_x_, undistort_map_y_); scaled_info_ = sensor_msgs::msg::CameraInfo{}; - scaled_info_->k.at(0) = new_K.at(0, 0); - scaled_info_->k.at(2) = new_K.at(0, 2); - scaled_info_->k.at(4) = new_K.at(1, 1); - scaled_info_->k.at(5) = new_K.at(1, 2); + scaled_info_->k.at(0) = new_k.at(0, 0); + scaled_info_->k.at(2) = new_k.at(0, 2); + scaled_info_->k.at(4) = new_k.at(1, 1); + scaled_info_->k.at(5) = new_k.at(1, 2); scaled_info_->k.at(8) = 1; scaled_info_->d.resize(5); scaled_info_->width = new_size.width; @@ -106,12 +110,12 @@ class UndistortNode : public rclcpp::Node void remap_and_publish(const cv::Mat & image, const std_msgs::msg::Header & header) { cv::Mat undistorted_image; - cv::remap(image, undistorted_image, undistort_map_x, undistort_map_y, cv::INTER_LINEAR); + cv::remap(image, undistorted_image, undistort_map_x_, undistort_map_y_, cv::INTER_LINEAR); // Publish CameraInfo { scaled_info_->header = info_->header; - if (OVERRIDE_FRAME_ID != "") scaled_info_->header.frame_id = OVERRIDE_FRAME_ID; + if (!OVERRIDE_FRAME_ID_.empty()) scaled_info_->header.frame_id = OVERRIDE_FRAME_ID_; pub_info_->publish(scaled_info_.value()); } @@ -119,8 +123,8 @@ class UndistortNode : public rclcpp::Node { cv_bridge::CvImage bridge; bridge.header.stamp = header.stamp; - if (OVERRIDE_FRAME_ID != "") - bridge.header.frame_id = OVERRIDE_FRAME_ID; + if (!OVERRIDE_FRAME_ID_.empty()) + bridge.header.frame_id = OVERRIDE_FRAME_ID_; else bridge.header.frame_id = header.frame_id; bridge.encoding = "bgr8"; @@ -134,7 +138,7 @@ class UndistortNode : public rclcpp::Node if (!info_.has_value()) { return; } - if (undistort_map_x.empty()) { + if (undistort_map_x_.empty()) { make_remap_lut(); } @@ -143,7 +147,7 @@ class UndistortNode : public rclcpp::Node sub_compressed_image_.reset(); } - tier4_autoware_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } @@ -153,11 +157,11 @@ class UndistortNode : public rclcpp::Node if (!info_.has_value()) { return; } - if (undistort_map_x.empty()) { + if (undistort_map_x_.empty()) { make_remap_lut(); } - tier4_autoware_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } diff --git a/localization/yabloc/yabloc_monitor/CMakeLists.txt b/localization/yabloc/yabloc_monitor/CMakeLists.txt index aa1515661b2f6..7267cda53714f 100644 --- a/localization/yabloc/yabloc_monitor/CMakeLists.txt +++ b/localization/yabloc/yabloc_monitor/CMakeLists.txt @@ -4,12 +4,16 @@ project(yabloc_monitor) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(yabloc_monitor - src/yabloc_monitor_node.cpp +ament_auto_add_library(${PROJECT_NAME} src/yabloc_monitor_core.cpp src/availability_module.cpp ) -ament_target_dependencies(yabloc_monitor) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "YabLocMonitor" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/yabloc/yabloc_monitor/launch/yabloc_monitor.launch.xml b/localization/yabloc/yabloc_monitor/launch/yabloc_monitor.launch.xml index cf9f73977d35d..3073208e2e1a2 100644 --- a/localization/yabloc/yabloc_monitor/launch/yabloc_monitor.launch.xml +++ b/localization/yabloc/yabloc_monitor/launch/yabloc_monitor.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/localization/yabloc/yabloc_monitor/package.xml b/localization/yabloc/yabloc_monitor/package.xml index 22a5d0eded6b6..a42a734dbab31 100644 --- a/localization/yabloc/yabloc_monitor/package.xml +++ b/localization/yabloc/yabloc_monitor/package.xml @@ -20,6 +20,7 @@ diagnostic_updater geometry_msgs rclcpp + rclcpp_components ament_cmake_gtest ament_lint_auto diff --git a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.cpp b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.cpp index 876b86fd2bc9e..31e6ec9c51e7a 100644 --- a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.cpp +++ b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.cpp @@ -18,7 +18,8 @@ #include -YabLocMonitor::YabLocMonitor() : Node("yabloc_monitor"), updater_(this) +YabLocMonitor::YabLocMonitor(const rclcpp::NodeOptions & options) +: Node("yabloc_monitor", options), updater_(this) { updater_.setHardwareID(get_name()); updater_.add("yabloc_status", this, &YabLocMonitor::update_diagnostics); @@ -46,3 +47,6 @@ void YabLocMonitor::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapp stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "NG"); } } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(YabLocMonitor) diff --git a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.hpp b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.hpp index 8b8b937da205b..5dd46d63de6f2 100644 --- a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.hpp +++ b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.hpp @@ -25,7 +25,7 @@ class YabLocMonitor : public rclcpp::Node { public: - YabLocMonitor(); + explicit YabLocMonitor(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_node.cpp b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_node.cpp deleted file mode 100644 index 9b58325c852ea..0000000000000 --- a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2023 TIER IV -// -// 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 "yabloc_monitor_core.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared(); - - rclcpp::spin(node); - - return 0; -} diff --git a/localization/yabloc/yabloc_particle_filter/CMakeLists.txt b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt index 1284c52690795..ac122d0b86f98 100644 --- a/localization/yabloc/yabloc_particle_filter/CMakeLists.txt +++ b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt @@ -11,9 +11,6 @@ find_package(PCL REQUIRED) # Sophus find_package(Sophus REQUIRED) -# glog -find_package(glog REQUIRED) - # GeographicLib find_package(PkgConfig) find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h @@ -38,8 +35,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} ) # =================================================== -# Libraries -# correction library +# Correction library ament_auto_add_library( abstract_corrector SHARED @@ -60,6 +56,7 @@ ament_auto_add_library(predictor target_include_directories(predictor SYSTEM PRIVATE ${PCL_INCLUDE_DIRS}) target_link_libraries(predictor Sophus::Sophus ${PCL_LIBRARIES}) +# =================================================== # ros2idl typesupport if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) rosidl_target_interfaces(abstract_corrector ${PROJECT_NAME} "rosidl_typesupport_cpp") @@ -70,48 +67,45 @@ else() target_link_libraries(predictor "${cpp_typesupport_target}") endif() -# Cost map Library -ament_auto_add_library(ll2_cost_map - SHARED - src/ll2_cost_map/hierarchical_cost_map.cpp - src/ll2_cost_map/direct_cost_map.cpp -) -target_include_directories(ll2_cost_map PUBLIC include) -target_include_directories(ll2_cost_map SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(ll2_cost_map ${PCL_LIBRARIES}) - # =================================================== # Executables # Predictor -ament_auto_add_executable(predictor_node - src/prediction/predictor_node.cpp -) -target_link_libraries(predictor_node predictor) - -# Visualizer -ament_auto_add_executable(particle_visualize - src/common/particle_visualize_node.cpp +rclcpp_components_register_node(predictor + PLUGIN "yabloc::modularized_particle_filter::Predictor" + EXECUTABLE yabloc_predictor_node + EXECUTOR SingleThreadedExecutor ) # GNSS corrector -set(TARGET gnss_particle_corrector_node) -ament_auto_add_executable(${TARGET} - src/gnss_corrector/gnss_corrector_node.cpp - src/gnss_corrector/gnss_corrector_core.cpp) +set(TARGET gnss_particle_corrector) +ament_auto_add_library(${TARGET} + src/gnss_corrector/gnss_corrector_core.cpp +) target_include_directories(${TARGET} PUBLIC include) target_include_directories(${TARGET} PUBLIC SYSTEM ${GeographicLib_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(${TARGET} Sophus::Sophus) +target_link_libraries(${TARGET} abstract_corrector Sophus::Sophus) +rclcpp_components_register_node(${TARGET} + PLUGIN "yabloc::modularized_particle_filter::GnssParticleCorrector" + EXECUTABLE yabloc_gnss_particle_corrector_node + EXECUTOR SingleThreadedExecutor +) # Camera corrector -set(TARGET camera_particle_corrector_node) -ament_auto_add_executable(${TARGET} - src/camera_corrector/camera_particle_corrector_node.cpp +set(TARGET camera_particle_corrector) +ament_auto_add_library(${TARGET} + src/ll2_cost_map/hierarchical_cost_map.cpp + src/ll2_cost_map/direct_cost_map.cpp src/camera_corrector/filter_line_segments.cpp src/camera_corrector/logit.cpp src/camera_corrector/camera_particle_corrector_core.cpp) target_include_directories(${TARGET} PUBLIC include) target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} Sophus::Sophus ${PCL_LIBRARIES} glog::glog) +target_link_libraries(${TARGET} abstract_corrector Sophus::Sophus ${PCL_LIBRARIES}) +rclcpp_components_register_node(${TARGET} + PLUGIN "yabloc::modularized_particle_filter::CameraParticleCorrector" + EXECUTABLE yabloc_camera_particle_corrector_node + EXECUTOR SingleThreadedExecutor +) # =================================================== # TEST diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp index 59a163a8cc0a1..4ca8c01b3be64 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp @@ -33,7 +33,7 @@ namespace yabloc::modularized_particle_filter { -cv::Point2f cv2pt(const Eigen::Vector3f v); +cv::Point2f cv2pt(const Eigen::Vector3f & v); float abs_cos(const Eigen::Vector3f & t, float deg); class CameraParticleCorrector : public modularized_particle_filter::AbstractCorrector @@ -49,7 +49,7 @@ class CameraParticleCorrector : public modularized_particle_filter::AbstractCorr using Bool = std_msgs::msg::Bool; using String = std_msgs::msg::String; using SetBool = std_srvs::srv::SetBool; - CameraParticleCorrector(); + explicit CameraParticleCorrector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const float min_prob_; diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp index caf93abd0028f..0b2954dd46ba5 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp @@ -18,15 +18,6 @@ namespace yabloc { float logit_to_prob(float logit, float gain = 1.0f); - -/** - * Convert probability to logit - * This function is much faster than logit_to_prob() because it refers to pre-computed table - * - * @param[in] prob - * @return logit - */ -float prob_to_logit(float prob); } // namespace yabloc #endif // YABLOC_PARTICLE_FILTER__CAMERA_CORRECTOR__LOGIT_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp index e6af4f0d76d49..4e2c6cd8a842c 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp @@ -20,18 +20,13 @@ #include -namespace yabloc -{ -namespace modularized_particle_filter +namespace yabloc::modularized_particle_filter { geometry_msgs::msg::Pose get_mean_pose( const yabloc_particle_filter::msg::ParticleArray & particle_array); Eigen::Matrix3f std_of_distribution( const yabloc_particle_filter::msg::ParticleArray & particle_array); - -float std_of_weight(const yabloc_particle_filter::msg::ParticleArray & particle_array); -} // namespace modularized_particle_filter -} // namespace yabloc +} // namespace yabloc::modularized_particle_filter #endif // YABLOC_PARTICLE_FILTER__COMMON__MEAN_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp index 6059f16826c98..140f1553b88ee 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp @@ -22,14 +22,12 @@ #include #include -namespace yabloc +namespace yabloc::modularized_particle_filter::util { -namespace modularized_particle_filter::util -{ -std::random_device seed_gen; -std::default_random_engine engine(seed_gen()); +inline std::random_device seed_gen; +inline std::default_random_engine engine(seed_gen()); -Eigen::Vector2d nrand_2d(const Eigen::Matrix2d cov) +inline Eigen::Vector2d nrand_2d(const Eigen::Matrix2d & cov) { Eigen::JacobiSVD svd; svd.compute(cov, Eigen::ComputeFullU | Eigen::ComputeFullV); @@ -50,7 +48,7 @@ T nrand(T std) return dist(engine); } -double normalize_radian(const double rad, const double min_rad = -M_PI) +inline double normalize_radian(const double rad, const double min_rad = -M_PI) { const auto max_rad = min_rad + 2 * M_PI; @@ -63,6 +61,5 @@ double normalize_radian(const double rad, const double min_rad = -M_PI) return value - std::copysign(2 * M_PI, value); } -} // namespace modularized_particle_filter::util -} // namespace yabloc +} // namespace yabloc::modularized_particle_filter::util #endif // YABLOC_PARTICLE_FILTER__COMMON__PREDICTION_UTIL_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp index f1ecf35e81761..3b35d1a9a0da6 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp @@ -21,9 +21,7 @@ #include #include -namespace yabloc -{ -namespace modularized_particle_filter +namespace yabloc::modularized_particle_filter { class ParticleVisualizer { @@ -37,9 +35,7 @@ class ParticleVisualizer private: rclcpp::Publisher::SharedPtr pub_marker_array_; - std_msgs::msg::ColorRGBA compute_color(float value); }; -} // namespace modularized_particle_filter -} // namespace yabloc +} // namespace yabloc::modularized_particle_filter #endif // YABLOC_PARTICLE_FILTER__COMMON__VISUALIZE_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp index 31363742576fd..33802bd666440 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp @@ -26,9 +26,7 @@ #include #include -namespace yabloc -{ -namespace modularized_particle_filter +namespace yabloc::modularized_particle_filter { class AbstractCorrector : public rclcpp::Node { @@ -36,7 +34,7 @@ class AbstractCorrector : public rclcpp::Node using Particle = yabloc_particle_filter::msg::Particle; using ParticleArray = yabloc_particle_filter::msg::ParticleArray; - explicit AbstractCorrector(const std::string & node_name); + explicit AbstractCorrector(const std::string & node_name, const rclcpp::NodeOptions & options); protected: const float acceptable_max_delay_; // [sec] @@ -55,7 +53,6 @@ class AbstractCorrector : public rclcpp::Node private: void on_particle_array(const ParticleArray & particle_array); }; -} // namespace modularized_particle_filter -} // namespace yabloc +} // namespace yabloc::modularized_particle_filter #endif // YABLOC_PARTICLE_FILTER__CORRECTION__ABSTRACT_CORRECTOR_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp deleted file mode 100644 index 2c2f10bf7ad2e..0000000000000 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp +++ /dev/null @@ -1,35 +0,0 @@ -// 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 YABLOC_PARTICLE_FILTER__CORRECTION__CORRECTION_UTIL_HPP_ -#define YABLOC_PARTICLE_FILTER__CORRECTION__CORRECTION_UTIL_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "yabloc_particle_filter/msg/particle_array.hpp" - -#include - -#include - -namespace yabloc -{ -namespace modularized_particle_filter -{ -std::optional find_synced_particles( - boost::circular_buffer circular_buffer, - rclcpp::Time time); -} -} // namespace yabloc - -#endif // YABLOC_PARTICLE_FILTER__CORRECTION__CORRECTION_UTIL_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp index b5a8d40594ee8..05423a46ffbd7 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp @@ -38,7 +38,7 @@ class GnssParticleCorrector : public AbstractCorrector using MarkerArray = visualization_msgs::msg::MarkerArray; using Float32 = std_msgs::msg::Float32; - GnssParticleCorrector(); + explicit GnssParticleCorrector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const float mahalanobis_distance_threshold_; diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp index 791f65eb9b835..f22662a188355 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp @@ -35,41 +35,48 @@ struct WeightManager } }; - Parameter for_fixed_; - Parameter for_not_fixed_; + Parameter for_fixed_{}; + Parameter for_not_fixed_{}; explicit WeightManager(rclcpp::Node * node) { - for_fixed_.flat_radius_ = node->declare_parameter("for_fixed/flat_radius"); - for_fixed_.max_radius_ = node->declare_parameter("for_fixed/max_radius"); - for_fixed_.min_weight_ = node->declare_parameter("for_fixed/min_weight"); - for_fixed_.max_weight_ = node->declare_parameter("for_fixed/max_weight"); + for_fixed_.flat_radius_ = + static_cast(node->declare_parameter("for_fixed/flat_radius")); + for_fixed_.max_radius_ = + static_cast(node->declare_parameter("for_fixed/max_radius")); + for_fixed_.min_weight_ = + static_cast(node->declare_parameter("for_fixed/min_weight")); + for_fixed_.max_weight_ = + static_cast(node->declare_parameter("for_fixed/max_weight")); for_fixed_.compute_coeff(); - for_not_fixed_.flat_radius_ = node->declare_parameter("for_not_fixed/flat_radius"); - for_not_fixed_.max_radius_ = node->declare_parameter("for_not_fixed/max_radius"); - for_not_fixed_.min_weight_ = node->declare_parameter("for_not_fixed/min_weight"); - for_not_fixed_.max_weight_ = node->declare_parameter("for_not_fixed/max_weight"); + for_not_fixed_.flat_radius_ = + static_cast(node->declare_parameter("for_not_fixed/flat_radius")); + for_not_fixed_.max_radius_ = + static_cast(node->declare_parameter("for_not_fixed/max_radius")); + for_not_fixed_.min_weight_ = + static_cast(node->declare_parameter("for_not_fixed/min_weight")); + for_not_fixed_.max_weight_ = + static_cast(node->declare_parameter("for_not_fixed/max_weight")); for_not_fixed_.compute_coeff(); } - float normal_pdf(float distance, const Parameter & param) const + [[nodiscard]] static float normal_pdf(float distance, const Parameter & param) { // NOTE: This is not exact normal distribution because of no scale factor depending on sigma float d = std::clamp(std::abs(distance) - param.flat_radius_, 0.f, param.max_radius_); return param.max_weight_ * std::exp(-param.coeff_ * d * d); } - float normal_pdf(float distance, bool is_rtk_fixed) const + [[nodiscard]] float normal_pdf(float distance, bool is_rtk_fixed) const { if (is_rtk_fixed) { return normal_pdf(distance, for_fixed_); - } else { - return normal_pdf(distance, for_not_fixed_); } + return normal_pdf(distance, for_not_fixed_); } - float inverse_normal_pdf(float prob, const Parameter & param) const + [[nodiscard]] static float inverse_normal_pdf(float prob, const Parameter & param) { prob = (param.max_weight_ - param.min_weight_) * prob + param.min_weight_; @@ -78,13 +85,12 @@ struct WeightManager return param.flat_radius_ + std::sqrt(-std::log(prob / param.max_weight_) / param.coeff_); } - float inverse_normal_pdf(float prob, bool is_rtk_fixed) const + [[nodiscard]] float inverse_normal_pdf(float prob, bool is_rtk_fixed) const { if (is_rtk_fixed) { return inverse_normal_pdf(prob, for_fixed_); - } else { - return inverse_normal_pdf(prob, for_not_fixed_); } + return inverse_normal_pdf(prob, for_not_fixed_); } }; } // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp index 5f56b8f079cd1..8169719a21f3b 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp @@ -20,9 +20,6 @@ namespace yabloc { cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity); - -cv::Mat visualize_direction_map(const cv::Mat & cost_map); - } // namespace yabloc #endif // YABLOC_PARTICLE_FILTER__LL2_COST_MAP__DIRECT_COST_MAP_HPP_ diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp index 32d579c4ab595..d6cc4553e29bc 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp @@ -37,34 +37,37 @@ namespace yabloc { struct Area { - Area() {} + Area() = default; explicit Area(const Eigen::Vector2f & v) { - if (unit_length_ < 0) { + if (unit_length < 0) { throw_error(); } - x = static_cast(std::floor(v.x() / unit_length_)); - y = static_cast(std::floor(v.y() / unit_length_)); + x = static_cast(std::floor(v.x() / unit_length)); + y = static_cast(std::floor(v.y() / unit_length)); } - Eigen::Vector2f real_scale() const { return {x * unit_length_, y * unit_length_}; } + [[nodiscard]] Eigen::Vector2f real_scale() const + { + return {static_cast(x) * unit_length, static_cast(y) * unit_length}; + } - std::array real_scale_boundary() const + [[nodiscard]] std::array real_scale_boundary() const { std::array boundary; boundary.at(0) = real_scale(); - boundary.at(1) = real_scale() + Eigen::Vector2f(unit_length_, unit_length_); + boundary.at(1) = real_scale() + Eigen::Vector2f(unit_length, unit_length); return boundary; }; - void throw_error() const + static void throw_error() { std::cerr << "Area::unit_length_ is not initialized" << std::endl; throw std::runtime_error("invalid Area::unit_length"); } - int x, y; - static float unit_length_; - static float image_size_; + int x{}, y{}; + static float unit_length; + static float image_size; friend bool operator==(const Area & one, const Area & other) { @@ -129,7 +132,7 @@ class HierarchicalCostMap rclcpp::Logger logger_; std::optional height_{std::nullopt}; - common::GammaConverter gamma_converter{4.0f}; + common::GammaConverter gamma_converter_{4.0f}; std::unordered_map map_accessed_; @@ -138,7 +141,7 @@ class HierarchicalCostMap std::vector bounding_boxes_; std::unordered_map cost_maps_; - cv::Point to_cv_point(const Area & are, const Eigen::Vector2f) const; + cv::Point to_cv_point(const Area & area, const Eigen::Vector2f & p) const; void build_map(const Area & area); cv::Mat create_available_area_image(const Area & area) const; diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp index e02393ee14db6..b619ed524affd 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp @@ -49,7 +49,7 @@ class Predictor : public rclcpp::Node using Marker = visualization_msgs::msg::Marker; using SetBool = std_srvs::srv::SetBool; - Predictor(); + explicit Predictor(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: // The number of particles of particle filter @@ -106,7 +106,7 @@ class Predictor : public rclcpp::Node void initialize_particles(const PoseCovStamped & initialpose); // void update_with_dynamic_noise( - ParticleArray & particle_array, const TwistCovStamped & twist, double dt); + ParticleArray & particle_array, const TwistCovStamped & twist, double dt) const; // void publish_mean_pose(const geometry_msgs::msg::Pose & mean_pose, const rclcpp::Time & stamp); void publish_range_marker(const Eigen::Vector3f & pos, const Eigen::Vector3f & tangent); diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp index f2a517216020a..7b3aa8553f90e 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp @@ -22,10 +22,10 @@ namespace yabloc::modularized_particle_filter { -class resampling_skip_exception : public std::runtime_error +class ResamplingSkipException : public std::runtime_error { public: - explicit resampling_skip_exception(const char * message) : runtime_error(message) {} + explicit ResamplingSkipException(const char * message) : runtime_error(message) {} }; class RetroactiveResampler @@ -56,9 +56,10 @@ class RetroactiveResampler int latest_resampling_generation_; // Random generator from 0 to 1 - double random_from_01_uniformly() const; + [[nodiscard]] static double random_from_01_uniformly(); // Check the sanity of the particles obtained from the particle corrector. - bool check_weighted_particles_validity(const ParticleArray & weighted_particles) const; + [[nodiscard]] bool check_weighted_particles_validity( + const ParticleArray & weighted_particles) const; }; } // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp index 5b24ee96d6df5..6c1e9737c3f0d 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp @@ -35,7 +35,7 @@ class ResamplingHistory } } - bool check_history_validity() const + [[nodiscard]] bool check_history_validity() const { for (auto & generation : resampling_history_) { bool result = std::any_of(generation.begin(), generation.end(), [this](int x) { 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 5dd483f142174..b544a6c2516ca 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 @@ -4,7 +4,7 @@ - + @@ -25,7 +25,7 @@ - + @@ -46,7 +46,7 @@ - + diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index 66c0d8af8865c..8e2b98d10931b 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -17,9 +17,10 @@ autoware_cmake rosidl_default_generators + autoware_universe_utils geometry_msgs - glog rclcpp + rclcpp_components sensor_msgs sophus std_msgs @@ -27,7 +28,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils visualization_msgs yabloc_common diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp index e28d70fddc246..d6a38e7b97b96 100644 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp @@ -15,9 +15,9 @@ #include "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp" #include "yabloc_particle_filter/camera_corrector/logit.hpp" +#include +#include #include -#include -#include #include #include #include @@ -25,13 +25,15 @@ #include +#include + namespace yabloc::modularized_particle_filter { -CameraParticleCorrector::CameraParticleCorrector() -: AbstractCorrector("camera_particle_corrector"), - min_prob_(declare_parameter("min_prob")), - far_weight_gain_(declare_parameter("far_weight_gain")), +CameraParticleCorrector::CameraParticleCorrector(const rclcpp::NodeOptions & options) +: AbstractCorrector("camera_particle_corrector", options), + min_prob_(static_cast(declare_parameter("min_prob"))), + far_weight_gain_(static_cast(declare_parameter("far_weight_gain"))), cost_map_(this) { using std::placeholders::_1; @@ -98,7 +100,8 @@ CameraParticleCorrector::split_line_segments(const PointCloud2 & msg) { LineSegments all_line_segments_cloud; pcl::fromROSMsg(msg, all_line_segments_cloud); - LineSegments reliable_cloud, iffy_cloud; + LineSegments reliable_cloud; + LineSegments iffy_cloud; { for (const auto & p : all_line_segments_cloud) { if (p.label == 0) @@ -111,7 +114,7 @@ CameraParticleCorrector::split_line_segments(const PointCloud2 & msg) auto [good_cloud, bad_cloud] = filt(iffy_cloud); { cv::Mat debug_image = cv::Mat::zeros(800, 800, CV_8UC3); - auto draw = [&debug_image](LineSegments & cloud, cv::Scalar color) -> void { + auto draw = [&debug_image](const LineSegments & cloud, const cv::Scalar & color) -> void { for (const auto & line : cloud) { const Eigen::Vector3f p1 = line.getVector3fMap(); const Eigen::Vector3f p2 = line.getNormalVector3fMap(); @@ -130,7 +133,7 @@ CameraParticleCorrector::split_line_segments(const PointCloud2 & msg) void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments_msg) { - tier4_autoware_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; const rclcpp::Time stamp = line_segments_msg.header.stamp; std::optional opt_array = this->get_synchronized_particle_array(stamp); if (!opt_array.has_value()) { @@ -162,7 +165,7 @@ void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments } } - cost_map_.set_height(mean_pose.position.z); + cost_map_.set_height(static_cast(mean_pose.position.z)); if (publish_weighted_particles) { for (auto & particle : weighted_particles.particles) { @@ -187,8 +190,7 @@ void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments // DEBUG: just visualization { - Pose mean_pose = get_mean_pose(weighted_particles); - Sophus::SE3f transform = common::pose_to_se3(mean_pose); + Sophus::SE3f transform = common::pose_to_se3(get_mean_pose(weighted_particles)); pcl::PointCloud cloud = evaluate_cloud( common::transform_line_segments(line_segments_cloud, transform), transform.translation()); @@ -199,20 +201,19 @@ void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments pcl::PointCloud rgb_cloud; pcl::PointCloud rgb_iffy_cloud; - float max_score = 0; - for (const auto p : cloud) { - max_score = std::max(max_score, std::abs(p.intensity)); - } + float max_score = std::accumulate( + cloud.begin(), cloud.end(), 0.0f, + [](float max_score, const auto & p) { return std::max(max_score, std::abs(p.intensity)); }); for (const auto p : cloud) { pcl::PointXYZRGB rgb; rgb.getVector3fMap() = p.getVector3fMap(); - rgb.rgba = common::color_scale::blue_red(p.intensity / max_score); + rgb.rgba = static_cast(common::color_scale::blue_red(p.intensity / max_score)); rgb_cloud.push_back(rgb); } for (const auto p : iffy_cloud) { pcl::PointXYZRGB rgb; rgb.getVector3fMap() = p.getVector3fMap(); - rgb.rgba = common::color_scale::blue_red(p.intensity / max_score); + rgb.rgba = static_cast(common::color_scale::blue_red(p.intensity / max_score)); rgb_iffy_cloud.push_back(rgb); } @@ -256,9 +257,9 @@ void CameraParticleCorrector::on_ll2(const PointCloud2 & ll2_msg) float abs_cos(const Eigen::Vector3f & t, float deg) { - const float radian = deg * M_PI / 180.0; + const auto radian = static_cast(deg * M_PI / 180.0); Eigen::Vector2f x(t.x(), t.y()); - Eigen::Vector2f y(tier4_autoware_utils::cos(radian), tier4_autoware_utils::sin(radian)); + Eigen::Vector2f y(autoware::universe_utils::cos(radian), autoware::universe_utils::sin(radian)); x.normalize(); return std::abs(x.dot(y)); } @@ -276,7 +277,7 @@ float CameraParticleCorrector::compute_logit( // NOTE: Close points are prioritized float squared_norm = (p - self_position).topRows(2).squaredNorm(); - float gain = exp(-far_weight_gain_ * squared_norm); // 0 < gain < 1 + float gain = std::exp(-far_weight_gain_ * squared_norm); // 0 < gain < 1 const CostMapValue v3 = cost_map_.at(p.topRows(2)); @@ -285,9 +286,10 @@ float CameraParticleCorrector::compute_logit( continue; } if (pn.label == 0) { // posteriori - logit += 0.2f * gain * (abs_cos(tangent, v3.angle) * v3.intensity - 0.5f); + logit += + 0.2f * gain * (abs_cos(tangent, static_cast(v3.angle)) * v3.intensity - 0.5f); } else { // apriori - logit += gain * (abs_cos(tangent, v3.angle) * v3.intensity - 0.5f); + logit += gain * (abs_cos(tangent, static_cast(v3.angle)) * v3.intensity - 0.5f); } } } @@ -307,11 +309,14 @@ pcl::PointCloud CameraParticleCorrector::evaluate_cloud( // NOTE: Close points are prioritized float squared_norm = (p - self_position).topRows(2).squaredNorm(); - float gain = std::exp(-far_weight_gain_ * squared_norm); CostMapValue v3 = cost_map_.at(p.topRows(2)); float logit = 0; - if (!v3.unmapped) logit = gain * (abs_cos(tangent, v3.angle) * v3.intensity - 0.5f); + if (!v3.unmapped) { + float gain = std::exp(-far_weight_gain_ * squared_norm); + + logit = gain * (abs_cos(tangent, static_cast(v3.angle)) * v3.intensity - 0.5f); + } pcl::PointXYZI xyzi(logit_to_prob(logit, 10.f)); xyzi.getVector3fMap() = p; @@ -321,3 +326,6 @@ pcl::PointCloud CameraParticleCorrector::evaluate_cloud( return cloud; } } // namespace yabloc::modularized_particle_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::modularized_particle_filter::CameraParticleCorrector) diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_node.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_node.cpp deleted file mode 100644 index 01378f05a0ac8..0000000000000 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_node.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// 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 "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp" - -#include - -int main(int argc, char * argv[]) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - } - - namespace mpf = yabloc::modularized_particle_filter; - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp index 3f782d85aec43..105f72920a6de 100644 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filter_line_segments.cpp @@ -22,33 +22,35 @@ namespace yabloc::modularized_particle_filter { -cv::Point2f cv2pt(const Eigen::Vector3f v) +cv::Point2f cv2pt(const Eigen::Vector3f & v) { - const float METRIC_PER_PIXEL = 0.05; - const float IMAGE_RADIUS = 400; - return {-v.y() / METRIC_PER_PIXEL + IMAGE_RADIUS, -v.x() / METRIC_PER_PIXEL + 2 * IMAGE_RADIUS}; + const float metric_per_pixel = 0.05; + const float image_radius = 400; + return {-v.y() / metric_per_pixel + image_radius, -v.x() / metric_per_pixel + 2 * image_radius}; } float normalized_atan2(const Eigen::Vector3f & t, float deg) { - float diff = std::atan2(t.y(), t.x()) - deg * M_PI / 180; - diff = std::fmod(diff, M_PI); + auto diff = static_cast(std::atan2(t.y(), t.x()) - deg * M_PI / 180); + diff = static_cast(std::fmod(diff, M_PI)); if (diff < 0) diff = -diff; if (diff < M_PI_2) { - return 1 - diff / M_PI_2; - } else if (diff < M_PI) { - return diff / M_PI_2 - 1; - } else { - throw std::runtime_error("invalid cos"); + return static_cast(1.0 - diff / M_PI_2); } + + if (diff < M_PI) { + return static_cast(diff / M_PI_2 - 1.0); + } + throw std::runtime_error("invalid cos"); } std::pair CameraParticleCorrector::filt(const LineSegments & iffy_lines) { - LineSegments good, bad; + LineSegments good; + LineSegments bad; if (!latest_pose_.has_value()) { throw std::runtime_error("latest_pose_ is nullopt"); } @@ -67,7 +69,7 @@ CameraParticleCorrector::filt(const LineSegments & iffy_lines) for (float distance = 0; distance < length; distance += 0.1f) { Eigen::Vector3f px = pose * (p2 + tangent * distance); CostMapValue v3 = cost_map_.at(px.topRows(2)); - float cos2 = normalized_atan2(pose.so3() * tangent, v3.angle); + float cos2 = normalized_atan2(pose.so3() * tangent, static_cast(v3.angle)); score += (cos2 * v3.intensity); count++; @@ -77,7 +79,7 @@ CameraParticleCorrector::filt(const LineSegments & iffy_lines) // rgb_cloud.push_back(rgb); } - if (score / count > 0.5f) { + if (score / static_cast(count) > 0.5f) { good.push_back(line); } else { bad.push_back(line); diff --git a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp index d8bb5690b6fc4..65b7039e25051 100644 --- a/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp @@ -27,7 +27,7 @@ struct ProbToLogitTable ProbToLogitTable() { for (int i = 0; i < 100; ++i) { - float p = i / 100.0f; + float p = static_cast(i) / 100.0f; table_.at(i) = std::log(p / std::max(1 - p, 1e-6f)); } } @@ -37,7 +37,7 @@ struct ProbToLogitTable return table_.at(index); } - std::array table_; + std::array table_{}; } prob_to_logit_table; } // namespace @@ -47,9 +47,4 @@ float logit_to_prob(float logit, float gain) return 1.f / (1 + std::exp(-gain * logit)); } -float prob_to_logit(float prob) -{ - return prob_to_logit_table(prob); -} - } // namespace yabloc diff --git a/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp index 0710c8c4dca64..1a0ef05508c76 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp @@ -70,7 +70,9 @@ geometry_msgs::msg::Pose get_mean_pose( mean_pose.position.y += particle.pose.position.y * normalized_weight; mean_pose.position.z += particle.pose.position.z * normalized_weight; - double yaw{0.0}, pitch{0.0}, roll{0.0}; + double yaw{0.0}; + double pitch{0.0}; + double roll{0.0}; tf2::getEulerYPR(particle.pose.orientation, yaw, pitch, roll); rolls.push_back(roll); @@ -93,43 +95,25 @@ Eigen::Matrix3f std_of_distribution(const yabloc_particle_filter::msg::ParticleA { using Particle = yabloc_particle_filter::msg::Particle; auto ori = get_mean_pose(array).orientation; - Eigen::Quaternionf orientation(ori.w, ori.x, ori.y, ori.z); - float invN = 1.f / array.particles.size(); + Eigen::Quaternionf orientation( + static_cast(ori.w), static_cast(ori.x), static_cast(ori.y), + static_cast(ori.z)); + float inv_n = 1.f / static_cast(array.particles.size()); Eigen::Vector3f mean = Eigen::Vector3f::Zero(); for (const Particle & p : array.particles) { Eigen::Affine3f affine = common::pose_to_affine(p.pose); mean += affine.translation(); } - mean *= invN; + mean *= inv_n; Eigen::Matrix3f sigma = Eigen::Matrix3f::Zero(); for (const Particle & p : array.particles) { Eigen::Affine3f affine = common::pose_to_affine(p.pose); Eigen::Vector3f d = affine.translation() - mean; d = orientation.conjugate() * d; - sigma += (d * d.transpose()) * invN; + sigma += (d * d.transpose()) * inv_n; } return sigma; } - -float std_of_weight(const yabloc_particle_filter::msg::ParticleArray & particle_array) -{ - using Particle = yabloc_particle_filter::msg::Particle; - - const float invN = 1.f / particle_array.particles.size(); - float mean = 0; - for (const Particle & p : particle_array.particles) { - mean += p.weight; - } - mean *= invN; - - float sigma = 0.0; - for (const Particle & p : particle_array.particles) { - sigma += (p.weight - mean) * (p.weight - mean); - } - sigma *= invN; - - return std::sqrt(sigma); -} } // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp index e6e59a836f6db..48f2041595a77 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp @@ -30,7 +30,8 @@ class ParticleVisualize : public rclcpp::Node using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; - ParticleVisualize() : Node("particle_visualize") + explicit ParticleVisualize(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("particle_visualize", options) { using std::placeholders::_1; // Subscriber @@ -38,36 +39,12 @@ class ParticleVisualize : public rclcpp::Node "/particle_array", 10, std::bind(&ParticleVisualize::on_particles, this, _1)); // Publisher - pub_marker_array = this->create_publisher("/marker_array", 10); + pub_marker_array_ = this->create_publisher("/marker_array", 10); } private: rclcpp::Subscription::SharedPtr sub_particles_; - rclcpp::Publisher::SharedPtr pub_marker_array; - - std_msgs::msg::ColorRGBA compute_color(float value) const - { - float r = 1.0f, g = 1.0f, b = 1.0f; - // clang-format off - value = std::clamp(value, 0.0f, 1.0f); - if (value < 0.25f) { - r = 0; g = 4 * (value); - } else if (value < 0.5f) { - r = 0; b = 1 + 4 * (0.25f - value); - } else if (value < 0.75f) { - r = 4 * (value - 0.5f); b = 0; - } else { - g = 1 + 4 * (0.75f - value); b = 0; - } - // clang-format on - - std_msgs::msg::ColorRGBA rgba; - rgba.r = r; - rgba.g = g; - rgba.b = b; - rgba.a = 1.0f; - return rgba; - } + rclcpp::Publisher::SharedPtr pub_marker_array_; void on_particles(const ParticleArray & msg) { @@ -101,16 +78,10 @@ class ParticleVisualize : public rclcpp::Node marker.pose.position.z = p.pose.position.z; marker_array.markers.push_back(marker); } - pub_marker_array->publish(marker_array); + pub_marker_array_->publish(marker_array); } }; } // namespace yabloc::modularized_particle_filter -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::modularized_particle_filter::ParticleVisualize) diff --git a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp index 888baf0bea0ac..17b22757b4bc5 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp @@ -33,7 +33,7 @@ void ParticleVisualizer::publish(const ParticleArray & msg) float min = minmax_weight.first->weight; float max = minmax_weight.second->weight; max = std::max(max, min + 1e-7f); - auto boundWeight = [min, max](float raw) -> float { return (raw - min) / (max - min); }; + auto bound_weight = [min, max](float raw) -> float { return (raw - min) / (max - min); }; int id = 0; for (const Particle & p : msg.particles) { @@ -46,7 +46,7 @@ void ParticleVisualizer::publish(const ParticleArray & msg) marker.scale.y = 0.1; marker.scale.z = 0.1; marker.color = - static_cast(common::color_scale::rainbow(boundWeight(p.weight))); + static_cast(common::color_scale::rainbow(bound_weight(p.weight))); marker.pose.orientation = p.pose.orientation; marker.pose.position.x = p.pose.position.x; marker.pose.position.y = p.pose.position.y; diff --git a/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp b/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp index f8db1a561f333..1c8e2f5976a1d 100644 --- a/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp @@ -16,9 +16,10 @@ namespace yabloc::modularized_particle_filter { -AbstractCorrector::AbstractCorrector(const std::string & node_name) -: Node(node_name), - acceptable_max_delay_(declare_parameter("acceptable_max_delay")), +AbstractCorrector::AbstractCorrector( + const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options), + acceptable_max_delay_(static_cast(declare_parameter("acceptable_max_delay"))), visualize_(declare_parameter("visualize")), logger_(rclcpp::get_logger("abstract_corrector")) { diff --git a/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp b/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp deleted file mode 100644 index 67ca4f5add947..0000000000000 --- a/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// 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 "yabloc_particle_filter/correction/correction_util.hpp" - -namespace yabloc::modularized_particle_filter -{ -std::optional find_synced_particles( - boost::circular_buffer circular_buffer, - rclcpp::Time time) -{ - for (int i{1}; i < static_cast(circular_buffer.size()); i++) { - if (rclcpp::Time(circular_buffer[i].header.stamp) < time) { - return circular_buffer[i]; - } - } - if (0 < static_cast(circular_buffer.size())) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("modularized_particle_filter.correction_util"), - "the sensor data is too old: " - << rclcpp::Time(circular_buffer[static_cast(circular_buffer.size()) - 1].header.stamp) - .seconds() - - time.seconds()); - } - return std::nullopt; -} -} // namespace yabloc::modularized_particle_filter diff --git a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp index e8c8890561a7f..1ccb24bfbea5c 100644 --- a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp @@ -19,9 +19,10 @@ namespace yabloc::modularized_particle_filter { -GnssParticleCorrector::GnssParticleCorrector() -: AbstractCorrector("gnss_particle_corrector"), - mahalanobis_distance_threshold_(declare_parameter("mahalanobis_distance_threshold")), +GnssParticleCorrector::GnssParticleCorrector(const rclcpp::NodeOptions & options) +: AbstractCorrector("gnss_particle_corrector", options), + mahalanobis_distance_threshold_( + static_cast(declare_parameter("mahalanobis_distance_threshold"))), weight_manager_(this) { using std::placeholders::_1; @@ -61,7 +62,8 @@ void GnssParticleCorrector::on_pose(const PoseCovStamped::ConstSharedPtr pose_ms const rclcpp::Time stamp = pose_msg->header.stamp; const auto & position = pose_msg->pose.pose.position; Eigen::Vector3f gnss_position; - gnss_position << position.x, position.y, position.z; + gnss_position << static_cast(position.x), static_cast(position.y), + static_cast(position.z); constexpr bool is_rtk_fixed = false; process(gnss_position, stamp, is_rtk_fixed); @@ -96,7 +98,6 @@ void GnssParticleCorrector::process( // Compute travel distance from last update position // If the distance is too short, skip weighting { - Eigen::Vector3f mean_position = common::pose_to_affine(mean_pose).translation(); if ((mean_position - last_mean_position_).squaredNorm() > 1) { this->set_weighted_particle_array(weighted_particles); last_mean_position_ = mean_position; @@ -113,12 +114,12 @@ void GnssParticleCorrector::publish_marker(const Eigen::Vector3f & position, boo using namespace std::literals::chrono_literals; using Point = geometry_msgs::msg::Point; - auto drawCircle = [](std::vector & points, float radius) -> void { - const int N = 10; - for (int theta = 0; theta < 2 * N + 1; theta++) { + auto draw_circle = [](std::vector & points, float radius) -> void { + const int n = 10; + for (int theta = 0; theta < 2 * n + 1; theta++) { geometry_msgs::msg::Point pt; - pt.x = radius * std::cos(theta * M_PI / N); - pt.y = radius * std::sin(theta * M_PI / N); + pt.x = radius * std::cos(theta * M_PI / n); + pt.y = radius * std::sin(theta * M_PI / n); points.push_back(pt); } }; @@ -135,11 +136,11 @@ void GnssParticleCorrector::publish_marker(const Eigen::Vector3f & position, boo marker.pose.position.y = position.y(); marker.pose.position.z = latest_height_.data; - float prob = i / 4.0f; + float prob = static_cast(i) / 4.0f; marker.color = static_cast(common::color_scale::rainbow(prob)); marker.color.a = 0.5f; marker.scale.x = 0.1; - drawCircle(marker.points, weight_manager_.inverse_normal_pdf(prob, is_rtk_fixed)); + draw_circle(marker.points, weight_manager_.inverse_normal_pdf(prob, is_rtk_fixed)); array_msg.markers.push_back(marker); } marker_pub_->publish(array_msg); @@ -151,7 +152,7 @@ GnssParticleCorrector::ParticleArray GnssParticleCorrector::weight_particles( ParticleArray weighted_particles{predicted_particles}; for (auto & particle : weighted_particles.particles) { - float distance = static_cast( + auto distance = static_cast( std::hypot(particle.pose.position.x - pose.x(), particle.pose.position.y - pose.y())); particle.weight = weight_manager_.normal_pdf(distance, is_rtk_fixed); } @@ -160,3 +161,6 @@ GnssParticleCorrector::ParticleArray GnssParticleCorrector::weight_particles( } } // namespace yabloc::modularized_particle_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::modularized_particle_filter::GnssParticleCorrector) diff --git a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_node.cpp b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_node.cpp deleted file mode 100644 index 6d76fd295ac02..0000000000000 --- a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_node.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// 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 "yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp" - -int main(int argc, char * argv[]) -{ - namespace mpf = yabloc::modularized_particle_filter; - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp index 7da87e8050ebe..c6359a8c8f5fc 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp @@ -18,14 +18,14 @@ namespace yabloc { cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity) { - constexpr int MAX_INT = std::numeric_limits::max(); + constexpr int max_int = std::numeric_limits::max(); std::vector> distances; distances.resize(cost_map.rows); for (int i = 0; i < cost_map.rows; i++) { distances.at(i).resize(cost_map.cols); - std::fill(distances.at(i).begin(), distances.at(i).end(), MAX_INT); - const uchar * intensity_ptr = intensity.ptr(i); + std::fill(distances.at(i).begin(), distances.at(i).end(), max_int); + const auto * intensity_ptr = intensity.ptr(i); for (int j = 0; j < cost_map.cols; j++) { if (intensity_ptr[j] == 0) distances.at(i).at(j) = 0; } @@ -36,7 +36,7 @@ cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity) // Forward for (int r = 1; r < cost_map.rows; r++) { const uchar * upper_ptr = dst.ptr(r - 1); - uchar * current_ptr = dst.ptr(r); + auto * current_ptr = dst.ptr(r); for (int c = 1; c < cost_map.cols; c++) { int up = distances.at(r - 1).at(c); @@ -56,7 +56,7 @@ cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity) // Backward for (int r = cost_map.rows - 2; r >= 0; r--) { const uchar * downer_ptr = dst.ptr(r + 1); - uchar * current_ptr = dst.ptr(r); + auto * current_ptr = dst.ptr(r); for (int c = cost_map.cols - 2; c >= 0; c--) { int down = distances.at(r + 1).at(c); @@ -76,24 +76,6 @@ cv::Mat direct_cost_map(const cv::Mat & cost_map, const cv::Mat & intensity) return dst; } -cv::Mat visualize_direction_map(const cv::Mat & cost_map) -{ - cv::Mat s = 255 * cv::Mat::ones(cost_map.size(), CV_8UC1); - cv::Mat v = 255 * cv::Mat::ones(cost_map.size(), CV_8UC1); - cv::Mat hsv, rgb; - cv::merge(std::vector{cost_map, s, v}, hsv); - cv::cvtColor(hsv, rgb, cv::COLOR_HSV2BGR); - - for (int r = 0; r < cost_map.rows; r++) { - const uchar * src_ptr = cost_map.ptr(r); - cv::Vec3b * dst_ptr = rgb.ptr(r); - for (int c = 0; c < cost_map.cols; c++) { - if (src_ptr[c] == 0) dst_ptr[c] = cv::Vec3b(0, 0, 0); - } - } - return rgb; -} - } // namespace yabloc // #include diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp index 62d8ed826b373..4f03e3d26ecb6 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp @@ -16,28 +16,28 @@ #include "yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp" +#include #include #include -#include #include namespace yabloc { -float Area::unit_length_ = -1; +float Area::unit_length = -1; HierarchicalCostMap::HierarchicalCostMap(rclcpp::Node * node) -: max_range_(node->declare_parameter("max_range")), - image_size_(node->declare_parameter("image_size")), +: max_range_(static_cast(node->declare_parameter("max_range"))), + image_size_(static_cast(node->declare_parameter("image_size"))), max_map_count_(10), logger_(node->get_logger()) { - Area::unit_length_ = max_range_; - float gamma = node->declare_parameter("gamma"); - gamma_converter.reset(gamma); + Area::unit_length = max_range_; + float gamma = static_cast(node->declare_parameter("gamma")); + gamma_converter_.reset(gamma); } -cv::Point2i HierarchicalCostMap::to_cv_point(const Area & area, const Eigen::Vector2f p) const +cv::Point2i HierarchicalCostMap::to_cv_point(const Area & area, const Eigen::Vector2f & p) const { Eigen::Vector2f relative = p - area.real_scale(); float px = relative.x() / max_range_ * image_size_; @@ -59,7 +59,7 @@ CostMapValue HierarchicalCostMap::at(const Eigen::Vector2f & position) cv::Point2i tmp = to_cv_point(key, position); cv::Vec3b b3 = cost_maps_.at(key).ptr(tmp.y)[tmp.x]; - return {b3[0] / 255.f, b3[1], b3[2] == 1}; + return {static_cast(b3[0]) / 255.f, b3[1], b3[2] == 1}; } void HierarchicalCostMap::set_height(float height) @@ -103,10 +103,13 @@ void HierarchicalCostMap::build_map(const Area & area) { if (!cloud_.has_value()) return; - cv::Mat image = 255 * cv::Mat::ones(cv::Size(image_size_, image_size_), CV_8UC1); - cv::Mat orientation = cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC1); + cv::Mat image = + 255 * + cv::Mat::ones(cv::Size(static_cast(image_size_), static_cast(image_size_)), CV_8UC1); + cv::Mat orientation = + cv::Mat::zeros(cv::Size(static_cast(image_size_), static_cast(image_size_)), CV_8UC1); - auto cvPoint = [this, area](const Eigen::Vector3f & p) -> cv::Point { + auto cv_point = [this, area](const Eigen::Vector3f & p) -> cv::Point { return this->to_cv_point(area, p.topRows(2)); }; @@ -117,12 +120,12 @@ void HierarchicalCostMap::build_map(const Area & area) if (std::abs(pn.normal_z - *height_) > 4) continue; } - cv::Point2i from = cvPoint(pn.getVector3fMap()); - cv::Point2i to = cvPoint(pn.getNormalVector3fMap()); + cv::Point2i from = cv_point(pn.getVector3fMap()); + cv::Point2i to = cv_point(pn.getNormalVector3fMap()); - float radian = std::atan2(from.y - to.y, from.x - to.x); + auto radian = static_cast(std::atan2(from.y - to.y, from.x - to.x)); if (radian < 0) radian += M_PI; - float degree = radian * 180 / M_PI; + auto degree = static_cast(radian * 180 / M_PI); cv::line(image, from, to, cv::Scalar::all(0), 1); cv::line(orientation, from, to, cv::Scalar::all(degree), 1); @@ -142,7 +145,7 @@ void HierarchicalCostMap::build_map(const Area & area) cv::Mat directed_cost_map; cv::merge( - std::vector{gamma_converter(distance), whole_orientation, available_area}, + std::vector{gamma_converter_(distance), whole_orientation, available_area}, directed_cost_map); cost_maps_[area] = directed_cost_map; @@ -169,13 +172,14 @@ HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const marker.header.frame_id = "map"; marker.id = id++; marker.type = Marker::LINE_STRIP; - marker.color = tier4_autoware_utils::createMarkerColor(0, 0, 1.0f, 1.0f); + marker.color = autoware::universe_utils::createMarkerColor(0, 0, 1.0f, 1.0f); marker.scale.x = 0.1; Eigen::Vector2f xy = area.real_scale(); marker.points.push_back(point_msg(xy.x(), xy.y())); - marker.points.push_back(point_msg(xy.x() + area.unit_length_, xy.y())); - marker.points.push_back(point_msg(xy.x() + area.unit_length_, xy.y() + area.unit_length_)); - marker.points.push_back(point_msg(xy.x(), xy.y() + area.unit_length_)); + marker.points.push_back(point_msg(xy.x() + yabloc::Area::unit_length, xy.y())); + marker.points.push_back( + point_msg(xy.x() + yabloc::Area::unit_length, xy.y() + yabloc::Area::unit_length)); + marker.points.push_back(point_msg(xy.x(), xy.y() + yabloc::Area::unit_length)); marker.points.push_back(point_msg(xy.x(), xy.y())); array_msg.markers.push_back(marker); } @@ -188,27 +192,33 @@ cv::Mat HierarchicalCostMap::get_map_image(const Pose & pose) // return cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC3); Eigen::Vector2f center; - center << pose.position.x, pose.position.y; + center << static_cast(pose.position.x), static_cast(pose.position.y); - float w = pose.orientation.w; - float z = pose.orientation.z; - Eigen::Matrix2f R = Eigen::Rotation2Df(2.f * std::atan2(z, w) - M_PI_2).toRotationMatrix(); + auto w = static_cast(pose.orientation.w); + auto z = static_cast(pose.orientation.z); + Eigen::Matrix2f r = + Eigen::Rotation2Df(static_cast(2.f * std::atan2(z, w) - M_PI_2)).toRotationMatrix(); - auto toVector2f = [this, center, R](float h, float w) -> Eigen::Vector2f { + auto to_vector2f = [this, center, r](float h, float w) -> Eigen::Vector2f { Eigen::Vector2f offset; offset.x() = (w / this->image_size_ - 0.5f) * this->max_range_ * 1.5f; offset.y() = -(h / this->image_size_ - 0.5f) * this->max_range_ * 1.5f; - return center + R * offset; + return center + r * offset; }; - cv::Mat image = cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC3); - for (int w = 0; w < image_size_; w++) { - for (int h = 0; h < image_size_; h++) { - CostMapValue v3 = this->at(toVector2f(h, w)); + cv::Mat image = + cv::Mat::zeros(cv::Size(static_cast(image_size_), static_cast(image_size_)), CV_8UC3); + for (int w_index = 0; static_cast(w_index) < image_size_; w_index++) { + for (int h_index = 0; static_cast(h_index) < image_size_; h_index++) { + CostMapValue v3 = + this->at(to_vector2f(static_cast(h_index), static_cast(w_index))); if (v3.unmapped) - image.at(h, w) = cv::Vec3b(v3.angle, 255 * v3.intensity, 50); + image.at(h_index, w_index) = + cv::Vec3b(v3.angle, static_cast(255 * v3.intensity), 50); else - image.at(h, w) = cv::Vec3b(v3.angle, 255 * v3.intensity, 255 * v3.intensity); + image.at(h_index, w_index) = cv::Vec3b( + v3.angle, static_cast(255 * v3.intensity), + static_cast(255 * v3.intensity)); } } @@ -235,7 +245,8 @@ void HierarchicalCostMap::erase_obsolete() cv::Mat HierarchicalCostMap::create_available_area_image(const Area & area) const { - cv::Mat available_area = cv::Mat::zeros(cv::Size(image_size_, image_size_), CV_8UC1); + cv::Mat available_area = + cv::Mat::zeros(cv::Size(static_cast(image_size_), static_cast(image_size_)), CV_8UC1); if (bounding_boxes_.empty()) return available_area; // Define current area diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp index 764ef88bbde18..701a2b8763fa3 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp @@ -31,12 +31,15 @@ namespace yabloc::modularized_particle_filter { -Predictor::Predictor() -: Node("predictor"), - number_of_particles_(declare_parameter("num_of_particles")), - resampling_interval_seconds_(declare_parameter("resampling_interval_seconds")), - static_linear_covariance_(declare_parameter("static_linear_covariance")), - static_angular_covariance_(declare_parameter("static_angular_covariance")), +Predictor::Predictor(const rclcpp::NodeOptions & options) +: Node("predictor", options), + number_of_particles_(static_cast(declare_parameter("num_of_particles"))), + resampling_interval_seconds_( + static_cast(declare_parameter("resampling_interval_seconds"))), + static_linear_covariance_( + static_cast(declare_parameter("static_linear_covariance"))), + static_angular_covariance_( + static_cast(declare_parameter("static_angular_covariance"))), cov_xx_yy_{this->template declare_parameter>("cov_xx_yy")} { tf2_broadcaster_ = std::make_unique(*this); @@ -93,11 +96,11 @@ void Predictor::on_trigger_service( RCLCPP_INFO_STREAM(get_logger(), "yabloc particle filter is deactivated"); } - const bool before_activated_ = yabloc_activated_; + const bool before_activated = yabloc_activated_; yabloc_activated_ = request->data; response->success = true; - if (yabloc_activated_ && (!before_activated_)) { + if (yabloc_activated_ && (!before_activated)) { RCLCPP_INFO_STREAM(get_logger(), "restart particle filter"); if (latest_ekf_pose_ptr_) { on_initial_pose(latest_ekf_pose_ptr_); @@ -113,10 +116,11 @@ void Predictor::on_initial_pose(const PoseCovStamped::ConstSharedPtr initialpose // Publish initial pose marker auto position = initialpose->pose.pose.position; Eigen::Vector3f pos_vec3f; - pos_vec3f << position.x, position.y, position.z; + pos_vec3f << static_cast(position.x), static_cast(position.y), + static_cast(position.z); auto orientation = initialpose->pose.pose.orientation; - float theta = 2 * std::atan2(orientation.z, orientation.w); + auto theta = static_cast(2 * std::atan2(orientation.z, orientation.w)); Eigen::Vector3f tangent; tangent << std::cos(theta), std::sin(theta), 0; @@ -152,7 +156,7 @@ void Predictor::initialize_particles(const PoseCovStamped & initialpose) pose.position.x += noise.x(); pose.position.y += noise.y(); - float noised_yaw = util::normalize_radian(yaw + util::nrand(yaw_std)); + auto noised_yaw = static_cast(util::normalize_radian(yaw + util::nrand(yaw_std))); pose.orientation.w = std::cos(noised_yaw / 2.0); pose.orientation.x = 0.0; pose.orientation.y = 0.0; @@ -184,14 +188,14 @@ void Predictor::on_twist_cov(const TwistCovStamped::ConstSharedPtr twist_cov) } void Predictor::update_with_dynamic_noise( - ParticleArray & particle_array, const TwistCovStamped & twist, double dt) + ParticleArray & particle_array, const TwistCovStamped & twist, double dt) const { // linear & angular velocity - const float linear_x = twist.twist.twist.linear.x; - const float angular_z = twist.twist.twist.angular.z; + const auto linear_x = static_cast(twist.twist.twist.linear.x); + const auto angular_z = static_cast(twist.twist.twist.angular.z); // standard deviation of linear & angular velocity - const float std_linear_x = std::sqrt(twist.twist.covariance[6 * 0 + 0]); - const float std_angular_z = std::sqrt(twist.twist.covariance[6 * 5 + 5]); + const auto std_linear_x = static_cast(std::sqrt(twist.twist.covariance[6 * 0 + 0])); + const auto std_angular_z = static_cast(std::sqrt(twist.twist.covariance[6 * 5 + 5])); // 1[rad/s] = 60[deg/s] // 1[m/s] = 3.6[km/h] const float truncated_angular_std = @@ -272,7 +276,7 @@ void Predictor::on_weighted_particles(const ParticleArray::ConstSharedPtr weight try { particle_array = resampler_ptr_->add_weight_retroactively(particle_array, *weighted_particles_ptr); - } catch (const resampling_skip_exception & e) { + } catch (const ResamplingSkipException & e) { // Do nothing (just skipping the resample()) RCLCPP_INFO_STREAM(this->get_logger(), "skipped resampling"); } @@ -284,16 +288,16 @@ void Predictor::on_weighted_particles(const ParticleArray::ConstSharedPtr weight // Exit if previous resampling time is not valid. if (!previous_resampling_time_opt_.has_value()) { previous_resampling_time_opt_ = current_time; - throw resampling_skip_exception("previous resampling time is not valid"); + throw ResamplingSkipException("previous resampling time is not valid"); } if (current_time - previous_resampling_time_opt_.value() <= resampling_interval_seconds_) { - throw resampling_skip_exception("it is not time to resample"); + throw ResamplingSkipException("it is not time to resample"); } particle_array = resampler_ptr_->resample(particle_array); previous_resampling_time_opt_ = current_time; - } catch (const resampling_skip_exception & e) { + } catch (const ResamplingSkipException & e) { void(); // Do nothing (just skipping the resample()) } @@ -397,7 +401,7 @@ Predictor::PoseCovStamped Predictor::rectify_initial_pose( msg.pose.pose.orientation.z = std::sin(theta / 2); Eigen::Matrix2f cov; - cov << cov_xx_yy_.at(0), 0, 0, cov_xx_yy_.at(1); + cov << static_cast(cov_xx_yy_.at(0)), 0, 0, static_cast(cov_xx_yy_.at(1)); Eigen::Rotation2D r(theta); cov = r * cov * r.inverse(); @@ -411,3 +415,6 @@ Predictor::PoseCovStamped Predictor::rectify_initial_pose( } } // namespace yabloc::modularized_particle_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::modularized_particle_filter::Predictor) diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor_node.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor_node.cpp deleted file mode 100644 index 81c756908b0f8..0000000000000 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// 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 "yabloc_particle_filter/prediction/predictor.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - namespace mpf = yabloc::modularized_particle_filter; - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp index cc4cbc3730e0c..c8c3971ba85b2 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp @@ -65,7 +65,7 @@ RetroactiveResampler::ParticleArray RetroactiveResampler::add_weight_retroactive { if (!check_weighted_particles_validity(weighted_particles)) { RCLCPP_ERROR_STREAM(logger_, "weighted_particles has invalid data"); - throw resampling_skip_exception("weighted_particles has invalid data"); + throw ResamplingSkipException("weighted_particles has invalid data"); } // Initialize corresponding index lookup table @@ -143,7 +143,7 @@ RetroactiveResampler::ParticleArray RetroactiveResampler::resample( // Copy particle to resampled variable resampled_particles.particles[m] = predicted_particles.particles[predicted_particle_index]; // Reset weight uniformly - resampled_particles.particles[m].weight = num_of_particles_inv; + resampled_particles.particles[m].weight = static_cast(num_of_particles_inv); // Make history resampling_history_[latest_resampling_generation_][m] = predicted_particle_index; } @@ -157,7 +157,7 @@ RetroactiveResampler::ParticleArray RetroactiveResampler::resample( return resampled_particles; } -double RetroactiveResampler::random_from_01_uniformly() const +double RetroactiveResampler::random_from_01_uniformly() { static std::default_random_engine engine(0); std::uniform_real_distribution dist(0.0, 1.0); diff --git a/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp b/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp index 124b0500e010e..915237b2b125c 100644 --- a/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp +++ b/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp @@ -22,20 +22,20 @@ namespace mpf = yabloc::modularized_particle_filter; using Particle = yabloc_particle_filter::msg::Particle; using ParticleArray = yabloc_particle_filter::msg::ParticleArray; -constexpr int PARTICLE_COUNT = 10; -constexpr int HISTORY_SIZE = 10; +constexpr int particle_count = 10; +constexpr int history_size = 10; TEST(ResamplerTestSuite, outOfHistory) { - mpf::RetroactiveResampler resampler(PARTICLE_COUNT, HISTORY_SIZE); + mpf::RetroactiveResampler resampler(particle_count, history_size); ParticleArray predicted; ParticleArray weighted; predicted.header.stamp = rclcpp::Time(0); predicted.id = 0; weighted.id = 0; - predicted.particles.resize(PARTICLE_COUNT); - weighted.particles.resize(PARTICLE_COUNT); + predicted.particles.resize(particle_count); + weighted.particles.resize(particle_count); for (auto & p : predicted.particles) p.weight = 1; for (auto & p : weighted.particles) p.weight = 1; @@ -64,7 +64,7 @@ TEST(ResamplerTestSuite, outOfHistory) } // Iterate resampling to fill all history - for (int t = 0; t < HISTORY_SIZE; ++t) { + for (int t = 0; t < history_size; ++t) { auto resampled = resampler.resample(predicted); EXPECT_EQ(resampled.id, t + 1); predicted = resampled; @@ -85,26 +85,26 @@ TEST(ResamplerTestSuite, outOfHistory) TEST(ResamplerTestSuite, simpleResampling) { - mpf::RetroactiveResampler resampler(PARTICLE_COUNT, HISTORY_SIZE); + mpf::RetroactiveResampler resampler(particle_count, history_size); ParticleArray predicted; predicted.header.stamp = rclcpp::Time(0); - predicted.particles.resize(PARTICLE_COUNT); + predicted.particles.resize(particle_count); predicted.id = 0; - for (int i = 0; i < PARTICLE_COUNT; ++i) predicted.particles.at(i).weight = 1; + for (int i = 0; i < particle_count; ++i) predicted.particles.at(i).weight = 1; ParticleArray weighted; - weighted.particles.resize(PARTICLE_COUNT); + weighted.particles.resize(particle_count); // Update by uniform distribution { // Weight weighted.id = 0; - for (int i = 0; i < PARTICLE_COUNT; ++i) weighted.particles.at(i).weight = 0.5; + for (int i = 0; i < particle_count; ++i) weighted.particles.at(i).weight = 0.5; ParticleArray array1 = resampler.add_weight_retroactively(predicted, weighted); // All weights must be equal - for (const auto & p : array1.particles) EXPECT_NEAR(p.weight, 1.0 / PARTICLE_COUNT, 1e-3); + for (const auto & p : array1.particles) EXPECT_NEAR(p.weight, 1.0 / particle_count, 1e-3); // Resample predicted = array1; @@ -117,10 +117,10 @@ TEST(ResamplerTestSuite, simpleResampling) { // Weight weighted.id = 0; - for (int i = 0; i < PARTICLE_COUNT; ++i) { + for (int i = 0; i < particle_count; ++i) { auto & p = predicted.particles.at(i); auto & q = weighted.particles.at(i); - if (i < PARTICLE_COUNT / 2) { + if (i < particle_count / 2) { p.pose.position.x = 1; q.weight = 2.0; } else { @@ -131,12 +131,12 @@ TEST(ResamplerTestSuite, simpleResampling) ParticleArray array1 = resampler.add_weight_retroactively(predicted, weighted); // All weight must match with following expectation - for (int i = 0; i < PARTICLE_COUNT; ++i) { + for (int i = 0; i < particle_count; ++i) { const auto & p = array1.particles.at(i); - if (i < PARTICLE_COUNT / 2) { - EXPECT_NEAR(p.weight, 2.0 / 1.5 / PARTICLE_COUNT, 1e-3f); + if (i < particle_count / 2) { + EXPECT_NEAR(p.weight, 2.0 / 1.5 / particle_count, 1e-3f); } else { - EXPECT_NEAR(p.weight, 1.0 / 1.5 / PARTICLE_COUNT, 1e-3f); + EXPECT_NEAR(p.weight, 1.0 / 1.5 / particle_count, 1e-3f); } } @@ -146,32 +146,33 @@ TEST(ResamplerTestSuite, simpleResampling) predicted = resampled; EXPECT_EQ(predicted.id, 2); - int centroid = 0; - for (const auto & p : predicted.particles) centroid += p.pose.position.x; + int centroid = std::accumulate( + predicted.particles.begin(), predicted.particles.end(), 0, + [](int sum, const auto & p) { return sum + static_cast(p.pose.position.x); }); EXPECT_GT(centroid, 0); } } TEST(ResamplerTestSuite, resamplingWithRetrogression) { - mpf::RetroactiveResampler resampler(PARTICLE_COUNT, HISTORY_SIZE); + mpf::RetroactiveResampler resampler(particle_count, history_size); ParticleArray predicted; predicted.header.stamp = rclcpp::Time(0); - predicted.particles.resize(PARTICLE_COUNT); + predicted.particles.resize(particle_count); predicted.id = 0; - for (int i = 0; i < PARTICLE_COUNT; ++i) { + for (int i = 0; i < particle_count; ++i) { auto & p = predicted.particles.at(i); p.weight = 1.0; - if (i < PARTICLE_COUNT / 2) + if (i < particle_count / 2) p.pose.position.x = 1; else p.pose.position.x = -1; } // Fill all history with biased weighted particles - for (int p = 0; p < HISTORY_SIZE; ++p) { + for (int p = 0; p < history_size; ++p) { auto resampled = resampler.resample(predicted); predicted = resampled; EXPECT_EQ(predicted.id, p + 1); @@ -179,18 +180,17 @@ TEST(ResamplerTestSuite, resamplingWithRetrogression) // Update by ancient measurement { - double before_centroid = 0; - for (const auto & p : predicted.particles) { - before_centroid += p.pose.position.x * p.weight; - } + double before_centroid = std::accumulate( + predicted.particles.begin(), predicted.particles.end(), 0.0, + [](double sum, const auto & p) { return sum + p.pose.position.x * p.weight; }); // Weight ParticleArray weighted; - weighted.particles.resize(PARTICLE_COUNT); + weighted.particles.resize(particle_count); weighted.id = 1; // ancient generation id - for (int i = 0; i < PARTICLE_COUNT; ++i) { + for (int i = 0; i < particle_count; ++i) { auto & q = weighted.particles.at(i); - if (i < PARTICLE_COUNT / 2) { + if (i < particle_count / 2) { q.weight = 2.0; } else { q.weight = 1.0; @@ -199,10 +199,10 @@ TEST(ResamplerTestSuite, resamplingWithRetrogression) predicted = resampler.add_weight_retroactively(predicted, weighted); - double after_centroid = 0; - for (const auto & p : predicted.particles) { - after_centroid += p.pose.position.x * p.weight; - } + double after_centroid = std::accumulate( + predicted.particles.begin(), predicted.particles.end(), 0.0, + [](double sum, const auto & p) { return sum + p.pose.position.x * p.weight; }); + EXPECT_TRUE(after_centroid > before_centroid); } } diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt index 91b272b413c4c..2e6d31ae2f12b 100644 --- a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -20,18 +20,22 @@ find_package(OpenCV REQUIRED) # =================================================== # Executable # Camera -set(TARGET camera_pose_initializer_node) -ament_auto_add_executable(${TARGET} +ament_auto_add_library(${PROJECT_NAME} src/camera/lane_image.cpp src/camera/marker_module.cpp src/camera/projector_module.cpp src/camera/semantic_segmentation.cpp - src/camera/camera_pose_initializer_core.cpp - 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}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) -ament_target_dependencies(${TARGET} OpenCV) + src/camera/camera_pose_initializer_core.cpp) +target_include_directories(${PROJECT_NAME} PUBLIC include) +target_include_directories(${PROJECT_NAME} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} Sophus::Sophus) +ament_target_dependencies(${PROJECT_NAME} OpenCV) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::CameraPoseInitializer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) # =================================================== ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp index 979d1b370699e..eb2942a76e5df 100644 --- a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp @@ -43,7 +43,7 @@ class CameraPoseInitializer : public rclcpp::Node using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped; - CameraPoseInitializer(); + explicit CameraPoseInitializer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const int angle_resolution_; 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 83c2c8fe09287..964cc61420b9c 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 @@ -2,7 +2,7 @@ - + diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index 7ed16c9a8b82d..22e9bdd0dd553 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -22,6 +22,7 @@ lanelet2_extension libopencv-dev rclcpp + rclcpp_components sensor_msgs tier4_localization_msgs visualization_msgs diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp index 083e7dd5bcd43..dd97ea2ad888f 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp @@ -26,8 +26,9 @@ namespace yabloc { -CameraPoseInitializer::CameraPoseInitializer() -: Node("camera_pose_initializer"), angle_resolution_(declare_parameter("angle_resolution")) +CameraPoseInitializer::CameraPoseInitializer(const rclcpp::NodeOptions & options) +: Node("camera_pose_initializer", options), + angle_resolution_(declare_parameter("angle_resolution")) { using std::placeholders::_1; using std::placeholders::_2; @@ -216,3 +217,6 @@ CameraPoseInitializer::PoseCovStamped CameraPoseInitializer::create_rectified_in } } // namespace yabloc + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::CameraPoseInitializer) diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_node.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_node.cpp deleted file mode 100644 index 2bcfe073351f8..0000000000000 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// 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 "yabloc_pose_initializer/camera/camera_pose_initializer.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 21bd39303250f..a30bfa1a8f633 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -150,6 +150,9 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie {{ json_to_markdown("map/map_loader/schema/lanelet2_map_loader.schema.json") }} +`use_waypoints` decides how to handle a centerline. +This flag enables to use the `overwriteLaneletsCenterlineWithWaypoints` function instead of `overwriteLaneletsCenterline`. Please see [the document of the lanelet2_extension package](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#centerline) in detail. + --- ## lanelet2_map_visualization diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index b830e038f1de2..48152605ec0a2 100755 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/map_loader/config/lanelet2_map_loader.param.yaml @@ -1,4 +1,5 @@ /**: ros__parameters: - center_line_resolution: 5.0 # [m] + center_line_resolution: 5.0 # [m] + use_waypoints: true # "centerline" in the Lanelet2 map will be used as a "waypoints" tag. lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path diff --git a/map/map_loader/schema/lanelet2_map_loader.schema.json b/map/map_loader/schema/lanelet2_map_loader.schema.json index fa2b4d363ff92..aae295f847ab2 100644 --- a/map/map_loader/schema/lanelet2_map_loader.schema.json +++ b/map/map_loader/schema/lanelet2_map_loader.schema.json @@ -11,13 +11,18 @@ "description": "Resolution of the Lanelet center line [m]", "default": "5.0" }, + "use_waypoints": { + "type": "boolean", + "description": "If true, `centerline` in the Lanelet2 map will be used as a `waypoints` tag.", + "default": true + }, "lanelet2_map_path": { "type": "string", "description": "The lanelet2 map path pointing to the .osm file", "default": "" } }, - "required": ["center_line_resolution", "lanelet2_map_path"], + "required": ["center_line_resolution", "use_waypoints", "lanelet2_map_path"], "additionalProperties": false } }, diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index b097e1809a385..ee89ad571b90a 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -63,6 +63,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options declare_parameter("lanelet2_map_path"); declare_parameter("center_line_resolution"); + declare_parameter("use_waypoints"); } void Lanelet2MapLoaderNode::on_map_projector_info( @@ -70,6 +71,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( { const auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string(); const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); + const auto use_waypoints = get_parameter("use_waypoints").as_bool(); // load map from file const auto map = load_map(lanelet2_filename, *msg); @@ -79,7 +81,11 @@ void Lanelet2MapLoaderNode::on_map_projector_info( } // overwrite centerline - lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + if (use_waypoints) { + lanelet::utils::overwriteLaneletsCenterlineWithWaypoints(map, center_line_resolution, false); + } else { + lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + } // create map bin msg const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename, now()); diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index bacbabe60a719..349fc2954fe28 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -67,12 +67,8 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - std::map pcd_metadata_dict; - try { - pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); - } catch (std::runtime_error & e) { - RCLCPP_ERROR_STREAM(get_logger(), e.what()); - } + // Parse the metadata file and get the map of (absolute pcd path, pcd file metadata) + auto pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); if (enable_partial_load) { partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); @@ -89,8 +85,25 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( const std::string & pcd_metadata_path, const std::vector & pcd_paths) const { if (fs::exists(pcd_metadata_path)) { + std::set missing_pcd_names; auto pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path); - pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict, pcd_paths); + + pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict, pcd_paths, missing_pcd_names); + + // Warning if some segments are missing + if (!missing_pcd_names.empty()) { + std::ostringstream oss; + + oss << "The following segment(s) are missing from the input PCDs: "; + + for (auto & fname : missing_pcd_names) { + oss << std::endl << fname; + } + + RCLCPP_ERROR_STREAM(get_logger(), oss.str()); + throw std::runtime_error("Missing PCD segments. Exiting map loader..."); + } + return pcd_metadata_dict; } else if (pcd_paths.size() == 1) { // An exception when using a single file PCD map so that the users do not have to provide diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 2430ce74e3e8b..96f9d114ed265 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -50,16 +50,28 @@ std::map loadPCDMetadata(const std::string & pcd_m std::map replaceWithAbsolutePath( const std::map & pcd_metadata_path, - const std::vector & pcd_paths) + const std::vector & pcd_paths, std::set & missing_pcd_names) { + // Initially, assume all segments are missing + for (auto & it : pcd_metadata_path) { + missing_pcd_names.insert(it.first); + } + std::map absolute_path_map; for (const auto & path : pcd_paths) { std::string filename = path.substr(path.find_last_of("/\\") + 1); auto it = pcd_metadata_path.find(filename); if (it != pcd_metadata_path.end()) { absolute_path_map[path] = it->second; + + // If a segment was found from the pcd paths, remove + // it from the missing segments + missing_pcd_names.erase(filename); } } + + // The remaining segments in the @missing_pcd are were not + // found from the pcd paths, which means they are missing return absolute_path_map; } diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 62d0b34e516e3..29d9a24d7b0e7 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -39,7 +40,7 @@ struct PCDFileMetadata std::map loadPCDMetadata(const std::string & pcd_metadata_path); std::map replaceWithAbsolutePath( const std::map & pcd_metadata_path, - const std::vector & pcd_paths); + const std::vector & pcd_paths, std::set & missing_pcd_names); bool cylinderAndBoxOverlapExists( const double center_x, const double center_y, const double radius, diff --git a/map/map_loader/test/lanelet2_map_loader_launch.test.py b/map/map_loader/test/lanelet2_map_loader_launch.test.py index f1aa9e0efe922..9f9a59f565e3f 100644 --- a/map/map_loader/test/lanelet2_map_loader_launch.test.py +++ b/map/map_loader/test/lanelet2_map_loader_launch.test.py @@ -34,7 +34,13 @@ def generate_test_description(): lanelet2_map_loader = Node( package="map_loader", executable="lanelet2_map_loader", - parameters=[{"lanelet2_map_path": lanelet2_map_path, "center_line_resolution": 5.0}], + parameters=[ + { + "lanelet2_map_path": lanelet2_map_path, + "center_line_resolution": 5.0, + "use_waypoints": True, + } + ], ) context = {} diff --git a/map/map_loader/test/test_replace_with_absolute_path.cpp b/map/map_loader/test/test_replace_with_absolute_path.cpp index 10680e05103ce..f61dd188f0679 100644 --- a/map/map_loader/test/test_replace_with_absolute_path.cpp +++ b/map/map_loader/test/test_replace_with_absolute_path.cpp @@ -36,7 +36,8 @@ TEST(ReplaceWithAbsolutePathTest, BasicFunctionality) {"/home/user/pcd/file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}}, }; - auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths); + std::set missing_pcd_names; + auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names); ASSERT_THAT(result, ContainerEq(expected)); } @@ -53,8 +54,9 @@ TEST(ReplaceWithAbsolutePathTest, NoMatchingFiles) }; std::map expected = {}; + std::set missing_pcd_names; - auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths); + auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names); ASSERT_THAT(result, ContainerEq(expected)); } diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 383051e8f67a5..e5b95b613b0d1 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -33,13 +33,9 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi msg.vertical_datum = data["vertical_datum"].as(); msg.mgrs_grid = data["mgrs_grid"].as(); - } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM) { - msg.vertical_datum = data["vertical_datum"].as(); - msg.map_origin.latitude = data["map_origin"]["latitude"].as(); - msg.map_origin.longitude = data["map_origin"]["longitude"].as(); - msg.map_origin.altitude = data["map_origin"]["altitude"].as(); - - } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::TRANSVERSE_MERCATOR) { + } else if ( + msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM || + msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::TRANSVERSE_MERCATOR) { msg.vertical_datum = data["vertical_datum"].as(); msg.map_origin.latitude = data["map_origin"]["latitude"].as(); msg.map_origin.longitude = data["map_origin"]["longitude"].as(); diff --git a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp index fdfe96434fe0d..7d66388c1a91e 100644 --- a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp +++ b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -41,7 +41,7 @@ void save_dummy_mgrs_lanelet2_map(const std::string & mgrs_coord, const std::str file << "\n"; file << "\n"; - file << " \n"; + file << R"( \n"; file << ""; file.close(); diff --git a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp index de53d78d7f782..8efb90cc87c89 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ #define AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ +#include +#include #include -#include -#include #include #include @@ -39,10 +39,10 @@ namespace autoware::crosswalk_traffic_light_estimator { +using autoware::universe_utils::DebugPublisher; +using autoware::universe_utils::StopWatch; using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; -using tier4_autoware_utils::DebugPublisher; -using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; diff --git a/perception/autoware_crosswalk_traffic_light_estimator/package.xml b/perception/autoware_crosswalk_traffic_light_estimator/package.xml index d844ea27b52b5..18b0b8a41264f 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/package.xml +++ b/perception/autoware_crosswalk_traffic_light_estimator/package.xml @@ -15,10 +15,10 @@ autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_universe_utils lanelet2_extension rclcpp rclcpp_components - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 27c2a5079f441..ce1c587cc3b01 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -15,18 +15,18 @@ #ifndef MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ #define MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "map_based_prediction/path_generator.hpp" #include "tf2/LinearMath/Quaternion.h" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include #include #include @@ -116,6 +116,7 @@ struct PredictionTimeHorizon using LaneletsData = std::vector; using ManeuverProbability = std::unordered_map; +using autoware::universe_utils::StopWatch; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; @@ -129,7 +130,6 @@ using autoware_perception_msgs::msg::TrafficLightElement; using autoware_perception_msgs::msg::TrafficLightGroup; using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::StringStamped; using TrajectoryPoints = std::vector; class MapBasedPredictionNode : public rclcpp::Node @@ -143,12 +143,12 @@ class MapBasedPredictionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_markers_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; - tier4_autoware_utils::InterProcessPollingSubscriber sub_traffic_signals_{ - this, "/traffic_signals"}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_traffic_signals_{this, "/traffic_signals"}; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; // Object History std::unordered_map> road_users_history; @@ -169,7 +169,7 @@ class MapBasedPredictionNode : public rclcpp::Node const std::vector & parameters); // Pose Transform Listener - tier4_autoware_utils::TransformListener transform_listener_{this}; + autoware::universe_utils::TransformListener transform_listener_{this}; // Path Generator std::shared_ptr path_generator_; @@ -221,7 +221,7 @@ class MapBasedPredictionNode : public rclcpp::Node bool match_lost_and_appeared_crosswalk_users_; bool remember_lost_crosswalk_users_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // Member Functions void mapCallback(const LaneletMapBin::ConstSharedPtr msg); @@ -314,8 +314,8 @@ class MapBasedPredictionNode : public rclcpp::Node inline std::vector calcTrajectoryCurvatureFrom3Points( const TrajectoryPoints & trajectory, size_t idx_dist) { - using tier4_autoware_utils::calcCurvature; - using tier4_autoware_utils::getPoint; + using autoware::universe_utils::calcCurvature; + using autoware::universe_utils::getPoint; if (trajectory.size() < 3) { const std::vector k_arr(trajectory.size(), 0.0); diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml index 3671c1a01b0a2..37bb7aafb01bf 100644 --- a/perception/autoware_map_based_prediction/package.xml +++ b/perception/autoware_map_based_prediction/package.xml @@ -14,17 +14,17 @@ ament_cmake autoware_cmake + autoware_motion_utils autoware_perception_msgs + autoware_universe_utils glog interpolation lanelet2_extension - motion_utils rclcpp rclcpp_components tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs unique_identifier_msgs visualization_msgs diff --git a/perception/autoware_map_based_prediction/src/debug.cpp b/perception/autoware_map_based_prediction/src/debug.cpp index 0c58d4aae1638..bd40ee0981c0e 100644 --- a/perception/autoware_map_based_prediction/src/debug.cpp +++ b/perception/autoware_map_based_prediction/src/debug.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "map_based_prediction/map_based_prediction_node.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" namespace autoware::map_based_prediction { @@ -29,7 +29,7 @@ visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( marker.type = visualization_msgs::msg::Marker::CUBE; marker.action = visualization_msgs::msg::Marker::ADD; marker.pose = object.kinematics.pose_with_covariance.pose; - marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0); + marker.scale = autoware::universe_utils::createMarkerScale(3.0, 1.0, 1.0); // Color by maneuver double r = 0.0; @@ -42,7 +42,7 @@ visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( } else { b = 1.0; } - marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.8); + marker.color = autoware::universe_utils::createMarkerColor(r, g, b, 0.8); return marker; } diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 20d532d7127d7..f505cb7cc5592 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -14,16 +14,16 @@ #include "map_based_prediction/map_based_prediction_node.hpp" +#include +#include +#include +#include +#include +#include #include #include #include #include -#include -#include -#include -#include -#include -#include #include @@ -92,10 +92,10 @@ double calcAbsLateralOffset( for (size_t i = 0; i < boundary_path.size(); ++i) { const double x = boundary_line[i].x(); const double y = boundary_line[i].y(); - boundary_path[i] = tier4_autoware_utils::createPoint(x, y, 0.0); + boundary_path[i] = autoware::universe_utils::createPoint(x, y, 0.0); } - return std::fabs(motion_utils::calcLateralOffset(boundary_path, search_pose.position)); + return std::fabs(autoware::motion_utils::calcLateralOffset(boundary_path, search_pose.position)); } /** @@ -218,7 +218,7 @@ double calcAbsYawDiffBetweenLaneletAndObject( const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta = std::fabs(normalized_delta_yaw); return abs_norm_delta; } @@ -418,7 +418,7 @@ boost::optional isReachableCrosswalkEdgePoints( const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; - const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + const auto yaw = autoware::universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) { @@ -529,7 +529,7 @@ bool hasPotentialToReach( { const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; - const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + const auto yaw = autoware::universe_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; constexpr double stop_velocity_th = 0.14; // [m/s] const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); @@ -547,16 +547,16 @@ bool hasPotentialToReach( const double pedestrian_to_crosswalk_left_direction = std::atan2(left_point.y() - obj_pos.y, left_point.x() - obj_pos.x); return std::make_pair( - tier4_autoware_utils::normalizeRadian( + autoware::universe_utils::normalizeRadian( pedestrian_to_crosswalk_right_direction - pedestrian_to_crosswalk_center_direction), - tier4_autoware_utils::normalizeRadian( + autoware::universe_utils::normalizeRadian( pedestrian_to_crosswalk_left_direction - pedestrian_to_crosswalk_center_direction)); }(); const double pedestrian_heading_rel_direction = [&]() { const double pedestrian_heading_direction = std::atan2(obj_vel.x * std::sin(yaw), obj_vel.x * std::cos(yaw)); - return tier4_autoware_utils::normalizeRadian( + return autoware::universe_utils::normalizeRadian( pedestrian_heading_direction - pedestrian_to_crosswalk_center_direction); }(); @@ -658,7 +658,7 @@ ObjectClassification::_label_type changeLabelForPrediction( if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; constexpr float high_speed_threshold = - tier4_autoware_utils::kmph2mps(25.0); // High speed bicycle 25 km/h + autoware::universe_utils::kmph2mps(25.0); // High speed bicycle 25 km/h // calc abs speed from x and y velocity const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -673,7 +673,7 @@ ObjectClassification::_label_type changeLabelForPrediction( case ObjectClassification::PEDESTRIAN: { const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); const float max_velocity_for_human_mps = - tier4_autoware_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h + autoware::universe_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); @@ -859,13 +859,15 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); processing_time_publisher_ = - std::make_unique(this, "map_based_prediction"); + std::make_unique(this, "map_based_prediction"); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); - stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_ = + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -873,7 +875,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "max_lateral_accel", max_lateral_accel_); updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_); @@ -1008,14 +1010,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt if ( label_for_prediction == ObjectClassification::PEDESTRIAN || label_for_prediction == ObjectClassification::BICYCLE) { - const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); current_crosswalk_users.emplace(object_id, object); } } std::unordered_set predicted_crosswalk_users_ids; for (const auto & object : in_objects->objects) { - std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + std::string object_id = autoware::universe_utils::toHexString(object.object_id); TrackedObject transformed_object = object; // transform object frame if it's based on map frame @@ -1034,7 +1036,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt switch (label) { case ObjectClassification::PEDESTRIAN: case ObjectClassification::BICYCLE: { - std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (match_lost_and_appeared_crosswalk_users_) { object_id = tryMatchNewObjectToDisappeared(object_id, current_crosswalk_users); } @@ -1337,11 +1339,11 @@ bool MapBasedPredictionNode::isIntersecting( const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4) { - const auto p1 = tier4_autoware_utils::createPoint(point1.x, point1.y, 0.0); - const auto p2 = tier4_autoware_utils::createPoint(point2.x, point2.y, 0.0); - const auto p3 = tier4_autoware_utils::createPoint(point3.x(), point3.y(), 0.0); - const auto p4 = tier4_autoware_utils::createPoint(point4.x(), point4.y(), 0.0); - const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + const auto p1 = autoware::universe_utils::createPoint(point1.x, point1.y, 0.0); + const auto p2 = autoware::universe_utils::createPoint(point2.x, point2.y, 0.0); + const auto p3 = autoware::universe_utils::createPoint(point3.x(), point3.y(), 0.0); + const auto p4 = autoware::universe_utils::createPoint(point4.x(), point4.y(), 0.0); + const auto intersection = autoware::universe_utils::intersect(p1, p2, p3, p4); return intersection.has_value(); } @@ -1489,7 +1491,7 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) // Compute yaw angle from the velocity and position of the object const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; - const auto future_object_pose = tier4_autoware_utils::calcOffsetPose( + const auto future_object_pose = autoware::universe_utils::calcOffsetPose( object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0); // assumption: the object vx is much larger than vy @@ -1508,15 +1510,16 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); // flip the angle object.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::pi + original_yaw); + autoware::universe_utils::createQuaternionFromYaw( + autoware::universe_utils::pi + original_yaw); break; } default: { - const auto updated_object_yaw = - tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position); + const auto updated_object_yaw = autoware::universe_utils::calcAzimuthAngle( + object_pose.position, future_object_pose.position); object.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(updated_object_yaw); + autoware::universe_utils::createQuaternionFromYaw(updated_object_yaw); break; } } @@ -1533,7 +1536,7 @@ void MapBasedPredictionNode::removeStaleTrafficLightInfo( const bool isDisappeared = std::none_of( in_objects->objects.begin(), in_objects->objects.end(), [&it](autoware_perception_msgs::msg::TrackedObject obj) { - return tier4_autoware_utils::toHexString(obj.object_id) == it->first.first; + return autoware::universe_utils::toHexString(obj.object_id) == it->first.first; }); if (isDisappeared) { it = stopped_times_against_green_.erase(it); @@ -1642,7 +1645,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( // If the object is in the objects history, we check if the target lanelet is // inside the current lanelets id or following lanelets - const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) != 0) { const std::vector & possible_lanelet = road_users_history.at(object_id).back().future_possible_lanelets; @@ -1660,7 +1663,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet.second, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta = std::fabs(normalized_delta_yaw); // Step3. Check if the closest lanelet is valid, and add all @@ -1687,7 +1690,7 @@ float MapBasedPredictionNode::calculateLocalLikelihood( const double obj_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double lane_yaw = lanelet::utils::getLaneletAngle(current_lanelet, obj_point); const double delta_yaw = obj_yaw - lane_yaw; - const double abs_norm_delta_yaw = std::fabs(tier4_autoware_utils::normalizeRadian(delta_yaw)); + const double abs_norm_delta_yaw = std::fabs(autoware::universe_utils::normalizeRadian(delta_yaw)); // compute lateral distance const auto centerline = current_lanelet.centerline(); @@ -1697,7 +1700,7 @@ float MapBasedPredictionNode::calculateLocalLikelihood( converted_centerline.push_back(converted_p); } const double lat_dist = - std::fabs(motion_utils::calcLateralOffset(converted_centerline, obj_point)); + std::fabs(autoware::motion_utils::calcLateralOffset(converted_centerline, obj_point)); // Compute Chi-squared distributed (Equation (8) in the paper) const double sigma_d = sigma_lateral_offset_; // Standard Deviation for lateral position @@ -1716,7 +1719,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const LaneletsData & current_lanelets_data) { - std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + std::string object_id = autoware::universe_utils::toHexString(object.object_id); const auto current_lanelets = getLanelets(current_lanelets_data); ObjectData single_object_data; @@ -1725,7 +1728,8 @@ void MapBasedPredictionNode::updateRoadUsersHistory( single_object_data.future_possible_lanelets = current_lanelets; single_object_data.pose = object.kinematics.pose_with_covariance.pose; const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - single_object_data.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(object_yaw); + single_object_data.pose.orientation = + autoware::universe_utils::createQuaternionFromYaw(object_yaw); single_object_data.time_delay = std::fabs((this->get_clock()->now() - header.stamp).seconds()); single_object_data.twist = object.kinematics.twist_with_covariance.twist; @@ -1929,7 +1933,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver( throw std::logic_error("Lane change detection method is invalid."); }(); - const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return current_maneuver; } @@ -1968,7 +1972,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange( const double /*object_detected_time*/) { // Step1. Check if we have the object in the buffer - const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return Maneuver::LANE_FOLLOW; } @@ -2039,7 +2043,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( const double /*object_detected_time*/) { // Step1. Check if we have the object in the buffer - const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return Maneuver::LANE_FOLLOW; } @@ -2077,7 +2081,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( for (const auto & lanelet : prev_lanelets) { const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, prev_pose.position); const double delta_yaw = tf2::getYaw(prev_pose.orientation) - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); if (normalized_delta_yaw < closest_prev_yaw) { closest_prev_yaw = normalized_delta_yaw; prev_lanelet = lanelet; @@ -2087,7 +2091,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( // Step4. Check if the vehicle has changed lane const auto current_lanelet = current_lanelet_data.lanelet; const auto current_pose = object.kinematics.pose_with_covariance.pose; - const double dist = tier4_autoware_utils::calcDistance2d(prev_pose, current_pose); + const double dist = autoware::universe_utils::calcDistance2d(prev_pose, current_pose); lanelet::routing::LaneletPaths possible_paths = routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false); bool has_lane_changed = true; @@ -2172,8 +2176,8 @@ geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay( const double curr_yaw = prev_yaw + wz * dt; geometry_msgs::msg::Pose current_pose; - current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, curr_z); - current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, curr_z); + current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); return current_pose; } @@ -2185,10 +2189,10 @@ double MapBasedPredictionNode::calcRightLateralOffset( for (size_t i = 0; i < boundary_path.size(); ++i) { const double x = boundary_line[i].x(); const double y = boundary_line[i].y(); - boundary_path[i] = tier4_autoware_utils::createPoint(x, y, 0.0); + boundary_path[i] = autoware::universe_utils::createPoint(x, y, 0.0); } - return std::fabs(motion_utils::calcLateralOffset(boundary_path, search_pose.position)); + return std::fabs(autoware::motion_utils::calcLateralOffset(boundary_path, search_pose.position)); } double MapBasedPredictionNode::calcLeftLateralOffset( @@ -2200,7 +2204,7 @@ double MapBasedPredictionNode::calcLeftLateralOffset( void MapBasedPredictionNode::updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths) { - std::string object_id = tier4_autoware_utils::toHexString(object.object_id); + std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return; } @@ -2327,7 +2331,7 @@ std::vector MapBasedPredictionNode::convertPathType( const double lane_yaw = std::atan2( current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x); - current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); + current_p.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); converted_path.push_back(current_p); prev_p = current_p; } @@ -2349,7 +2353,7 @@ std::vector MapBasedPredictionNode::convertPathType( // Prevent from inserting same points if (!converted_path.empty()) { const auto last_p = converted_path.back(); - const double tmp_dist = tier4_autoware_utils::calcDistance2d(last_p, current_p); + const double tmp_dist = autoware::universe_utils::calcDistance2d(last_p, current_p); if (tmp_dist < 1e-6) { prev_p = current_p; continue; @@ -2358,7 +2362,7 @@ std::vector MapBasedPredictionNode::convertPathType( const double lane_yaw = std::atan2( current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x); - current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); + current_p.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); converted_path.push_back(current_p); prev_p = current_p; } @@ -2366,7 +2370,7 @@ std::vector MapBasedPredictionNode::convertPathType( // Resample Path const auto resampled_converted_path = - motion_utils::resamplePoseVector(converted_path, reference_path_resolution_); + autoware::motion_utils::resamplePoseVector(converted_path, reference_path_resolution_); converted_paths.push_back(resampled_converted_path); } @@ -2398,7 +2402,7 @@ bool MapBasedPredictionNode::isDuplicated( for (const auto & prev_predicted_path : predicted_paths) { const auto prev_path_end = prev_predicted_path.path.back().position; const auto current_path_end = predicted_path.path.back().position; - const double dist = tier4_autoware_utils::calcDistance2d(prev_path_end, current_path_end); + const double dist = autoware::universe_utils::calcDistance2d(prev_path_end, current_path_end); if (dist < CLOSE_PATH_THRESHOLD) { return true; } @@ -2447,10 +2451,11 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( return elem_opt ? elem_opt.value().color : TrafficLightElement::UNKNOWN; }(); - const auto key = std::make_pair(tier4_autoware_utils::toHexString(object.object_id), signal_id); + const auto key = + std::make_pair(autoware::universe_utils::toHexString(object.object_id), signal_id); if ( signal_color == TrafficLightElement::GREEN && - tier4_autoware_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < + autoware::universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < threshold_velocity_assumed_as_stopping_) { stopped_times_against_green_.try_emplace(key, this->get_clock()->now()); diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index 873219ffcf130..aeb1b0fedd33f 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -14,9 +14,9 @@ #include "map_based_prediction/path_generator.hpp" +#include +#include #include -#include -#include #include @@ -49,8 +49,9 @@ PredictedPath PathGenerator::generatePathToTargetPoint( const auto arrival_time = pedestrian_to_entry_point.norm() / velocity; const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized(); - const auto pedestrian_to_entry_point_orientation = tier4_autoware_utils::createQuaternionFromYaw( - std::atan2(pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); + const auto pedestrian_to_entry_point_orientation = + autoware::universe_utils::createQuaternionFromYaw(std::atan2( + pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); for (double dt = 0.0; dt < arrival_time + ep; dt += sampling_time_interval_) { geometry_msgs::msg::Pose world_frame_pose; @@ -92,10 +93,11 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( const auto arrival_time = pedestrian_to_entry_point.norm() / velocity; const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized(); - const auto pedestrian_to_entry_point_orientation = tier4_autoware_utils::createQuaternionFromYaw( - std::atan2(pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); + const auto pedestrian_to_entry_point_orientation = + autoware::universe_utils::createQuaternionFromYaw(std::atan2( + pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x())); const auto entry_to_exit_point_normalized = entry_to_exit_point.normalized(); - const auto entry_to_exit_point_orientation = tier4_autoware_utils::createQuaternionFromYaw( + const auto entry_to_exit_point_orientation = autoware::universe_utils::createQuaternionFromYaw( std::atan2(entry_to_exit_point_normalized.y(), entry_to_exit_point_normalized.x())); for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { @@ -172,7 +174,7 @@ PredictedPath PathGenerator::generateStraightPath( path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); path.path.reserve(static_cast((duration) / sampling_time_interval_)); for (double dt = 0.0; dt < duration; dt += sampling_time_interval_) { - const auto future_obj_pose = tier4_autoware_utils::calcOffsetPose( + const auto future_obj_pose = autoware::universe_utils::calcOffsetPose( object_pose, object_twist.linear.x * dt, object_twist.linear.y * dt, 0.0); path.path.push_back(future_obj_pose); } @@ -185,7 +187,7 @@ PredictedPath PathGenerator::generatePolynomialPath( const double lateral_duration, const double speed_limit) const { // Get current Frenet Point - const double ref_path_len = motion_utils::calcArcLength(ref_path); + const double ref_path_len = autoware::motion_utils::calcArcLength(ref_path); const auto current_point = getFrenetPoint(object, ref_path, speed_limit, duration); // Step1. Set Target Frenet Point @@ -317,7 +319,7 @@ PosePath PathGenerator::interpolateReferencePath( base_path_y.at(i) = base_path.at(i).position.y; base_path_z.at(i) = base_path.at(i).position.z; if (i > 0) { - base_path_s.at(i) = base_path_s.at(i - 1) + tier4_autoware_utils::calcDistance2d( + base_path_s.at(i) = base_path_s.at(i - 1) + autoware::universe_utils::calcDistance2d( base_path.at(i - 1), base_path.at(i)); } } @@ -342,16 +344,16 @@ PosePath PathGenerator::interpolateReferencePath( for (size_t i = 0; i < interpolate_num - 1; ++i) { geometry_msgs::msg::Pose interpolated_pose; const auto current_point = - tier4_autoware_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0); - const auto next_point = tier4_autoware_utils::createPoint( + autoware::universe_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0); + const auto next_point = autoware::universe_utils::createPoint( spline_ref_path_x.at(i + 1), spline_ref_path_y.at(i + 1), 0.0); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(current_point, next_point); - interpolated_pose.position = tier4_autoware_utils::createPoint( + const double yaw = autoware::universe_utils::calcAzimuthAngle(current_point, next_point); + interpolated_pose.position = autoware::universe_utils::createPoint( spline_ref_path_x.at(i), spline_ref_path_y.at(i), spline_ref_path_z.at(i)); - interpolated_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + interpolated_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); interpolated_path.at(i) = interpolated_pose; } - interpolated_path.back().position = tier4_autoware_utils::createPoint( + interpolated_path.back().position = autoware::universe_utils::createPoint( spline_ref_path_x.back(), spline_ref_path_y.back(), spline_ref_path_z.back()); interpolated_path.back().orientation = interpolated_path.at(interpolate_num - 2).orientation; @@ -373,14 +375,15 @@ PredictedPath PathGenerator::convertToPredictedPath( const auto & frenet_point = frenet_predicted_path.at(i); // Converted Pose - auto predicted_pose = tier4_autoware_utils::calcOffsetPose(ref_pose, 0.0, frenet_point.d, 0.0); + auto predicted_pose = + autoware::universe_utils::calcOffsetPose(ref_pose, 0.0, frenet_point.d, 0.0); predicted_pose.position.z = object.kinematics.pose_with_covariance.pose.position.z; if (i == 0) { predicted_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation; } else { - const double yaw = tier4_autoware_utils::calcAzimuthAngle( + const double yaw = autoware::universe_utils::calcAzimuthAngle( predicted_path.path.at(i - 1).position, predicted_pose.position); - predicted_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + predicted_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); } predicted_path.path.at(i) = predicted_pose; } @@ -395,9 +398,10 @@ FrenetPoint PathGenerator::getFrenetPoint( FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; - const size_t nearest_segment_idx = motion_utils::findNearestSegmentIndex(ref_path, obj_point); - const double l = - motion_utils::calcLongitudinalOffsetToSegment(ref_path, nearest_segment_idx, obj_point); + const size_t nearest_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(ref_path, obj_point); + const double l = autoware::motion_utils::calcLongitudinalOffsetToSegment( + ref_path, nearest_segment_idx, obj_point); const float vx = static_cast(object.kinematics.twist_with_covariance.twist.linear.x); const float vy = static_cast(object.kinematics.twist_with_covariance.twist.linear.y); const float obj_yaw = @@ -476,8 +480,9 @@ FrenetPoint PathGenerator::getFrenetPoint( const float acceleration_adjusted_velocity_x = get_acceleration_adjusted_velocity(vx, ax); const float acceleration_adjusted_velocity_y = get_acceleration_adjusted_velocity(vy, ay); - frenet_point.s = motion_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l; - frenet_point.d = motion_utils::calcLateralOffset(ref_path, obj_point); + frenet_point.s = + autoware::motion_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l; + frenet_point.d = autoware::motion_utils::calcLateralOffset(ref_path, obj_point); frenet_point.s_vel = acceleration_adjusted_velocity_x * std::cos(delta_yaw) - acceleration_adjusted_velocity_y * std::sin(delta_yaw); frenet_point.d_vel = acceleration_adjusted_velocity_x * std::sin(delta_yaw) + diff --git a/perception/cluster_merger/include/cluster_merger/node.hpp b/perception/cluster_merger/include/cluster_merger/node.hpp index f36116378af78..a60a76f1c2ef0 100644 --- a/perception/cluster_merger/include/cluster_merger/node.hpp +++ b/perception/cluster_merger/include/cluster_merger/node.hpp @@ -15,11 +15,11 @@ #ifndef CLUSTER_MERGER__NODE_HPP_ #define CLUSTER_MERGER__NODE_HPP_ +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "message_filters/subscriber.h" #include "message_filters/sync_policies/approximate_time.h" #include "message_filters/synchronizer.h" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" @@ -59,7 +59,7 @@ class ClusterMergerNode : public rclcpp::Node std::string output_frame_id_; std::vector::SharedPtr> sub_objects_array{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; void objectsCallback( const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, diff --git a/perception/cluster_merger/package.xml b/perception/cluster_merger/package.xml index 220837bfd7e4e..17455dc27b6cc 100644 --- a/perception/cluster_merger/package.xml +++ b/perception/cluster_merger/package.xml @@ -12,12 +12,12 @@ ament_cmake_auto + autoware_universe_utils geometry_msgs message_filters object_recognition_utils rclcpp rclcpp_components - tier4_autoware_utils tier4_perception_msgs autoware_cmake ament_lint_auto diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt index d56cb3fb31584..a76b0fc56e2a0 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/compare_map_segmentation/CMakeLists.txt @@ -44,7 +44,7 @@ ament_target_dependencies(compare_map_segmentation rclcpp rclcpp_components sensor_msgs - tier4_autoware_utils + autoware_universe_utils autoware_map_msgs nav_msgs ) diff --git a/perception/compare_map_segmentation/package.xml b/perception/compare_map_segmentation/package.xml index ca427db9811ba..7403dc0713475 100644 --- a/perception/compare_map_segmentation/package.xml +++ b/perception/compare_map_segmentation/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_map_msgs + autoware_universe_utils grid_map_pcl grid_map_ros nav_msgs @@ -26,7 +27,6 @@ rclcpp rclcpp_components sensor_msgs - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp index 0f870d37c0de1..2962ba1e28a75 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp @@ -109,8 +109,8 @@ DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent( { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "distance_based_compare_map_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 7ad479a6e74bd..e14ae7d55a1d4 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -72,8 +72,8 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_based_approximate_compare_map_filter"); diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp index 595336145dabd..449ae76181bc6 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp @@ -31,8 +31,8 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_based_compare_map_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index f169e2069aee7..0a660bfffd7fb 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -101,8 +101,8 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "voxel_distance_based_compare_map_filter"); diff --git a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp b/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp index 424ff87cbc0d7..02e4c9c3ca29a 100644 --- a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp +++ b/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp @@ -15,8 +15,8 @@ #ifndef DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_ #define DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_ +#include #include -#include #include #include @@ -36,7 +36,7 @@ class DetectedObjectFeatureRemover : public rclcpp::Node private: rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void objectCallback(const DetectedObjectsWithFeature::ConstSharedPtr input); void convert(const DetectedObjectsWithFeature & objs_with_feature, DetectedObjects & objs); }; diff --git a/perception/detected_object_feature_remover/package.xml b/perception/detected_object_feature_remover/package.xml index 4beafa18d5146..9eb47e0e69557 100644 --- a/perception/detected_object_feature_remover/package.xml +++ b/perception/detected_object_feature_remover/package.xml @@ -11,9 +11,9 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils rclcpp rclcpp_components - tier4_autoware_utils tier4_perception_msgs ament_cmake_ros diff --git a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp b/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp index c5f977a4a3354..3fe9e6fde8683 100644 --- a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp +++ b/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp @@ -23,7 +23,8 @@ DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOpt pub_ = this->create_publisher("~/output", rclcpp::QoS(1)); sub_ = this->create_subscription( "~/input", 1, std::bind(&DetectedObjectFeatureRemover::objectCallback, this, _1)); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void DetectedObjectFeatureRemover::objectCallback( diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index fa28362d85fe8..9b9894d56a424 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -17,11 +17,11 @@ #include "detected_object_validation/utils/utils.hpp" +#include +#include +#include #include #include -#include -#include -#include #include #include @@ -35,10 +35,10 @@ namespace object_lanelet_filter { -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::MultiPoint2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; class ObjectLaneletFilterNode : public rclcpp::Node { @@ -52,7 +52,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr object_pub_; rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr object_sub_; - std::unique_ptr debug_publisher_{nullptr}; + std::unique_ptr debug_publisher_{nullptr}; lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; @@ -89,7 +89,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node const autoware_perception_msgs::msg::DetectedObject & object); geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &); - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace object_lanelet_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index 02ec00fe0457f..ea70d62fd952d 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -17,9 +17,9 @@ #include "detected_object_validation/utils/utils.hpp" +#include +#include #include -#include -#include #include #include @@ -54,7 +54,7 @@ class ObjectPositionFilterNode : public rclcpp::Node utils::FilterTargetLabel filter_target_; bool isObjectInBounds(const autoware_perception_msgs::msg::DetectedObject & object) const; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace object_position_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 5362dab62432a..47685dec7dbdc 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -19,9 +19,9 @@ #include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp" +#include +#include #include -#include -#include #include #include @@ -131,7 +131,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node rclcpp::Publisher::SharedPtr objects_pub_; message_filters::Subscriber objects_sub_; message_filters::Subscriber obstacle_pointcloud_sub_; - std::unique_ptr debug_publisher_{nullptr}; + std::unique_ptr debug_publisher_{nullptr}; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -145,7 +145,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node std::shared_ptr debugger_; bool using_2d_validator_; std::unique_ptr validator_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; private: void onObjectsAndObstaclePointCloud( diff --git a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp index 62c3e43d8e730..beb8faf5db1a3 100644 --- a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp @@ -15,11 +15,11 @@ #ifndef DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ #define DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#include #include #include #include #include -#include #include #include @@ -43,7 +43,7 @@ class OccupancyGridBasedValidator : public rclcpp::Node message_filters::Subscriber occ_grid_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; typedef message_filters::sync_policies::ApproximateTime< autoware_perception_msgs::msg::DetectedObjects, nav_msgs::msg::OccupancyGrid> diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index c48785438324c..7abfbe6625f01 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -19,6 +19,7 @@ autoware_map_msgs autoware_perception_msgs + autoware_universe_utils geometry_msgs lanelet2_extension message_filters @@ -30,7 +31,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 05c5426944ec7..cc3a600783a8f 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -14,10 +14,10 @@ #include "detected_object_validation/detected_object_filter/object_lanelet_filter.hpp" +#include #include #include #include -#include #include #include @@ -63,8 +63,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod "output/object", rclcpp::QoS{1}); debug_publisher_ = - std::make_unique(this, "object_lanelet_filter"); - published_time_publisher_ = std::make_unique(this); + std::make_unique(this, "object_lanelet_filter"); + published_time_publisher_ = + std::make_unique(this); } void ObjectLaneletFilterNode::mapCallback( @@ -236,7 +237,8 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( const auto footprint = setFootprint(object); for (const auto & point : footprint.points) { const geometry_msgs::msg::Point32 point_transformed = - tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + autoware::universe_utils::transformPoint( + point, object.kinematics.pose_with_covariance.pose); polygon.outer().emplace_back(point_transformed.x, point_transformed.y); } polygon.outer().push_back(polygon.outer().front()); @@ -245,7 +247,8 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( // if object do not have bounding box, check each footprint is inside polygon for (const auto & point : object.shape.footprint.points) { const geometry_msgs::msg::Point32 point_transformed = - tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + autoware::universe_utils::transformPoint( + point, object.kinematics.pose_with_covariance.pose); geometry_msgs::msg::Pose point2d; point2d.position.x = point_transformed.x; point2d.position.y = point_transformed.y; @@ -295,7 +298,7 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_velocity_yaw - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold) { diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index 278b4829cae40..dccff8a6ccc67 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -42,7 +42,8 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n "input/object", rclcpp::QoS{1}, std::bind(&ObjectPositionFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void ObjectPositionFilterNode::objectCallback( diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 0dd3c1ed08f15..fbb3f863be7c4 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -14,8 +14,8 @@ #include "detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" +#include #include -#include #include @@ -33,7 +33,7 @@ namespace obstacle_pointcloud_based_validator { namespace bg = boost::geometry; using Shape = autoware_perception_msgs::msg::Shape; -using Polygon2d = tier4_autoware_utils::Polygon2d; +using Polygon2d = autoware::universe_utils::Polygon2d; Validator::Validator(PointsNumThresholdParam & points_num_threshold_param) { @@ -96,8 +96,8 @@ std::optional Validator2D::getPointCloudWithinObject( { std::vector vertices_array; pcl::Vertices vertices; - Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + Polygon2d poly2d = autoware::universe_utils::toPolygon2d( + object.kinematics.pose_with_covariance.pose, object.shape); if (bg::is_empty(poly2d)) return std::nullopt; pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); @@ -211,8 +211,8 @@ std::optional Validator3D::getPointCloudWithinObject( auto const object_height = object.shape.dimensions.x; auto z_min = object_position.z - object_height / 2.0f; auto z_max = object_position.z + object_height / 2.0f; - Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + Polygon2d poly2d = autoware::universe_utils::toPolygon2d( + object.kinematics.pose_with_covariance.pose, object.shape); if (bg::is_empty(poly2d)) return std::nullopt; pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); @@ -304,12 +304,13 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - debug_publisher_ = std::make_unique( + debug_publisher_ = std::make_unique( this, "obstacle_pointcloud_based_validator"); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 792483eeebf81..b8682a6e56b3b 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -14,9 +14,9 @@ #include "detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" +#include #include #include -#include #include @@ -33,7 +33,7 @@ namespace occupancy_grid_based_validator { using Shape = autoware_perception_msgs::msg::Shape; -using Polygon2d = tier4_autoware_utils::Polygon2d; +using Polygon2d = autoware::universe_utils::Polygon2d; OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptions & node_options) : rclcpp::Node("occupancy_grid_based_validator", node_options), @@ -52,7 +52,8 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio mean_threshold_ = declare_parameter("mean_threshold", 0.6); enable_debug_ = declare_parameter("enable_debug", false); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void OccupancyGridBasedValidator::onObjectsAndOccGrid( @@ -106,8 +107,8 @@ std::optional OccupancyGridBasedValidator::getMask( const auto & resolution = occupancy_grid.info.resolution; const auto & origin = occupancy_grid.info.origin; std::vector pixel_vertices; - Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + Polygon2d poly2d = autoware::universe_utils::toPolygon2d( + object.kinematics.pose_with_covariance.pose, object.shape); bool is_polygon_within_image = true; for (const auto & p : poly2d.outer()) { diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp index 51e93bfea33dd..0169082a2253a 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -15,13 +15,13 @@ #ifndef DETECTION_BY_TRACKER__DEBUGGER_HPP_ #define DETECTION_BY_TRACKER__DEBUGGER_HPP_ +#include +#include #include #include #include #include #include -#include -#include #include #include @@ -60,9 +60,9 @@ class Debugger divided_objects_pub_ = node->create_publisher( "debug/divided_objects", 1); processing_time_publisher_ = - std::make_unique(node, "detection_by_tracker"); + std::make_unique(node, "detection_by_tracker"); stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); this->startStopWatch(); } @@ -103,8 +103,8 @@ class Debugger rclcpp::Publisher::SharedPtr merged_objects_pub_; rclcpp::Publisher::SharedPtr divided_objects_pub_; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; autoware_perception_msgs::msg::DetectedObjects removeFeature( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index d0e5cc8436ec8..d6d030aadb000 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -18,12 +18,12 @@ #include "detection_by_tracker/debugger.hpp" #include "detection_by_tracker/utils.hpp" +#include #include #include #include #include #include -#include #include #include @@ -84,7 +84,7 @@ class DetectionByTracker : public rclcpp::Node detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; void setMaxSearchRange(); diff --git a/perception/detection_by_tracker/package.xml b/perception/detection_by_tracker/package.xml index dae58f2d7da78..52d4d3fcdd7a2 100644 --- a/perception/detection_by_tracker/package.xml +++ b/perception/detection_by_tracker/package.xml @@ -13,6 +13,7 @@ autoware_cmake eigen3_cmake_module + autoware_universe_utils eigen euclidean_cluster libpcl-all-dev @@ -24,7 +25,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 8ee6cc2739b43..4e502b359120d 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -16,8 +16,8 @@ #include "object_recognition_utils/object_recognition_utils.hpp" -#include -#include +#include +#include #include #include @@ -61,7 +61,7 @@ boost::optional getReferenceYawInfo(const uint8_t label, const const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label; if (is_vehicle) { - return ReferenceYawInfo{yaw, tier4_autoware_utils::deg2rad(30)}; + return ReferenceYawInfo{yaw, autoware::universe_utils::deg2rad(30)}; } else { return boost::none; } @@ -136,9 +136,9 @@ bool TrackerHandler::estimateTrackedObjects( estimated_object.kinematics.pose_with_covariance.pose.position.y = y + vx * std::sin(yaw) * dt.seconds(); estimated_object.kinematics.pose_with_covariance.pose.position.z = z; - const float yaw_hat = tier4_autoware_utils::normalizeRadian(yaw + wz * dt.seconds()); + const float yaw_hat = autoware::universe_utils::normalizeRadian(yaw + wz * dt.seconds()); estimated_object.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw_hat); + autoware::universe_utils::createQuaternionFromYaw(yaw_hat); output.objects.push_back(estimated_object); } return true; @@ -177,7 +177,8 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void DetectionByTracker::setMaxSearchRange() @@ -283,7 +284,7 @@ void DetectionByTracker::divideUnderSegmentedObjects( for (const auto & initial_object : in_cluster_objects.feature_objects) { // search near object - const float distance = tier4_autoware_utils::calcDistance2d( + const float distance = autoware::universe_utils::calcDistance2d( tracked_object.kinematics.pose_with_covariance.pose, initial_object.object.kinematics.pose_with_covariance.pose); if (max_search_range < distance) { @@ -419,7 +420,7 @@ void DetectionByTracker::mergeOverSegmentedObjects( pcl::PointCloud pcl_merged_cluster; for (const auto & initial_object : in_cluster_objects.feature_objects) { - const float distance = tier4_autoware_utils::calcDistance2d( + const float distance = autoware::universe_utils::calcDistance2d( tracked_object.kinematics.pose_with_covariance.pose, initial_object.object.kinematics.pose_with_covariance.pose); diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index 88f7bf4a9c2a9..fda0fcddc1bc7 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -15,12 +15,12 @@ #ifndef ELEVATION_MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ #define ELEVATION_MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ +#include #include #include #include #include #include -#include #include "tier4_external_api_msgs/msg/map_hash.hpp" #include diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index 26412539d8571..afbd88da84bde 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -12,11 +12,12 @@ ament_cmake_auto autoware_cmake + autoware_grid_map_utils autoware_map_msgs + autoware_universe_utils grid_map_cv grid_map_pcl grid_map_ros - grid_map_utils lanelet2_extension libpcl-all-dev pcl_conversions @@ -25,7 +26,6 @@ rosbag2_storage_default_plugins tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_external_api_msgs ament_lint_auto diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index afb85fc9318de..3b249e45ae194 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -14,12 +14,12 @@ #include "elevation_map_loader/elevation_map_loader_node.hpp" +#include #include #include #include #include #include -#include #include #include #include @@ -356,7 +356,7 @@ void ElevationMapLoaderNode::inpaintElevationMap(const float radius) // Convert elevation layer to OpenCV image to fill in holes. // Get the inpaint mask (nonzero pixels indicate where values need to be filled in). namespace bg = boost::geometry; - using tier4_autoware_utils::Point2d; + using autoware::universe_utils::Point2d; elevation_map_.add("inpaint_mask", 0.0); @@ -382,8 +382,8 @@ void ElevationMapLoaderNode::inpaintElevationMap(const float radius) for (const auto & p : lane_polygon) { polygon.addVertex(grid_map::Position(p[0], p[1])); } - for (grid_map_utils::PolygonIterator iterator(elevation_map_, polygon); !iterator.isPastEnd(); - ++iterator) { + for (autoware::grid_map_utils::PolygonIterator iterator(elevation_map_, polygon); + !iterator.isPastEnd(); ++iterator) { if (!elevation_map_.isValid(*iterator, layer_name_)) { elevation_map_.at("inpaint_mask", *iterator) = 1.0; } diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index 746ef1bafb583..f986b0334935f 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -38,9 +38,10 @@ EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) cluster_pub_ = this->create_publisher( "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); - stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_ = + std::make_unique>(); debug_publisher_ = - std::make_unique(this, "euclidean_cluster"); + std::make_unique(this, "euclidean_cluster"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp index 16e179b2512d3..92f10696d17dc 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp @@ -16,9 +16,9 @@ #include "euclidean_cluster/euclidean_cluster.hpp" +#include +#include #include -#include -#include #include #include @@ -41,8 +41,8 @@ class EuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index 7e6a456561900..e9425095a3b0d 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -42,8 +42,9 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( cluster_pub_ = this->create_publisher( "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); - stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique( + stop_watch_ptr_ = + std::make_unique>(); + debug_publisher_ = std::make_unique( this, "voxel_grid_based_euclidean_cluster"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp index 18330b5f12074..b0c5d0e5a7fbf 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -16,9 +16,9 @@ #include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include +#include #include -#include -#include #include #include @@ -41,8 +41,8 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace euclidean_cluster diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index 314c9cfb97a75..3ae41173a7a7d 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -280,9 +280,9 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult onParameter(const std::vector & p); // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index e638baba12521..7853aa400b6a5 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -14,10 +14,10 @@ #include "ground_segmentation/scan_ground_filter_nodelet.hpp" +#include +#include +#include #include -#include -#include -#include #include #include @@ -25,12 +25,12 @@ namespace ground_segmentation { +using autoware::universe_utils::calcDistance3d; +using autoware::universe_utils::deg2rad; +using autoware::universe_utils::normalizeDegree; +using autoware::universe_utils::normalizeRadian; using autoware::vehicle_info_utils::VehicleInfoUtils; using pointcloud_preprocessor::get_param; -using tier4_autoware_utils::calcDistance3d; -using tier4_autoware_utils::deg2rad; -using tier4_autoware_utils::normalizeDegree; -using tier4_autoware_utils::normalizeRadian; ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("ScanGroundFilter", options) @@ -79,8 +79,8 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "scan_ground_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 0f4cb7112f74a..b6dfbaf402cc8 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -127,6 +127,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_library(pointpainting_lib SHARED src/pointpainting_fusion/node.cpp + src/pointpainting_fusion/pointcloud_densification.cpp src/pointpainting_fusion/pointpainting_trt.cpp src/pointpainting_fusion/voxel_generator.cpp ) diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index a2a78c99cd5ba..df8bf66433300 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -15,10 +15,10 @@ #ifndef IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ #define IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ +#include +#include #include #include -#include -#include #include #include @@ -138,8 +138,8 @@ class FusionNode : public rclcpp::Node float filter_scope_max_z_; /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp new file mode 100644 index 0000000000000..03609eb18e689 --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp @@ -0,0 +1,72 @@ +// Copyright 2024 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 IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ + +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include +#include + +namespace image_projection_based_fusion +{ +struct PointCloudWithTransform +{ + sensor_msgs::msg::PointCloud2 pointcloud_msg; + Eigen::Affine3f affine_past2world; +}; + +class PointCloudDensification +{ +public: + explicit PointCloudDensification(const centerpoint::DensificationParam & param); + + bool enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); + + double getCurrentTimestamp() const { return current_timestamp_; } + Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; } + std::list::iterator getPointCloudCacheIter() + { + return pointcloud_cache_.begin(); + } + bool isCacheEnd(std::list::iterator iter) + { + return iter == pointcloud_cache_.end(); + } + unsigned int pointcloud_cache_size() const { return param_.pointcloud_cache_size(); } + +private: + void enqueue(const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine); + void dequeue(); + + centerpoint::DensificationParam param_; + double current_timestamp_{0.0}; + Eigen::Affine3f affine_world2current_; + std::list pointcloud_cache_; +}; + +} // namespace image_projection_based_fusion + +#endif // IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp old mode 100755 new mode 100644 index d0f44b9d58706..4cdca8e49ac7e --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp @@ -15,19 +15,35 @@ #ifndef IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_ #define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_ +#include +#include #include #include +#include #include namespace image_projection_based_fusion { -class VoxelGenerator : public centerpoint::VoxelGenerator + +class VoxelGenerator { public: - using centerpoint::VoxelGenerator::VoxelGenerator; + explicit VoxelGenerator( + const centerpoint::DensificationParam & param, const centerpoint::CenterPointConfig & config); + + bool enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); + + std::size_t generateSweepPoints(std::vector & points); + +protected: + std::unique_ptr pd_ptr_{nullptr}; - std::size_t generateSweepPoints(std::vector & points) override; + centerpoint::CenterPointConfig config_; + std::array range_; + std::array grid_size_; + std::array recip_voxel_size_; }; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index a59b6992c5328..65e03c6bc6316 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -15,8 +15,8 @@ #ifndef IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ #define IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ +#include "autoware/universe_utils/ros/debug_publisher.hpp" #include "image_projection_based_fusion/fusion_node.hpp" -#include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "autoware_perception_msgs/msg/object_classification.hpp" diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp index 0b6497135612e..ffa1666396a1d 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp @@ -34,10 +34,10 @@ #include "image_projection_based_fusion/fusion_node.hpp" +#include #include #include #include -#include #include "autoware_perception_msgs/msg/shape.hpp" #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index c1bd85cb945de..8d5a2ef1fe519 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -18,6 +18,7 @@ autoware_perception_msgs autoware_point_types + autoware_universe_utils cv_bridge euclidean_cluster image_transport @@ -33,7 +34,6 @@ tf2_eigen tf2_ros tf2_sensor_msgs - tier4_autoware_utils tier4_perception_msgs ament_cmake_gtest diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index fa2dcd2b0e7e1..20273fd1de547 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -130,8 +130,8 @@ FusionNode::FusionNode( // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, get_name()); stop_watch_ptr_->tic("cyclic_time"); 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 24ff1245813ac..7973c22cc1d78 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -16,6 +16,8 @@ #include "autoware_point_types/types.hpp" +#include +#include #include #include #include @@ -23,8 +25,6 @@ #include #include #include -#include -#include #include diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp new file mode 100644 index 0000000000000..9edf6a73cd59b --- /dev/null +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp @@ -0,0 +1,103 @@ +// Copyright 2024 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 "image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp" + +#include + +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +namespace +{ +boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, + const std::string & source_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_centerpoint"), ex.what()); + return boost::none; + } +} + +Eigen::Affine3f transformToEigen(const geometry_msgs::msg::Transform & t) +{ + Eigen::Affine3f a; + a.matrix() = tf2::transformToEigen(t).matrix().cast(); + return a; +} + +} // namespace + +namespace image_projection_based_fusion +{ +PointCloudDensification::PointCloudDensification(const centerpoint::DensificationParam & param) +: param_(param) +{ +} + +bool PointCloudDensification::enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer) +{ + const auto header = pointcloud_msg.header; + + if (param_.pointcloud_cache_size() > 1) { + auto transform_world2current = + getTransform(tf_buffer, header.frame_id, param_.world_frame_id(), header.stamp); + if (!transform_world2current) { + return false; + } + auto affine_world2current = transformToEigen(transform_world2current.get()); + + enqueue(pointcloud_msg, affine_world2current); + } else { + enqueue(pointcloud_msg, Eigen::Affine3f::Identity()); + } + + dequeue(); + + return true; +} + +void PointCloudDensification::enqueue( + const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current) +{ + affine_world2current_ = affine_world2current; + current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds(); + PointCloudWithTransform pointcloud = {msg, affine_world2current.inverse()}; + pointcloud_cache_.push_front(pointcloud); +} + +void PointCloudDensification::dequeue() +{ + if (pointcloud_cache_.size() > param_.pointcloud_cache_size()) { + pointcloud_cache_.pop_back(); + } +} + +} // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index d44620995c61b..c1e431ed83c99 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -14,10 +14,10 @@ #include "image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp" +#include #include #include #include -#include #include #include diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index ea10cb1237436..cb3fc33d3e022 100755 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -18,6 +18,32 @@ namespace image_projection_based_fusion { + +VoxelGenerator::VoxelGenerator( + const centerpoint::DensificationParam & param, const centerpoint::CenterPointConfig & config) +: config_(config) +{ + pd_ptr_ = std::make_unique(param); + range_[0] = config.range_min_x_; + range_[1] = config.range_min_y_; + range_[2] = config.range_min_z_; + range_[3] = config.range_max_x_; + range_[4] = config.range_max_y_; + range_[5] = config.range_max_z_; + grid_size_[0] = config.grid_size_x_; + grid_size_[1] = config.grid_size_y_; + grid_size_[2] = config.grid_size_z_; + recip_voxel_size_[0] = 1 / config.voxel_size_x_; + recip_voxel_size_[1] = 1 / config.voxel_size_y_; + recip_voxel_size_[2] = 1 / config.voxel_size_z_; +} + +bool VoxelGenerator::enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer) +{ + return pd_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer); +} + size_t VoxelGenerator::generateSweepPoints(std::vector & points) { Eigen::Vector3f point_current, point_past; diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index cb462e103a216..ea5c2005bdd46 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -42,14 +42,14 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); } -void RoiPointCloudFusionNode::preprocess(__attribute__((unused)) - sensor_msgs::msg::PointCloud2 & pointcloud_msg) +void RoiPointCloudFusionNode::preprocess( + __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) { return; } -void RoiPointCloudFusionNode::postprocess(__attribute__((unused)) - sensor_msgs::msg::PointCloud2 & pointcloud_msg) +void RoiPointCloudFusionNode::postprocess( + __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) { const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + pub_objects_ptr_->get_intra_process_subscription_count(); diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 950c73bc2fb6d..2e997f3e8e60f 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -70,7 +70,7 @@ void closest_cluster( std::memcpy(&point.y, &cluster.data[i * point_step + y_offset], sizeof(float)); std::memcpy(&point.z, &cluster.data[i * point_step + z_offset], sizeof(float)); - point_data.distance = tier4_autoware_utils::calcDistance2d(center, point); + point_data.distance = autoware::universe_utils::calcDistance2d(center, point); point_data.orig_index = i; points_data.push_back(point_data); } @@ -258,7 +258,7 @@ pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); for (std::size_t i = 0; i < cluster.points.size(); ++i) { pcl::PointXYZ point = cluster.points.at(i); - double dist_closest_point = tier4_autoware_utils::calcDistance2d(point, orig_point); + double dist_closest_point = autoware::universe_utils::calcDistance2d(point, orig_point); if (min_dist > dist_closest_point) { min_dist = dist_closest_point; closest_point = pcl::PointXYZ(point.x, point.y, point.z); diff --git a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt index c4c73927ad255..462e9d22f9da2 100644 --- a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(pcl_conversions REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(tf2_eigen REQUIRED) -find_package(tier4_autoware_utils REQUIRED) +find_package(autoware_universe_utils REQUIRED) find_package(tier4_debug_msgs REQUIRED) find_package(tier4_perception_msgs REQUIRED) @@ -40,7 +40,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE "$" ${cuda_utils_INCLUDE_DIRS} ${pcl_conversions_INCLUDE_DIRS} - ${tier4_autoware_utils_INCLUDE_DIRS} + ${autoware_universe_utils_INCLUDE_DIRS} ${autoware_perception_msgs_INCLUDE_DIRS} ) diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp index ca2ae95149e9a..1e5966f5751af 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp @@ -19,11 +19,11 @@ #include "feature_generator.hpp" #include "lidar_apollo_instance_segmentation/node.hpp" +#include #include #include #include #include -#include #include #include diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp index 0ed5f37b6fa14..f159f125677c4 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp @@ -17,9 +17,9 @@ #include "lidar_apollo_instance_segmentation/debugger.hpp" +#include +#include #include -#include -#include #include #include @@ -47,8 +47,8 @@ class LidarInstanceSegmentationNode : public rclcpp::Node std::shared_ptr detector_ptr_; std::shared_ptr debugger_ptr_; void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; public: explicit LidarInstanceSegmentationNode(const rclcpp::NodeOptions & node_options); diff --git a/perception/lidar_apollo_instance_segmentation/package.xml b/perception/lidar_apollo_instance_segmentation/package.xml index c5b3dbb9c0585..1157b7575933c 100755 --- a/perception/lidar_apollo_instance_segmentation/package.xml +++ b/perception/lidar_apollo_instance_segmentation/package.xml @@ -13,6 +13,7 @@ ament_cmake autoware_perception_msgs + autoware_universe_utils cuda_utils libpcl-all-dev pcl_conversions @@ -20,7 +21,6 @@ rclcpp_components tensorrt_common tf2_eigen - tier4_autoware_utils tier4_debug_msgs tier4_perception_msgs diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index 236f0998ab21f..b704bab39338a 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -88,7 +88,8 @@ bool LidarApolloInstanceSegmentation::transformCloud( target_frame_, input.header.frame_id, input.header.stamp, std::chrono::milliseconds(500)); Eigen::Matrix4f affine_matrix = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - tier4_autoware_utils::transformPointCloud(pcl_input, pcl_transformed_cloud, affine_matrix); + autoware::universe_utils::transformPointCloud( + pcl_input, pcl_transformed_cloud, affine_matrix); transformed_cloud.header.frame_id = target_frame_; pcl_transformed_cloud.header.frame_id = target_frame_; } catch (tf2::TransformException & ex) { @@ -103,7 +104,7 @@ bool LidarApolloInstanceSegmentation::transformCloud( pcl::PointCloud pointcloud_with_z_offset; Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); - tier4_autoware_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( pcl_transformed_cloud, pcl_transformed_cloud, z_up_transform); pcl::toROSMsg(pcl_transformed_cloud, transformed_cloud); diff --git a/perception/lidar_apollo_instance_segmentation/src/node.cpp b/perception/lidar_apollo_instance_segmentation/src/node.cpp index 5bf8ed0d027e5..93b2f3344de99 100644 --- a/perception/lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/node.cpp @@ -16,8 +16,8 @@ #include "lidar_apollo_instance_segmentation/detector.hpp" -#include -#include +#include +#include namespace lidar_apollo_instance_segmentation { @@ -28,8 +28,8 @@ LidarInstanceSegmentationNode::LidarInstanceSegmentationNode( using std::placeholders::_1; // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "lidar_apollo_instance_segmentation"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/lidar_apollo_segmentation_tvm/package.xml b/perception/lidar_apollo_segmentation_tvm/package.xml index 54b4e5d4b7836..643a331e54a5a 100755 --- a/perception/lidar_apollo_segmentation_tvm/package.xml +++ b/perception/lidar_apollo_segmentation_tvm/package.xml @@ -14,6 +14,7 @@ libpcl-all-dev autoware_perception_msgs + autoware_universe_utils geometry_msgs pcl_conversions rclcpp @@ -21,7 +22,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_perception_msgs tvm_utility tvm_vendor diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index 85f0a69356d55..e78cd57b3a4a1 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include -#include #include #include @@ -147,7 +147,7 @@ void ApolloLidarSegmentation::transformCloud( tf_buffer_->lookupTransform(target_frame_, input.header.frame_id, time_point); Eigen::Matrix4f affine_matrix = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - tier4_autoware_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( in_cluster, transformed_cloud_cluster, affine_matrix); transformed_cloud_cluster.header.frame_id = target_frame_; } else { @@ -158,7 +158,7 @@ void ApolloLidarSegmentation::transformCloud( if (z_offset != 0) { Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); - tier4_autoware_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( transformed_cloud_cluster, transformed_cloud_cluster, z_up_transform); } diff --git a/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp b/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp index a4b967de2e9bc..de2f1734e0733 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp @@ -46,7 +46,7 @@ float calcApproximateLog(float num) return log_table.data[integer_num]; } } - return std::log(1.0f + num); + return std::log1p(num); } } // namespace lidar_apollo_segmentation_tvm } // namespace perception diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index 04457b63e1a9e..fa42655b6ca1f 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -64,12 +64,207 @@ ros2 launch lidar_centerpoint lidar_centerpoint.launch.xml model_name:=centerpoi You can download the onnx format of trained models by clicking on the links below. -- Centerpoint : [pts_voxel_encoder_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint.onnx), [pts_backbone_neck_head_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint.onnx) +- Centerpoint: [pts_voxel_encoder_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint.onnx), [pts_backbone_neck_head_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint.onnx) - Centerpoint tiny: [pts_voxel_encoder_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint_tiny.onnx), [pts_backbone_neck_head_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint_tiny.onnx) `Centerpoint` was trained in `nuScenes` (~28k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs. `Centerpoint tiny` was trained in `Argoverse 2` (~110k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs. +## Training CenterPoint Model and Deploying to the Autoware + +### Overview + +This guide provides instructions on training a CenterPoint model using the **mmdetection3d** repository +and seamlessly deploying it within Autoware. + +### Installation + +#### Install prerequisites + +**Step 1.** Download and install Miniconda from the [official website](https://mmpretrain.readthedocs.io/en/latest/get_started.html). + +**Step 2.** Create a conda virtual environment and activate it + +```bash +conda create --name train-centerpoint python=3.8 -y +conda activate train-centerpoint +``` + +**Step 3.** Install PyTorch + +Please ensure you have PyTorch installed, and compatible with CUDA 11.6, as it is a requirement for current Autoware. + +```bash +conda install pytorch==1.13.1 torchvision==0.14.1 pytorch-cuda=11.6 -c pytorch -c nvidia +``` + +#### Install mmdetection3d + +**Step 1.** Install MMEngine, MMCV, and MMDetection using MIM + +```bash +pip install -U openmim +mim install mmengine +mim install 'mmcv>=2.0.0rc4' +mim install 'mmdet>=3.0.0rc5, <3.3.0' +``` + +**Step 2.** Install mmdetection3d forked repository + +Introduced several valuable enhancements in our fork of the mmdetection3d repository. +Notably, we've made the PointPillar z voxel feature input optional to maintain compatibility with the original paper. +In addition, we've integrated a PyTorch to ONNX converter and a T4 format reader for added functionality. + +```bash +git clone https://github.com/autowarefoundation/mmdetection3d.git +cd mmdetection3d +pip install -v -e . +``` + +#### Use Training Repository with Docker + +Alternatively, you can use Docker to run the mmdetection3d repository. We provide a Dockerfile to build a Docker image with the mmdetection3d repository and its dependencies. + +Clone fork of the mmdetection3d repository + +```bash +git clone https://github.com/autowarefoundation/mmdetection3d.git +``` + +Build the Docker image by running the following command: + +```bash +cd mmdetection3d +docker build -t mmdetection3d -f docker/Dockerfile . +``` + +Run the Docker container: + +```bash +docker run --gpus all --shm-size=8g -it -v {DATA_DIR}:/mmdetection3d/data mmdetection3d +``` + +### Preparing NuScenes dataset for training + +**Step 1.** Download the NuScenes dataset from the [official website](https://www.nuscenes.org/download) and extract the dataset to a folder of your choice. + +**Note:** The NuScenes dataset is large and requires significant disk space. Ensure you have enough storage available before proceeding. + +**Step 2.** Create a symbolic link to the dataset folder + +```bash +ln -s /path/to/nuscenes/dataset/ /path/to/mmdetection3d/data/nuscenes/ +``` + +**Step 3.** Prepare the NuScenes data by running: + +```bash +cd mmdetection3d +python tools/create_data.py nuscenes --root-path ./data/nuscenes --out-dir ./data/nuscenes --extra-tag nuscenes +``` + +### Training CenterPoint with NuScenes Dataset + +#### Prepare the config file + +The configuration file that illustrates how to train the CenterPoint model with the NuScenes dataset is +located at `mmdetection3d/projects/AutowareCenterPoint/configs`. This configuration file is a derived version of +[this centerpoint configuration file](https://github.com/autowarefoundation/mmdetection3d/blob/5c0613be29bd2e51771ec5e046d89ba3089887c7/configs/centerpoint/centerpoint_pillar02_second_secfpn_head-circlenms_8xb4-cyclic-20e_nus-3d.py) +from mmdetection3D. +In this custom configuration, the **use_voxel_center_z parameter** is set as **False** to deactivate the z coordinate of the voxel center, +aligning with the original paper's specifications and making the model compatible with Autoware. Additionally, the filter size is set as **[32, 32]**. + +The CenterPoint model can be tailored to your specific requirements by modifying various parameters within the configuration file. +This includes adjustments related to preprocessing operations, training, testing, model architecture, dataset, optimizer, learning rate scheduler, and more. + +#### Start training + +```bash +python tools/train.py projects/AutowareCenterPoint/configs/centerpoint_custom.py --work-dir ./work_dirs/centerpoint_custom +``` + +#### Evaluation of the trained model + +For evaluation purposes, we have included a sample dataset captured from the vehicle which consists of the following LiDAR sensors: +1 x Velodyne VLS128, 4 x Velodyne VLP16, and 1 x Robosense RS Bpearl. This dataset comprises 600 LiDAR frames and encompasses 5 distinct classes, 6905 cars, 3951 pedestrians, +75 cyclists, 162 buses, and 326 trucks 3D annotation. In the sample dataset, frames are annotated as 2 frames for each second. You can employ this dataset for a wide range of purposes, +including training, evaluation, and fine-tuning of models. It is organized in the T4 format. + +##### Download the sample dataset + +```bash +wget https://autoware-files.s3.us-west-2.amazonaws.com/dataset/lidar_detection_sample_dataset.tar.gz +#Extract the dataset to a folder of your choice +tar -xvf lidar_detection_sample_dataset.tar.gz +#Create a symbolic link to the dataset folder +ln -s /PATH/TO/DATASET/ /PATH/TO/mmdetection3d/data/tier4_dataset/ +``` + +##### Prepare dataset and evaluate trained model + +Create `.pkl` files for training, evaluation, and testing. + +The dataset was formatted according to T4Dataset specifications, with 'sample_dataset' designated as one of its versions. + +```bash +python tools/create_data.py T4Dataset --root-path data/sample_dataset/ --out-dir data/sample_dataset/ --extra-tag T4Dataset --version sample_dataset --annotation-hz 2 +``` + +Run evaluation + +```bash +python tools/test.py projects/AutowareCenterPoint/configs/centerpoint_custom_test.py /PATH/OF/THE/CHECKPOINT --task lidar_det +``` + +Evaluation results could be relatively low because of the e to variations in sensor modalities between the sample dataset +and the training dataset. The model's training parameters are originally tailored to the NuScenes dataset, which employs a single lidar +sensor positioned atop the vehicle. In contrast, the provided sample dataset comprises concatenated point clouds positioned at +the base link location of the vehicle. + +### Deploying CenterPoint model to Autoware + +#### Convert CenterPoint PyTorch model to ONNX Format + +The lidar_centerpoint implementation requires two ONNX models as input the voxel encoder and the backbone-neck-head of the CenterPoint model, other aspects of the network, +such as preprocessing operations, are implemented externally. Under the fork of the mmdetection3d repository, +we have included a script that converts the CenterPoint model to Autoware compatible ONNX format. +You can find it in `mmdetection3d/projects/AutowareCenterPoint` file. + +```bash +python projects/AutowareCenterPoint/centerpoint_onnx_converter.py --cfg projects/AutowareCenterPoint/configs/centerpoint_custom.py --ckpt work_dirs/centerpoint_custom/YOUR_BEST_MODEL.pth --work-dir ./work_dirs/onnx_models +``` + +#### Create the config file for the custom model + +Create a new config file named **centerpoint_custom.param.yaml** under the config file directory of the lidar_centerpoint node. Sets the parameters of the config file like +point_cloud_range, point_feature_size, voxel_size, etc. according to the training config file. + +```yaml +/**: + ros__parameters: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + point_feature_size: 4 + max_voxel_size: 40000 + point_cloud_range: [-51.2, -51.2, -3.0, 51.2, 51.2, 5.0] + voxel_size: [0.2, 0.2, 8.0] + downsample_factor: 1 + encoder_in_feature_size: 9 + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] +``` + +#### Launch the lidar_centerpoint node + +```bash +cd /YOUR/AUTOWARE/PATH/Autoware +source install/setup.bash +ros2 launch lidar_centerpoint lidar_centerpoint.launch.xml model_name:=centerpoint_custom model_path:=/PATH/TO/ONNX/FILE/ +``` + ### Changelog #### v1 (2022/07/06) @@ -144,3 +339,14 @@ Example: [v1-head-centerpoint]: https://awf.ml.dev.web.auto/perception/models/centerpoint/v1/pts_backbone_neck_head_centerpoint.onnx [v1-encoder-centerpoint-tiny]: https://awf.ml.dev.web.auto/perception/models/centerpoint/v1/pts_voxel_encoder_centerpoint_tiny.onnx [v1-head-centerpoint-tiny]: https://awf.ml.dev.web.auto/perception/models/centerpoint/v1/pts_backbone_neck_head_centerpoint_tiny.onnx + +## Acknowledgment: deepen.ai's 3D Annotation Tools Contribution + +Special thanks to [Deepen AI](https://www.deepen.ai/) for providing their 3D Annotation tools, which have been instrumental in creating our sample dataset. + +## Legal Notice + +_The nuScenes dataset is released publicly for non-commercial use under the Creative +Commons Attribution-NonCommercial-ShareAlike 4.0 International Public License. +Additional Terms of Use can be found at . +To inquire about a commercial license please contact ._ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp index 8cf250be0c049..55157f70fcfc3 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp @@ -15,15 +15,14 @@ #ifndef LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ #define LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ -#include -#include -#include -#include - -#include - -#include -#include +#include "lidar_centerpoint/cuda_utils.hpp" +#include "lidar_centerpoint/network/network_trt.hpp" +#include "lidar_centerpoint/postprocess/postprocess_kernel.hpp" +#include "lidar_centerpoint/preprocess/voxel_generator.hpp" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" + +#include "sensor_msgs/msg/point_cloud2.hpp" #include #include diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/cuda_utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/cuda_utils.hpp index 5ab26d75a0a41..efe3bbb9a5482 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/cuda_utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/cuda_utils.hpp @@ -42,7 +42,7 @@ #ifndef LIDAR_CENTERPOINT__CUDA_UTILS_HPP_ #define LIDAR_CENTERPOINT__CUDA_UTILS_HPP_ -#include +#include "cuda_runtime_api.h" #include #include diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp index cd1a0e58e115e..d961dd998af76 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp @@ -17,10 +17,10 @@ #include -#include -#include -#include -#include +#include "autoware_perception_msgs/msg/detected_object_kinematics.hpp" +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/shape.hpp" #include diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp index 639aa0ba8bad3..1bf77248efe08 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp @@ -15,8 +15,8 @@ #ifndef LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ #define LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ -#include -#include +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "lidar_centerpoint/network/tensorrt_wrapper.hpp" #include diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp index 3dd00c25fd9e7..ad5e222ce01e8 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp @@ -15,8 +15,8 @@ #ifndef LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_ #define LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_ -#include -#include +#include "cuda.h" +#include "cuda_runtime_api.h" namespace centerpoint { diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp index 7f55ab6f5e414..4af5aac7ff7fe 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp @@ -15,10 +15,9 @@ #ifndef LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ #define LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ -#include -#include - -#include +#include "NvInfer.h" +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "tensorrt_common/tensorrt_common.hpp" #include #include @@ -32,7 +31,7 @@ class TensorRTWrapper public: explicit TensorRTWrapper(const CenterPointConfig & config); - ~TensorRTWrapper(); + virtual ~TensorRTWrapper(); bool init( const std::string & onnx_path, const std::string & engine_path, const std::string & precision); diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index fa9c84c41b394..154ff97cdb887 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -15,14 +15,14 @@ #ifndef LIDAR_CENTERPOINT__NODE_HPP_ #define LIDAR_CENTERPOINT__NODE_HPP_ +#include "lidar_centerpoint/centerpoint_trt.hpp" +#include "lidar_centerpoint/detection_class_remapper.hpp" #include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" -#include -#include +#include +#include +#include #include -#include -#include -#include #include #include @@ -62,11 +62,11 @@ class LidarCenterPointNode : public rclcpp::Node std::unique_ptr detector_ptr_{nullptr}; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/circle_nms_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/circle_nms_kernel.hpp index cd2d71d33806d..5b55636a3eec4 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/circle_nms_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/circle_nms_kernel.hpp @@ -15,9 +15,8 @@ #ifndef LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ #define LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ -#include - -#include +#include "lidar_centerpoint/utils.hpp" +#include "thrust/device_vector.h" namespace centerpoint { diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp index d790e896dea9a..e15d3022c947c 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp @@ -15,12 +15,11 @@ #ifndef LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ #define LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ -#include -#include - -#include -#include -#include +#include "cuda.h" +#include "cuda_runtime_api.h" +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "lidar_centerpoint/utils.hpp" +#include "thrust/device_vector.h" #include diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp index 209fc74e6c58f..c1d6a6b060955 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp @@ -15,17 +15,19 @@ #ifndef LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ #define LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ -#include -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" #include #include #include +#ifdef ROS_DISTRO_GALACTIC +#include "tf2_sensor_msgs/tf2_sensor_msgs.h" +#else +#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" +#endif + +#include "lidar_centerpoint/cuda_utils.hpp" namespace centerpoint { @@ -48,7 +50,10 @@ class DensificationParam struct PointCloudWithTransform { - sensor_msgs::msg::PointCloud2 pointcloud_msg; + cuda::unique_ptr points_d{nullptr}; + std_msgs::msg::Header header; + size_t num_points{0}; + size_t point_step{0}; Eigen::Affine3f affine_past2world; }; @@ -58,7 +63,8 @@ class PointCloudDensification explicit PointCloudDensification(const DensificationParam & param); bool enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, + cudaStream_t stream); double getCurrentTimestamp() const { return current_timestamp_; } Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; } @@ -73,7 +79,8 @@ class PointCloudDensification unsigned int pointcloud_cache_size() const { return param_.pointcloud_cache_size(); } private: - void enqueue(const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine); + void enqueue( + const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine, cudaStream_t stream); void dequeue(); DensificationParam param_; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp index 9488b67475509..3abcd89cb5c55 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp @@ -15,11 +15,15 @@ #ifndef LIDAR_CENTERPOINT__PREPROCESS__PREPROCESS_KERNEL_HPP_ #define LIDAR_CENTERPOINT__PREPROCESS__PREPROCESS_KERNEL_HPP_ -#include -#include +#include "cuda.h" +#include "cuda_runtime_api.h" namespace centerpoint { +cudaError_t generateSweepPoints_launch( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform, int num_features, float * output_points, cudaStream_t stream); + cudaError_t generateVoxels_random_launch( const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp index be15d51e91715..3ade5e677cdbe 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp @@ -15,10 +15,10 @@ #ifndef LIDAR_CENTERPOINT__PREPROCESS__VOXEL_GENERATOR_HPP_ #define LIDAR_CENTERPOINT__PREPROCESS__VOXEL_GENERATOR_HPP_ -#include -#include +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "lidar_centerpoint/preprocess/pointcloud_densification.hpp" -#include +#include "sensor_msgs/msg/point_cloud2.hpp" #include #include @@ -31,10 +31,11 @@ class VoxelGeneratorTemplate explicit VoxelGeneratorTemplate( const DensificationParam & param, const CenterPointConfig & config); - virtual std::size_t generateSweepPoints(std::vector & points) = 0; + virtual std::size_t generateSweepPoints(float * d_points, cudaStream_t stream) = 0; bool enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, + cudaStream_t stream); protected: std::unique_ptr pd_ptr_{nullptr}; @@ -50,7 +51,7 @@ class VoxelGenerator : public VoxelGeneratorTemplate public: using VoxelGeneratorTemplate::VoxelGeneratorTemplate; - std::size_t generateSweepPoints(std::vector & points) override; + std::size_t generateSweepPoints(float * d_points, cudaStream_t stream) override; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp index e33d86cd0aba7..484fbcfd36657 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp @@ -17,12 +17,9 @@ // ros packages cannot be included from cuda. -#include +#include "lidar_centerpoint/utils.hpp" -#include -#include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 2804e172b73fa..6f379167b0e71 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -14,10 +14,11 @@ #include "lidar_centerpoint/centerpoint_trt.hpp" -#include -#include -#include -#include +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "lidar_centerpoint/network/scatter_kernel.hpp" +#include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" + +#include #include #include @@ -130,14 +131,11 @@ bool CenterPointTRT::detect( bool CenterPointTRT::preprocess( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer) { - bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer); + bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer, stream_); if (!is_success) { return false; } - const auto count = vg_ptr_->generateSweepPoints(points_); - CHECK_CUDA_ERROR(cudaMemcpyAsync( - points_d_.get(), points_.data(), count * config_.point_feature_size_ * sizeof(float), - cudaMemcpyHostToDevice, stream_)); + const auto count = vg_ptr_->generateSweepPoints(points_d_.get(), stream_); CHECK_CUDA_ERROR(cudaMemsetAsync(num_voxels_d_.get(), 0, sizeof(unsigned int), stream_)); CHECK_CUDA_ERROR( cudaMemsetAsync(voxels_buffer_d_.get(), 0, voxels_buffer_size_ * sizeof(float), stream_)); diff --git a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp index ecfbb4360ecf0..9805fc7a661d1 100644 --- a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp +++ b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "lidar_centerpoint/detection_class_remapper.hpp" namespace centerpoint { diff --git a/perception/lidar_centerpoint/lib/network/scatter_kernel.cu b/perception/lidar_centerpoint/lib/network/scatter_kernel.cu index 09cc83bf606fe..f139e230426ca 100644 --- a/perception/lidar_centerpoint/lib/network/scatter_kernel.cu +++ b/perception/lidar_centerpoint/lib/network/scatter_kernel.cu @@ -13,8 +13,7 @@ // limitations under the License. #include "lidar_centerpoint/network/scatter_kernel.hpp" - -#include +#include "lidar_centerpoint/utils.hpp" namespace { diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index 91fcce9a80f78..bdfde42a12a02 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -14,7 +14,7 @@ #include "lidar_centerpoint/network/tensorrt_wrapper.hpp" -#include +#include "NvOnnxParser.h" #include #include diff --git a/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu index 398e75a55c44b..c208eefe135fb 100644 --- a/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu @@ -21,12 +21,10 @@ Written by Shaoshuai Shi All Rights Reserved 2019-2020. */ +#include "lidar_centerpoint/cuda_utils.hpp" #include "lidar_centerpoint/postprocess/circle_nms_kernel.hpp" - -#include -#include - -#include +#include "lidar_centerpoint/utils.hpp" +#include "thrust/host_vector.h" namespace { diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index d4d7b1379776a..16f4fc89bbcd6 100644 --- a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -14,9 +14,10 @@ #include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" -#include "object_recognition_utils/geometry.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include +#include +#include + namespace centerpoint { @@ -48,7 +49,7 @@ bool NonMaximumSuppression::isTargetPairObject( } const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; - const auto sqr_dist_2d = tier4_autoware_utils::calcSquaredDistance2d( + const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d( object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_sqr_dist_2d; } diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 34bbd2811041c..12835bab038a6 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -13,12 +13,10 @@ // limitations under the License. #include "lidar_centerpoint/postprocess/circle_nms_kernel.hpp" - -#include - -#include -#include -#include +#include "lidar_centerpoint/postprocess/postprocess_kernel.hpp" +#include "thrust/count.h" +#include "thrust/device_vector.h" +#include "thrust/sort.h" namespace { diff --git a/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp b/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp index 66c348fda50b4..2859d58d8f669 100644 --- a/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp @@ -14,20 +14,19 @@ #include "lidar_centerpoint/preprocess/pointcloud_densification.hpp" -#include +#include "pcl_conversions/pcl_conversions.h" +#include "pcl_ros/transforms.hpp" -#include +#include "boost/optional.hpp" -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC -#include +#include "tf2_eigen/tf2_eigen.h" #else -#include +#include "tf2_eigen/tf2_eigen.hpp" #endif -#include -#include - namespace { boost::optional getTransform( @@ -61,7 +60,8 @@ PointCloudDensification::PointCloudDensification(const DensificationParam & para } bool PointCloudDensification::enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer) + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer, + cudaStream_t stream) { const auto header = pointcloud_msg.header; @@ -73,9 +73,9 @@ bool PointCloudDensification::enqueuePointCloud( } auto affine_world2current = transformToEigen(transform_world2current.get()); - enqueue(pointcloud_msg, affine_world2current); + enqueue(pointcloud_msg, affine_world2current, stream); } else { - enqueue(pointcloud_msg, Eigen::Affine3f::Identity()); + enqueue(pointcloud_msg, Eigen::Affine3f::Identity(), stream); } dequeue(); @@ -84,12 +84,24 @@ bool PointCloudDensification::enqueuePointCloud( } void PointCloudDensification::enqueue( - const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current) + const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current, + cudaStream_t stream) { affine_world2current_ = affine_world2current; current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds(); - PointCloudWithTransform pointcloud = {msg, affine_world2current.inverse()}; - pointcloud_cache_.push_front(pointcloud); + + assert(sizeof(uint8_t) * msg.width * msg.height * msg.point_step % sizeof(float) == 0); + auto points_d = cuda::make_unique( + sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(float)); + CHECK_CUDA_ERROR(cudaMemcpyAsync( + points_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step, + cudaMemcpyHostToDevice, stream)); + + PointCloudWithTransform pointcloud = { + std::move(points_d), msg.header, msg.width * msg.height, msg.point_step, + affine_world2current.inverse()}; + + pointcloud_cache_.push_front(std::move(pointcloud)); } void PointCloudDensification::dequeue() diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index 218aaee125c02..4e78176739c26 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -28,9 +28,11 @@ * limitations under the License. */ +#include "lidar_centerpoint/cuda_utils.hpp" #include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" +#include "lidar_centerpoint/utils.hpp" -#include +#include namespace { @@ -41,6 +43,51 @@ const std::size_t ENCODER_IN_FEATURE_SIZE = 9; // the same as encoder_in_featur namespace centerpoint { + +__global__ void generateSweepPoints_kernel( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform_array, int num_features, float * output_points) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= points_size) return; + + const float input_x = input_points[point_idx * input_point_step + 0]; + const float input_y = input_points[point_idx * input_point_step + 1]; + const float input_z = input_points[point_idx * input_point_step + 2]; + + // transform_array is expected to be column-major + output_points[point_idx * num_features + 0] = transform_array[0] * input_x + + transform_array[4] * input_y + + transform_array[8] * input_z + transform_array[12]; + output_points[point_idx * num_features + 1] = transform_array[1] * input_x + + transform_array[5] * input_y + + transform_array[9] * input_z + transform_array[13]; + output_points[point_idx * num_features + 2] = transform_array[2] * input_x + + transform_array[6] * input_y + + transform_array[10] * input_z + transform_array[14]; + output_points[point_idx * num_features + 3] = time_lag; +} + +cudaError_t generateSweepPoints_launch( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform_array, int num_features, float * output_points, cudaStream_t stream) +{ + auto transform_d = cuda::make_unique(16); + CHECK_CUDA_ERROR(cudaMemcpyAsync( + transform_d.get(), transform_array, 16 * sizeof(float), cudaMemcpyHostToDevice, stream)); + + dim3 blocks((points_size + 256 - 1) / 256); + dim3 threads(256); + assert(num_features == 4); + + generateSweepPoints_kernel<<>>( + input_points, points_size, input_point_step, time_lag, transform_d.get(), num_features, + output_points); + + cudaError_t err = cudaGetLastError(); + return err; +} + __global__ void generateVoxels_random_kernel( const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, diff --git a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp index 07a1a2de725f5..e90474fa07327 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -14,7 +14,12 @@ #include "lidar_centerpoint/preprocess/voxel_generator.hpp" -#include +#include "lidar_centerpoint/centerpoint_trt.hpp" +#include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" + +#include "sensor_msgs/point_cloud2_iterator.hpp" + +#include namespace centerpoint { @@ -38,35 +43,40 @@ VoxelGeneratorTemplate::VoxelGeneratorTemplate( } bool VoxelGeneratorTemplate::enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer) + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, + cudaStream_t stream) { - return pd_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer); + return pd_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer, stream); } -std::size_t VoxelGenerator::generateSweepPoints(std::vector & points) +std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t stream) { - Eigen::Vector3f point_current, point_past; - size_t point_counter{}; + size_t point_counter = 0; for (auto pc_cache_iter = pd_ptr_->getPointCloudCacheIter(); !pd_ptr_->isCacheEnd(pc_cache_iter); pc_cache_iter++) { - auto pc_msg = pc_cache_iter->pointcloud_msg; + auto sweep_num_points = pc_cache_iter->num_points; + auto point_step = pc_cache_iter->point_step; auto affine_past2current = pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world; float time_lag = static_cast( - pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_msg.header.stamp).seconds()); - - for (sensor_msgs::PointCloud2ConstIterator x_iter(pc_msg, "x"), y_iter(pc_msg, "y"), - z_iter(pc_msg, "z"); - x_iter != x_iter.end(); ++x_iter, ++y_iter, ++z_iter) { - point_past << *x_iter, *y_iter, *z_iter; - point_current = affine_past2current * point_past; + pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_cache_iter->header.stamp).seconds()); - points.at(point_counter * config_.point_feature_size_) = point_current.x(); - points.at(point_counter * config_.point_feature_size_ + 1) = point_current.y(); - points.at(point_counter * config_.point_feature_size_ + 2) = point_current.z(); - points.at(point_counter * config_.point_feature_size_ + 3) = time_lag; - ++point_counter; + if (point_counter + sweep_num_points > CAPACITY_POINT) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_centerpoint"), + "Requested number of points exceeds the maximum capacity. Current points = " + << point_counter); + break; } + + static_assert(std::is_same::value); + static_assert(!Eigen::Matrix4f::IsRowMajor, "matrices should be col-major."); + generateSweepPoints_launch( + pc_cache_iter->points_d.get(), sweep_num_points, point_step / sizeof(float), time_lag, + affine_past2current.matrix().data(), config_.point_feature_size_, + points_d + config_.point_feature_size_ * point_counter, stream); + + point_counter += sweep_num_points; } return point_counter; } diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp index 9be77b67e26dc..039d340cc2dc4 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint/lib/ros_utils.cpp @@ -14,9 +14,9 @@ #include "lidar_centerpoint/ros_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/constants.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" namespace centerpoint { @@ -50,14 +50,14 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = -box3d.yaw - tier4_autoware_utils::pi / 2; + float yaw = -box3d.yaw - autoware::universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = - tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); + autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw); + autoware::universe_utils::createQuaternionFromYaw(yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = - tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); + autoware::universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); if (has_variance) { obj.kinematics.has_position_covariance = has_variance; obj.kinematics.pose_with_covariance.covariance = convertPoseCovarianceMatrix(box3d); @@ -102,7 +102,7 @@ uint8_t getSemanticType(const std::string & class_name) std::array convertPoseCovarianceMatrix(const Box3D & box3d) { - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; std::array pose_covariance{}; pose_covariance[POSE_IDX::X_X] = box3d.x_variance; pose_covariance[POSE_IDX::Y_Y] = box3d.y_variance; @@ -113,7 +113,7 @@ std::array convertPoseCovarianceMatrix(const Box3D & box3d) std::array convertTwistCovarianceMatrix(const Box3D & box3d) { - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; std::array twist_covariance{}; twist_covariance[POSE_IDX::X_X] = box3d.vel_x_variance; twist_covariance[POSE_IDX::Y_Y] = box3d.vel_y_variance; diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 3077aa2ac2e24..280e2219fb1f1 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils object_recognition_utils pcl_ros python3-open3d @@ -22,7 +23,6 @@ tf2_eigen tf2_geometry_msgs tf2_sensor_msgs - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index b9415ba898d14..8bf62e3e0168b 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -14,17 +14,7 @@ #include "lidar_centerpoint/node.hpp" -#include -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif +#include "pcl_ros/transforms.hpp" #include #include @@ -33,6 +23,17 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#else +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#endif + +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "lidar_centerpoint/preprocess/pointcloud_densification.hpp" +#include "lidar_centerpoint/ros_utils.hpp" +#include "lidar_centerpoint/utils.hpp" + namespace centerpoint { LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_options) @@ -117,8 +118,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "lidar_centerpoint"); stop_watch_ptr_->tic("cyclic_time"); @@ -129,7 +130,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); } - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void LidarCenterPointNode::pointCloudCallback( diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp b/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp index d0c9123da9e77..8f35b98119afe 100644 --- a/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp +++ b/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp @@ -31,17 +31,17 @@ class PostprocessKernelTest : public testing::Test void SetUp() override; void TearDown() override; - cudaStream_t stream_{nullptr}; + cudaStream_t stream_{}; - std::unique_ptr postprocess_cuda_ptr_; - std::unique_ptr config_ptr_; + std::unique_ptr postprocess_cuda_ptr_{}; + std::unique_ptr config_ptr_{}; - cuda::unique_ptr head_out_heatmap_d_; - cuda::unique_ptr head_out_offset_d_; - cuda::unique_ptr head_out_z_d_; - cuda::unique_ptr head_out_dim_d_; - cuda::unique_ptr head_out_rot_d_; - cuda::unique_ptr head_out_vel_d_; + cuda::unique_ptr head_out_heatmap_d_{}; + cuda::unique_ptr head_out_offset_d_{}; + cuda::unique_ptr head_out_z_d_{}; + cuda::unique_ptr head_out_dim_d_{}; + cuda::unique_ptr head_out_rot_d_{}; + cuda::unique_ptr head_out_vel_d_{}; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp b/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp index 57ff51966a417..1a3e850026886 100644 --- a/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp +++ b/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp @@ -28,7 +28,7 @@ class PreprocessKernelTest : public testing::Test void SetUp() override; void TearDown() override; - cudaStream_t stream_{nullptr}; + cudaStream_t stream_{}; std::size_t max_voxel_size_{}; std::size_t max_point_in_voxel_size_{}; diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.cpp b/perception/lidar_centerpoint/test/test_voxel_generator.cpp index 5b0b3cd112e6c..6bded105536dc 100644 --- a/perception/lidar_centerpoint/test/test_voxel_generator.cpp +++ b/perception/lidar_centerpoint/test/test_voxel_generator.cpp @@ -14,9 +14,9 @@ #include "test_voxel_generator.hpp" -#include +#include "gtest/gtest.h" -#include +#include "sensor_msgs/point_cloud2_iterator.hpp" void VoxelGeneratorTest::SetUp() { @@ -95,6 +95,8 @@ void VoxelGeneratorTest::SetUp() transform2_ = transform1_; transform2_.header.stamp = cloud2_->header.stamp; transform2_.transform.translation.x = world_origin_x + delta_pointcloud_x_; + + cudaStreamCreate(&stream_); } void VoxelGeneratorTest::TearDown() @@ -117,8 +119,17 @@ TEST_F(VoxelGeneratorTest, SingleFrame) points.resize(capacity_ * config.point_feature_size_); std::fill(points.begin(), points.end(), std::nan("")); - bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); - std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + auto points_d = cuda::make_unique(capacity_ * config.point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_, stream_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points_d.get(), stream_); + + cudaMemcpy( + points.data(), points_d.get(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); EXPECT_TRUE(status1); EXPECT_EQ(points_per_pointcloud_, generated_points_num); @@ -155,9 +166,18 @@ TEST_F(VoxelGeneratorTest, TwoFramesNoTf) points.resize(capacity_ * config.point_feature_size_); std::fill(points.begin(), points.end(), std::nan("")); - bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); - bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); - std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + auto points_d = cuda::make_unique(capacity_ * config.point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_, stream_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_, stream_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points_d.get(), stream_); + + cudaMemcpy( + points.data(), points_d.get(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); EXPECT_FALSE(status1); EXPECT_FALSE(status2); @@ -180,12 +200,21 @@ TEST_F(VoxelGeneratorTest, TwoFrames) points.resize(capacity_ * config.point_feature_size_); std::fill(points.begin(), points.end(), std::nan("")); + auto points_d = cuda::make_unique(capacity_ * config.point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + tf2_buffer_->setTransform(transform1_, "authority1"); tf2_buffer_->setTransform(transform2_, "authority1"); - bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); - bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); - std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_, stream_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_, stream_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points_d.get(), stream_); + + cudaMemcpy( + points.data(), points_d.get(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); EXPECT_TRUE(status1); EXPECT_TRUE(status2); diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.hpp b/perception/lidar_centerpoint/test/test_voxel_generator.hpp index 8fb7d8dffa44d..48355b8a331ba 100644 --- a/perception/lidar_centerpoint/test/test_voxel_generator.hpp +++ b/perception/lidar_centerpoint/test/test_voxel_generator.hpp @@ -33,34 +33,36 @@ class VoxelGeneratorTest : public testing::Test void TearDown() override; // These need to be public so that they can be accessed in the test cases - rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr node_{}; - std::unique_ptr cloud1_, cloud2_; - geometry_msgs::msg::TransformStamped transform1_, transform2_; + std::unique_ptr cloud1_{}, cloud2_{}; + geometry_msgs::msg::TransformStamped transform1_{}, transform2_{}; - std::unique_ptr densification_param_ptr_; - std::unique_ptr config_ptr_; + std::unique_ptr densification_param_ptr_{}; + std::unique_ptr config_ptr_{}; - std::unique_ptr tf2_buffer_; + std::unique_ptr tf2_buffer_{}; - std::string world_frame_; - std::string lidar_frame_; - std::size_t points_per_pointcloud_; - std::size_t capacity_; - double delta_pointcloud_x_; + std::string world_frame_{}; + std::string lidar_frame_{}; + std::size_t points_per_pointcloud_{}; + std::size_t capacity_{}; + double delta_pointcloud_x_{}; - std::size_t class_size_; - float point_feature_size_; - std::size_t max_voxel_size_; + std::size_t class_size_{}; + float point_feature_size_{}; + std::size_t max_voxel_size_{}; - std::vector point_cloud_range_; - std::vector voxel_size_; - std::size_t downsample_factor_; - std::size_t encoder_in_feature_size_; - float score_threshold_; - float circle_nms_dist_threshold_; - std::vector yaw_norm_thresholds_; - bool has_variance_; + std::vector point_cloud_range_{}; + std::vector voxel_size_{}; + std::size_t downsample_factor_{}; + std::size_t encoder_in_feature_size_{}; + float score_threshold_{}; + float circle_nms_dist_threshold_{}; + std::vector yaw_norm_thresholds_{}; + bool has_variance_{}; + + cudaStream_t stream_{}; }; #endif // TEST_VOXEL_GENERATOR_HPP_ diff --git a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp index 7a7d3ff3f1f15..88a43139908a7 100644 --- a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp +++ b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/node.hpp @@ -15,11 +15,11 @@ #ifndef LIDAR_CENTERPOINT_TVM__NODE_HPP_ #define LIDAR_CENTERPOINT_TVM__NODE_HPP_ +#include +#include #include #include #include -#include -#include #include #include @@ -59,9 +59,9 @@ class LIDAR_CENTERPOINT_TVM_PUBLIC LidarCenterPointTVMNode : public rclcpp::Node std::unique_ptr detector_ptr_{nullptr}; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; }; } // namespace lidar_centerpoint_tvm diff --git a/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp b/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp index 2e0f9ad28bfb6..91f727448b76a 100644 --- a/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp +++ b/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp @@ -14,12 +14,12 @@ #include "lidar_centerpoint_tvm/centerpoint_tvm.hpp" +#include #include #include #include #include #include -#include #include #include diff --git a/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp b/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp index cde4b0fe4112d..94639d71e66c0 100644 --- a/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint_tvm/lib/ros_utils.cpp @@ -14,8 +14,8 @@ #include "lidar_centerpoint_tvm/ros_utils.hpp" -#include -#include +#include +#include namespace autoware { @@ -65,14 +65,14 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = -box3d.yaw - tier4_autoware_utils::pi / 2; + float yaw = -box3d.yaw - autoware::universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = - tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); + autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw); + autoware::universe_utils::createQuaternionFromYaw(yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = - tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); + autoware::universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); // twist if (has_twist) { diff --git a/perception/lidar_centerpoint_tvm/package.xml b/perception/lidar_centerpoint_tvm/package.xml index eac9bc3d2bf45..c71a27e4a1677 100644 --- a/perception/lidar_centerpoint_tvm/package.xml +++ b/perception/lidar_centerpoint_tvm/package.xml @@ -12,13 +12,13 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils pcl_ros rclcpp rclcpp_components tf2_eigen tf2_geometry_msgs tf2_sensor_msgs - tier4_autoware_utils tvm_utility tvm_vendor diff --git a/perception/lidar_centerpoint_tvm/src/node.cpp b/perception/lidar_centerpoint_tvm/src/node.cpp index 288b8d59aec21..9cf82b1c6bcae 100644 --- a/perception/lidar_centerpoint_tvm/src/node.cpp +++ b/perception/lidar_centerpoint_tvm/src/node.cpp @@ -94,8 +94,8 @@ LidarCenterPointTVMNode::LidarCenterPointTVMNode(const rclcpp::NodeOptions & nod // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "lidar_centerpoint_tvm"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp index 86303618d7877..a82013fac6ce7 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp @@ -21,10 +21,10 @@ #include "lidar_transfusion/transfusion_trt.hpp" #include "lidar_transfusion/visibility_control.hpp" +#include +#include +#include #include -#include -#include -#include #include #include @@ -63,10 +63,10 @@ class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node std::unique_ptr detector_ptr_{nullptr}; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; - std::unique_ptr published_time_pub_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr published_time_pub_{nullptr}; }; } // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp index a94c3c1871136..0a9ea413dc30d 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp @@ -24,7 +24,7 @@ #include "lidar_transfusion/utils.hpp" #include "lidar_transfusion/visibility_control.hpp" -#include +#include #include @@ -83,7 +83,7 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT std::unique_ptr network_trt_ptr_{nullptr}; std::unique_ptr vg_ptr_{nullptr}; - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; std::unique_ptr pre_ptr_{nullptr}; std::unique_ptr post_ptr_{nullptr}; diff --git a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp index c1179a3f6673d..dd12c52970d38 100644 --- a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -14,9 +14,9 @@ #include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include #include #include -#include namespace lidar_transfusion { @@ -49,7 +49,7 @@ bool NonMaximumSuppression::isTargetPairObject( } const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; - const auto sqr_dist_2d = tier4_autoware_utils::calcSquaredDistance2d( + const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d( object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_sqr_dist_2d; } diff --git a/perception/lidar_transfusion/lib/ros_utils.cpp b/perception/lidar_transfusion/lib/ros_utils.cpp index 0ecb34faf732d..ef5c45c339b64 100644 --- a/perception/lidar_transfusion/lib/ros_utils.cpp +++ b/perception/lidar_transfusion/lib/ros_utils.cpp @@ -14,9 +14,9 @@ #include "lidar_transfusion/ros_utils.hpp" +#include +#include #include -#include -#include namespace lidar_transfusion { @@ -49,14 +49,14 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = box3d.yaw + tier4_autoware_utils::pi / 2; + float yaw = box3d.yaw + autoware::universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = - tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); + autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw); + autoware::universe_utils::createQuaternionFromYaw(yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = - tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); + autoware::universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); } uint8_t getSemanticType(const std::string & class_name) diff --git a/perception/lidar_transfusion/lib/transfusion_trt.cpp b/perception/lidar_transfusion/lib/transfusion_trt.cpp index c876c0906162b..d940b83c12cb4 100644 --- a/perception/lidar_transfusion/lib/transfusion_trt.cpp +++ b/perception/lidar_transfusion/lib/transfusion_trt.cpp @@ -17,7 +17,7 @@ #include "lidar_transfusion/preprocess/preprocess_kernel.hpp" #include "lidar_transfusion/transfusion_config.hpp" -#include +#include #include #include @@ -37,7 +37,8 @@ TransfusionTRT::TransfusionTRT( network_trt_ptr_->init( network_param.onnx_path(), network_param.engine_path(), network_param.trt_precision()); vg_ptr_ = std::make_unique(densification_param, config_, stream_); - stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_ = + std::make_unique>(); stop_watch_ptr_->tic("processing/inner"); initPtr(); diff --git a/perception/lidar_transfusion/package.xml b/perception/lidar_transfusion/package.xml index 29643d0957eb1..ffbe091b998e5 100644 --- a/perception/lidar_transfusion/package.xml +++ b/perception/lidar_transfusion/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils launch_ros object_recognition_utils pcl_ros @@ -22,7 +23,6 @@ tf2_eigen tf2_geometry_msgs tf2_sensor_msgs - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp index 9313015002973..e3ea6b3780de8 100644 --- a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp @@ -94,12 +94,12 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) objects_pub_ = this->create_publisher( "~/output/objects", rclcpp::QoS(1)); - published_time_pub_ = std::make_unique(this); + published_time_pub_ = std::make_unique(this); // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic"); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index c685024d6ef8b..7c1b814258bcf 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -17,8 +17,8 @@ #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include #include -#include #include "unique_identifier_msgs/msg/uuid.hpp" #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 9ab5576faccd8..3019f16ada7c9 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -17,11 +17,11 @@ #include "multi_object_tracker/debugger/debug_object.hpp" +#include +#include #include #include #include -#include -#include #include #include @@ -56,7 +56,7 @@ class TrackerDebugger rclcpp::Node & node_; rclcpp::Publisher::SharedPtr debug_tentative_objects_pub_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr processing_time_publisher_; rclcpp::Publisher::SharedPtr debug_objects_markers_pub_; diagnostic_updater::Updater diagnostic_updater_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 569bc4f43a3d4..4c1ccced1bcf7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -73,7 +73,7 @@ class MultiObjectTracker : public rclcpp::Node // debugger std::unique_ptr debugger_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // publish timer rclcpp::TimerBase::SharedPtr publish_timer_; diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index 4e2696700bfdc..67f56273e79c5 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -14,6 +14,7 @@ eigen3_cmake_module autoware_perception_msgs + autoware_universe_utils diagnostic_updater eigen glog @@ -24,7 +25,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils unique_identifier_msgs ament_lint_auto diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 0ff3188e069f2..013137aa8b82c 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -52,8 +52,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & tracker_quat, const bool distinguish_front_or_back = true) { const double measurement_yaw = - tier4_autoware_utils::normalizeRadian(tf2::getYaw(measurement_quat)); - const double tracker_yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(tracker_quat)); + autoware::universe_utils::normalizeRadian(tf2::getYaw(measurement_quat)); + const double tracker_yaw = autoware::universe_utils::normalizeRadian(tf2::getYaw(tracker_quat)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed measurement_yaw to be in the range of +-90 or 180 degrees of X_t(IDX::YAW) @@ -170,7 +170,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double dist = tier4_autoware_utils::calcDistance2d( + const double dist = autoware::universe_utils::calcDistance2d( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position); @@ -183,7 +183,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = tier4_autoware_utils::getArea(measurement_object.shape); + const double area = autoware::universe_utils::getArea(measurement_object.shape); if (area < min_area || max_area < area) passed_gate = false; } // angle gate diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index e6a0b6242a168..fe6d3d5c75564 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -236,8 +236,9 @@ void TrackerObjectDebugger::draw( stream << std::fixed << std::setprecision(0) << object_data_front.existence_vector[i] * 100; existence_probability_text += channel_names_[i] + stream.str() + ":"; } - existence_probability_text = - existence_probability_text.substr(0, existence_probability_text.size() - 1); + if (!existence_probability_text.empty()) { + existence_probability_text.pop_back(); + } existence_probability_text += "\n" + object_data_front.uuid_str.substr(0, 6); text_marker.text = existence_probability_text; diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index 7b4f9d717a927..a3ece17eab112 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -24,7 +24,7 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_ // initialize debug publishers if (debug_settings_.publish_processing_time) { processing_time_publisher_ = - std::make_unique(&node_, "multi_object_tracker"); + std::make_unique(&node_, "multi_object_tracker"); } if (debug_settings_.publish_tentative_objects) { diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 70433f16125c9..54255e706e35a 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -199,7 +199,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // Debugger debugger_ = std::make_unique(*this, world_frame_id_); debugger_->setObjectChannels(input_names_short); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void MultiObjectTracker::onTrigger() diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index b54c9b832b0cf..05e06107c2247 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -20,9 +20,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -57,9 +57,9 @@ BicycleTracker::BicycleTracker( // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - double r_stddev_x = 0.5; // in vehicle coordinate [m] - double r_stddev_y = 0.4; // in vehicle coordinate [m] - double r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // in map coordinate [rad] + double r_stddev_x = 0.5; // in vehicle coordinate [m] + double r_stddev_y = 0.4; // in vehicle coordinate [m] + double r_stddev_yaw = autoware::universe_utils::deg2rad(30); // in map coordinate [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -102,8 +102,8 @@ BicycleTracker::BicycleTracker( // Set motion limits { - constexpr double max_vel = tier4_autoware_utils::kmph2mps(80); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + constexpr double max_vel = autoware::universe_utils::kmph2mps(80); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -126,7 +126,7 @@ BicycleTracker::BicycleTracker( constexpr double p0_stddev_x = 0.8; // in object coordinate [m] constexpr double p0_stddev_y = 0.5; // in object coordinate [m] constexpr double p0_stddev_yaw = - tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + autoware::universe_utils::deg2rad(25); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -145,14 +145,15 @@ BicycleTracker::BicycleTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] vel_cov = std::pow(p0_stddev_vel, 2.0); } else { vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double p0_stddev_slip = + autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model @@ -327,7 +328,7 @@ bool BicycleTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index ebf5a977ed44a..4414d8a21ca01 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -20,9 +20,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -58,10 +58,10 @@ BigVehicleTracker::BigVehicleTracker( // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - float r_stddev_x = 0.5; // in vehicle coordinate [m] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] - float r_stddev_vel = 1.0; // in object coordinate [m/s] + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = autoware::universe_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -70,7 +70,7 @@ BigVehicleTracker::BigVehicleTracker( // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + velocity_deviation_threshold_ = autoware::universe_utils::kmph2mps(10); // [m/s] // OBJECT SHAPE MODEL if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -120,8 +120,8 @@ BigVehicleTracker::BigVehicleTracker( // Set motion limits { - constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -144,7 +144,7 @@ BigVehicleTracker::BigVehicleTracker( constexpr double p0_stddev_x = 1.5; // in object coordinate [m] constexpr double p0_stddev_y = 0.5; // in object coordinate [m] constexpr double p0_stddev_yaw = - tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + autoware::universe_utils::deg2rad(25); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -163,14 +163,15 @@ BigVehicleTracker::BigVehicleTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] vel_cov = std::pow(p0_stddev_vel, 2.0); } else { vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double p0_stddev_slip = + autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model @@ -422,7 +423,7 @@ bool BigVehicleTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 3b6f644614d9b..1fff1a13add65 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -20,9 +20,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -58,10 +58,10 @@ NormalVehicleTracker::NormalVehicleTracker( // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - float r_stddev_x = 0.5; // in vehicle coordinate [m] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] - float r_stddev_vel = 1.0; // in object coordinate [m/s] + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = autoware::universe_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -70,7 +70,7 @@ NormalVehicleTracker::NormalVehicleTracker( // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + velocity_deviation_threshold_ = autoware::universe_utils::kmph2mps(10); // [m/s] // OBJECT SHAPE MODEL if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -121,8 +121,8 @@ NormalVehicleTracker::NormalVehicleTracker( // Set motion limits { - constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } @@ -145,7 +145,7 @@ NormalVehicleTracker::NormalVehicleTracker( constexpr double p0_stddev_x = 1.0; // in object coordinate [m] constexpr double p0_stddev_y = 0.3; // in object coordinate [m] constexpr double p0_stddev_yaw = - tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + autoware::universe_utils::deg2rad(25); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -164,14 +164,15 @@ NormalVehicleTracker::NormalVehicleTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] vel_cov = std::pow(p0_stddev_vel, 2.0); } else { vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double p0_stddev_slip = + autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] const double slip_cov = std::pow(p0_stddev_slip, 2.0); // initialize motion model @@ -424,7 +425,7 @@ bool NormalVehicleTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 6c8198b51ecd0..887810066f038 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -20,9 +20,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -57,9 +57,9 @@ PedestrianTracker::PedestrianTracker( // Initialize parameters // measurement noise covariance - float r_stddev_x = 0.4; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + float r_stddev_x = 0.4; // [m] + float r_stddev_y = 0.4; // [m] + float r_stddev_yaw = autoware::universe_utils::deg2rad(30); // [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); @@ -86,18 +86,18 @@ PedestrianTracker::PedestrianTracker( // Set motion model parameters { - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_yaw = autoware::universe_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_wz = autoware::universe_utils::deg2rad(30); // [rad/(s*s)] motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); } // Set motion limits motion_model_.setMotionLimits( - tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ - 30.0 /* [deg/s] maximum turn rate */ + autoware::universe_utils::kmph2mps(100), /* [m/s] maximum velocity */ + 30.0 /* [deg/s] maximum turn rate */ ); // Set initial state @@ -121,7 +121,7 @@ PedestrianTracker::PedestrianTracker( constexpr double p0_stddev_x = 2.0; // in object coordinate [m] constexpr double p0_stddev_y = 2.0; // in object coordinate [m] constexpr double p0_stddev_yaw = - tier4_autoware_utils::deg2rad(1000); // in map coordinate [rad] + autoware::universe_utils::deg2rad(1000); // in map coordinate [rad] constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; @@ -140,9 +140,9 @@ PedestrianTracker::PedestrianTracker( if (!object.kinematics.has_twist_covariance) { constexpr double p0_stddev_vel = - tier4_autoware_utils::kmph2mps(120); // in object coordinate [m/s] + autoware::universe_utils::kmph2mps(120); // in object coordinate [m/s] constexpr double p0_stddev_wz = - tier4_autoware_utils::deg2rad(360); // in object coordinate [rad/s] + autoware::universe_utils::deg2rad(360); // in object coordinate [rad/s] vel_cov = std::pow(p0_stddev_vel, 2.0); wz_cov = std::pow(p0_stddev_wz, 2.0); } else { @@ -323,7 +323,7 @@ bool PedestrianTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index f15d5042a2316..8e484623ef0aa 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -16,9 +16,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -67,8 +67,8 @@ UnknownTracker::UnknownTracker( // Set motion limits motion_model_.setMotionLimits( - tier4_autoware_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ - tier4_autoware_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ + autoware::universe_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ + autoware::universe_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ ); // Set initial state @@ -107,8 +107,8 @@ UnknownTracker::UnknownTracker( } if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/s] - constexpr double p0_stddev_vy = tier4_autoware_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vx = autoware::universe_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vy = autoware::universe_utils::kmph2mps(10); // [m/s] const double p0_cov_vx = std::pow(p0_stddev_vx, 2.0); const double p0_cov_vy = std::pow(p0_stddev_vy, 2.0); twist_cov[utils::MSG_COV_IDX::X_X] = p0_cov_vx; diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index ef8a45f608098..f21855c2356ef 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -21,8 +21,8 @@ #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include -#include +#include +#include #define EIGEN_MPL2_ONLY #include @@ -60,8 +60,8 @@ void BicycleMotionModel::setDefaultParams() lr_min); // set motion limitations - constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30.0; // [deg] maximum slip angle + constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30.0; // [deg] maximum slip angle setMotionLimits(max_vel, max_slip); // set prediction parameters @@ -79,13 +79,13 @@ void BicycleMotionModel::setMotionParams( // set process noise covariance parameters motion_params_.q_stddev_acc_long = q_stddev_acc_long; motion_params_.q_stddev_acc_lat = q_stddev_acc_lat; - motion_params_.q_stddev_yaw_rate_min = tier4_autoware_utils::deg2rad(q_stddev_yaw_rate_min); - motion_params_.q_stddev_yaw_rate_max = tier4_autoware_utils::deg2rad(q_stddev_yaw_rate_max); + motion_params_.q_stddev_yaw_rate_min = autoware::universe_utils::deg2rad(q_stddev_yaw_rate_min); + motion_params_.q_stddev_yaw_rate_max = autoware::universe_utils::deg2rad(q_stddev_yaw_rate_max); motion_params_.q_cov_slip_rate_min = - std::pow(tier4_autoware_utils::deg2rad(q_stddev_slip_rate_min), 2.0); + std::pow(autoware::universe_utils::deg2rad(q_stddev_slip_rate_min), 2.0); motion_params_.q_cov_slip_rate_max = - std::pow(tier4_autoware_utils::deg2rad(q_stddev_slip_rate_max), 2.0); - motion_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(q_max_slip_angle); + std::pow(autoware::universe_utils::deg2rad(q_stddev_slip_rate_max), 2.0); + motion_params_.q_max_slip_angle = autoware::universe_utils::deg2rad(q_max_slip_angle); constexpr double minimum_wheel_pos = 0.01; // minimum of 0.01m if (lf_min < minimum_wheel_pos || lr_min < minimum_wheel_pos) { @@ -103,7 +103,7 @@ void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & { // set motion limitations motion_params_.max_vel = max_vel; - motion_params_.max_slip = tier4_autoware_utils::deg2rad(max_slip); + motion_params_.max_slip = autoware::universe_utils::deg2rad(max_slip); } bool BicycleMotionModel::initialize( @@ -252,7 +252,7 @@ bool BicycleMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index 9e8327e381057..79ce6e5dbb61c 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -21,8 +21,8 @@ #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include -#include +#include +#include #define EIGEN_MPL2_ONLY #include @@ -40,17 +40,17 @@ CTRVMotionModel::CTRVMotionModel() : MotionModel(), logger_(rclcpp::get_logger(" void CTRVMotionModel::setDefaultParams() { // process noise covariance - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_yaw = autoware::universe_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_wz = autoware::universe_utils::deg2rad(30); // [rad/(s*s)] setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); // set motion limitations - constexpr double max_vel = tier4_autoware_utils::kmph2mps(10); // [m/s] maximum velocity - constexpr double max_wz = 30.0; // [deg] maximum yaw rate + constexpr double max_vel = autoware::universe_utils::kmph2mps(10); // [m/s] maximum velocity + constexpr double max_wz = 30.0; // [deg] maximum yaw rate setMotionLimits(max_vel, max_wz); // set prediction parameters @@ -74,7 +74,7 @@ void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max { // set motion limitations motion_params_.max_vel = max_vel; - motion_params_.max_wz = tier4_autoware_utils::deg2rad(max_wz); + motion_params_.max_wz = autoware::universe_utils::deg2rad(max_wz); } bool CTRVMotionModel::initialize( @@ -220,7 +220,7 @@ bool CTRVMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index a9ad03e7875c5..8b3e9014f52d7 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -21,8 +21,8 @@ #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include -#include +#include +#include #include @@ -49,8 +49,8 @@ void CVMotionModel::setDefaultParams() setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); // set motion limitations - constexpr double max_vx = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum x velocity - constexpr double max_vy = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum y velocity + constexpr double max_vx = autoware::universe_utils::kmph2mps(60); // [m/s] maximum x velocity + constexpr double max_vy = autoware::universe_utils::kmph2mps(60); // [m/s] maximum y velocity setMotionLimits(max_vx, max_vy); // set prediction parameters diff --git a/perception/object_merger/include/object_merger/node.hpp b/perception/object_merger/include/object_merger/node.hpp index 7fb9bae7d9986..2f5918a9c7f17 100644 --- a/perception/object_merger/include/object_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -17,10 +17,10 @@ #include "object_merger/data_association/data_association.hpp" +#include +#include +#include #include -#include -#include -#include #include @@ -84,10 +84,10 @@ class ObjectAssociationMergerNode : public rclcpp::Node } overlapped_judge_param_; // debug publisher - std::unique_ptr processing_time_publisher_; - std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace object_association diff --git a/perception/object_merger/package.xml b/perception/object_merger/package.xml index 82a6399c09ba7..0340a477936b8 100644 --- a/perception/object_merger/package.xml +++ b/perception/object_merger/package.xml @@ -14,6 +14,7 @@ eigen3_cmake_module autoware_perception_msgs + autoware_universe_utils eigen mussp object_recognition_utils @@ -21,7 +22,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp index 3abecd80178c0..c32fe480ff4af 100644 --- a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp +++ b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp @@ -14,10 +14,10 @@ #include "object_merger/data_association/data_association.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_merger/data_association/solver/gnn_solver.hpp" #include "object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -31,8 +31,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, const bool distinguish_front_or_back = true) { - const double yaw0 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat0)); - const double yaw1 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat1)); + const double yaw0 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat0)); + const double yaw1 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat1)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 @@ -133,7 +133,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { const double max_dist = max_dist_matrix_(object1_label, object0_label); - const double dist = tier4_autoware_utils::calcDistance2d( + const double dist = autoware::universe_utils::calcDistance2d( object0.kinematics.pose_with_covariance.pose.position, object1.kinematics.pose_with_covariance.pose.position); diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index a4a74f5c763fb..5077268731955 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -14,9 +14,9 @@ #include "object_merger/node.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -43,7 +43,7 @@ bool isUnknownObjectOverlapped( const double distance_threshold = distance_threshold_map.at( object_recognition_utils::getHighestProbLabel(known_object.classification)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); - const double sq_distance = tier4_autoware_utils::calcSquaredDistance2d( + const double sq_distance = autoware::universe_utils::calcSquaredDistance2d( unknown_object.kinematics.pose_with_covariance.pose, known_object.kinematics.pose_with_covariance.pose); if (sq_distance_threshold < sq_distance) return false; @@ -121,11 +121,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio // Debug publisher processing_time_publisher_ = - std::make_unique(this, "object_association_merger"); - stop_watch_ptr_ = std::make_unique>(); + std::make_unique(this, "object_association_merger"); + stop_watch_ptr_ = + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void ObjectAssociationMergerNode::objectsCallback( diff --git a/perception/object_velocity_splitter/package.xml b/perception/object_velocity_splitter/package.xml index a0ffe5e802801..32fd5b2ff6337 100644 --- a/perception/object_velocity_splitter/package.xml +++ b/perception/object_velocity_splitter/package.xml @@ -13,10 +13,10 @@ ament_cmake_auto autoware_perception_msgs + autoware_universe_utils geometry_msgs rclcpp rclcpp_components - tier4_autoware_utils autoware_cmake ament_lint_auto diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp index 2e496d7ee2ed6..e15871d18840f 100644 --- a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp +++ b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp @@ -14,7 +14,7 @@ #include "object_velocity_splitter/object_velocity_splitter_node.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include @@ -74,7 +74,7 @@ void ObjectVelocitySplitterNode::onObjects(const DetectedObjects::ConstSharedPtr for (const auto & object : objects_data_->objects) { if ( - std::abs(tier4_autoware_utils::calcNorm( + std::abs(autoware::universe_utils::calcNorm( object.kinematics.twist_with_covariance.twist.linear)) < node_param_.velocity_threshold) { low_speed_objects.objects.emplace_back(object); } else { diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 0faa40255a1ca..ae6d7a34fca2a 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -17,9 +17,9 @@ #include "pointcloud_preprocessor/filter.hpp" +#include #include #include -#include #include #include @@ -120,9 +120,9 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node // Debugger std::shared_ptr debugger_ptr_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; - std::unique_ptr published_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; // ROS Parameters std::string map_frame_; diff --git a/perception/occupancy_grid_map_outlier_filter/package.xml b/perception/occupancy_grid_map_outlier_filter/package.xml index dafd73f38faef..0e920549eb728 100644 --- a/perception/occupancy_grid_map_outlier_filter/package.xml +++ b/perception/occupancy_grid_map_outlier_filter/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils autoware_vehicle_msgs diagnostic_updater image_transport @@ -37,7 +38,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_pcl_extensions diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index bbd138030d76f..f62a3f7e689fc 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -14,10 +14,10 @@ #include "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" +#include +#include +#include #include -#include -#include -#include #include @@ -73,7 +73,7 @@ geometry_msgs::msg::PoseStamped getPoseStamped( RCLCPP_WARN_THROTTLE( rclcpp::get_logger("occupancy_grid_map_outlier_filter"), clock, 5000, "%s", ex.what()); } - return tier4_autoware_utils::transform2pose(tf_stamped); + return autoware::universe_utils::transform2pose(tf_stamped); } boost::optional getCost( @@ -225,8 +225,8 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( { // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "occupancy_grid_map_outlier_filter"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp index 19311559a3e7d..84ca13c8b1881 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp @@ -20,10 +20,10 @@ #include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" #include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include +#include #include #include -#include -#include #include #include @@ -85,8 +85,8 @@ class GridMapFusionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr fused_map_pub_; rclcpp::Publisher::SharedPtr single_frame_pub_; std::vector::SharedPtr> grid_map_subs_; - std::unique_ptr> stop_watch_ptr_{}; - std::unique_ptr debug_publisher_ptr_{}; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; // Topics manager std::size_t num_input_topics_{1}; diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 93486e0ca56af..1d119102dc28d 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -19,11 +19,11 @@ #include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" #include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include +#include #include #include #include -#include -#include #include #include @@ -68,8 +68,8 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node rclcpp::Publisher::SharedPtr occupancy_grid_map_pub_; message_filters::Subscriber obstacle_pointcloud_sub_; message_filters::Subscriber raw_pointcloud_sub_; - std::unique_ptr> stop_watch_ptr_{}; - std::unique_ptr debug_publisher_ptr_{}; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; std::shared_ptr tf2_{std::make_shared(get_clock())}; std::shared_ptr tf2_listener_{std::make_shared(*tf2_)}; diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index c58e747f84af2..0e84e4860fdf3 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -101,15 +101,19 @@ def launch_setup(context, *args, **kwargs): remappings=[ ( "~/input/obstacle_pointcloud", - LaunchConfiguration("input/obstacle_pointcloud") - if not downsample_input_pointcloud - else "obstacle/downsample/pointcloud", + ( + LaunchConfiguration("input/obstacle_pointcloud") + if not downsample_input_pointcloud + else "obstacle/downsample/pointcloud" + ), ), ( "~/input/raw_pointcloud", - LaunchConfiguration("input/raw_pointcloud") - if not downsample_input_pointcloud - else "raw/downsample/pointcloud", + ( + LaunchConfiguration("input/raw_pointcloud") + if not downsample_input_pointcloud + else "raw/downsample/pointcloud" + ), ), ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index 3efed07535504..d8ae702ef60ca 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils eigen3_cmake_module grid_map_costmap_2d grid_map_msgs @@ -31,7 +32,6 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - tier4_autoware_utils visualization_msgs pointcloud_preprocessor diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index f191a23eb8e65..fa93db802b0ce 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -142,8 +142,8 @@ GridMapFusionNode::GridMapFusionNode(const rclcpp::NodeOptions & node_options) // debug tools { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "synchronized_grid_map_fusion"); stop_watch_ptr_->tic("cyclic_time"); diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 2722f2ef98249..d4e177209f99d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -17,9 +17,9 @@ #include "probabilistic_occupancy_grid_map/cost_value.hpp" #include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include #include #include -#include #include @@ -55,9 +55,9 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { - constexpr double min_angle = tier4_autoware_utils::deg2rad(-180.0); - constexpr double max_angle = tier4_autoware_utils::deg2rad(180.0); - constexpr double angle_increment = tier4_autoware_utils::deg2rad(0.1); + constexpr double min_angle = autoware::universe_utils::deg2rad(-180.0); + constexpr double max_angle = autoware::universe_utils::deg2rad(180.0); + constexpr double angle_increment = autoware::universe_utils::deg2rad(0.1); const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform from base_link to map frame diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index eae66eb90355b..79fc6e8c40b5e 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -17,10 +17,10 @@ #include "probabilistic_occupancy_grid_map/cost_value.hpp" #include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include #include #include #include -#include #include @@ -56,9 +56,9 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { - constexpr double min_angle = tier4_autoware_utils::deg2rad(-180.0); - constexpr double max_angle = tier4_autoware_utils::deg2rad(180.0); - constexpr double angle_increment = tier4_autoware_utils::deg2rad(0.1); + constexpr double min_angle = autoware::universe_utils::deg2rad(-180.0); + constexpr double max_angle = autoware::universe_utils::deg2rad(180.0); + constexpr double angle_increment = autoware::universe_utils::deg2rad(0.1); const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); // Transform from base_link to map frame diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 96afb0f2e852c..d019280aefda0 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -19,9 +19,9 @@ #include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" #include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include +#include #include -#include -#include #include @@ -119,8 +119,8 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "pointcloud_based_occupancy_grid_map"); diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 8009a503167ea..209207fe10f34 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -14,7 +14,7 @@ #include "probabilistic_occupancy_grid_map/utils/utils.hpp" -#include +#include #include @@ -50,7 +50,7 @@ void transformPointcloud( const sensor_msgs::msg::PointCloud2 & input, const geometry_msgs::msg::Pose & pose, sensor_msgs::msg::PointCloud2 & output) { - const auto transform = tier4_autoware_utils::pose2transform(pose); + const auto transform = autoware::universe_utils::pose2transform(pose); Eigen::Matrix4f tf_matrix = tf2::transformToEigen(transform).matrix().cast(); pcl_ros::transformPointCloud(tf_matrix, input, output); @@ -96,7 +96,7 @@ geometry_msgs::msg::Pose getPose( geometry_msgs::msg::TransformStamped tf_stamped; tf_stamped = tf2.lookupTransform( target_frame, source_header.frame_id, source_header.stamp, rclcpp::Duration::from_seconds(0.5)); - pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); + pose = autoware::universe_utils::transform2pose(tf_stamped.transform); return pose; } @@ -108,7 +108,7 @@ geometry_msgs::msg::Pose getPose( geometry_msgs::msg::TransformStamped tf_stamped; tf_stamped = tf2.lookupTransform(target_frame, source_frame, stamp, rclcpp::Duration::from_seconds(0.5)); - pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); + pose = autoware::universe_utils::transform2pose(tf_stamped.transform); return pose; } diff --git a/perception/radar_crossing_objects_noise_filter/package.xml b/perception/radar_crossing_objects_noise_filter/package.xml index 2cfe2528a8bbd..0db40b24e809c 100644 --- a/perception/radar_crossing_objects_noise_filter/package.xml +++ b/perception/radar_crossing_objects_noise_filter/package.xml @@ -15,12 +15,12 @@ ament_cmake_auto autoware_perception_msgs + autoware_universe_utils geometry_msgs rclcpp rclcpp_components tf2 tf2_geometry_msgs - tier4_autoware_utils autoware_cmake ament_lint_auto diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp index a3f4d2a6ce9d0..2f5884c04ce37 100644 --- a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp +++ b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp @@ -14,8 +14,8 @@ #include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include @@ -124,14 +124,14 @@ rcl_interfaces::msg::SetParametersResult RadarCrossingObjectsNoiseFilterNode::on bool RadarCrossingObjectsNoiseFilterNode::isNoise(const DetectedObject & object) { - const double velocity = - std::abs(tier4_autoware_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear)); + const double velocity = std::abs( + autoware::universe_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear)); const double object_angle = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double object_position_angle = std::atan2( object.kinematics.pose_with_covariance.pose.position.y, object.kinematics.pose_with_covariance.pose.position.x); const double crossing_yaw = - tier4_autoware_utils::normalizeRadian(object_angle - object_position_angle); + autoware::universe_utils::normalizeRadian(object_angle - object_position_angle); if ( velocity > node_param_.velocity_threshold && diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp index a1c432d9f47b3..44ccaae8ee7fc 100644 --- a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp +++ b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -48,9 +48,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) { rclcpp::init(0, nullptr); { - auto velocity = tier4_autoware_utils::createVector3(40.0, 30.0, 0.0); - auto position = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); - auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware::universe_utils::createVector3(40.0, 30.0, 0.0); + auto position = autoware::universe_utils::createPoint(1.0, 0.0, 0.0); + auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -81,9 +81,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = tier4_autoware_utils::createVector3(40.0, 30.0, 0.0); - auto position = tier4_autoware_utils::createPoint(1.0, 2.0, 0.0); - auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware::universe_utils::createVector3(40.0, 30.0, 0.0); + auto position = autoware::universe_utils::createPoint(1.0, 2.0, 0.0); + auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -112,9 +112,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = tier4_autoware_utils::createVector3(24.0, 18.0, 0.0); - auto position = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); - auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware::universe_utils::createVector3(24.0, 18.0, 0.0); + auto position = autoware::universe_utils::createPoint(1.0, 0.0, 0.0); + auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; @@ -143,9 +143,9 @@ TEST(RadarCrossingObjectsFilter, IsNoise) } { - auto velocity = tier4_autoware_utils::createVector3(24.0, 18.0, 0.0); - auto position = tier4_autoware_utils::createPoint(1.0, 2.0, 0.0); - auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto velocity = autoware::universe_utils::createVector3(24.0, 18.0, 0.0); + auto position = autoware::universe_utils::createPoint(1.0, 2.0, 0.0); + auto orientation = autoware::universe_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp index 716d4ee8b05ff..cd7e7de6b2ff4 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp @@ -15,8 +15,8 @@ #ifndef RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ #define RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "rclcpp/logger.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #define EIGEN_MPL2_ONLY #include #include @@ -32,14 +32,14 @@ namespace radar_fusion_to_detected_object { +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::PoseWithCovariance; using geometry_msgs::msg::Twist; using geometry_msgs::msg::TwistWithCovariance; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::Point2d; class RadarFusionToDetectedObject { diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml index 4fe51b1002b15..aa0711f1cc6ce 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -16,13 +16,13 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils eigen geometry_msgs message_filters rclcpp rclcpp_components std_msgs - tier4_autoware_utils ament_lint_common autoware_lint_common diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 6a170386917e7..5a0c1b5412312 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -15,8 +15,8 @@ #include "radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp" -#include -#include +#include +#include #include @@ -29,14 +29,14 @@ namespace radar_fusion_to_detected_object { +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::PoseWithCovariance; using geometry_msgs::msg::Twist; using geometry_msgs::msg::TwistWithCovariance; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::Point2d; void RadarFusionToDetectedObject::setParam(const Param & param) { @@ -136,7 +136,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( bool RadarFusionToDetectedObject::hasTwistCovariance( const TwistWithCovariance & twist_with_covariance) { - using IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; auto covariance = twist_with_covariance.covariance; if (covariance[IDX::X_X] == 0.0 && covariance[IDX::Y_Y] == 0.0 && covariance[IDX::Z_Z] == 0.0) { return false; @@ -151,11 +151,11 @@ bool RadarFusionToDetectedObject::isYawCorrect( const DetectedObject & object, const TwistWithCovariance & twist_with_covariance, const double & yaw_threshold) { - const double twist_yaw = tier4_autoware_utils::normalizeRadian( + const double twist_yaw = autoware::universe_utils::normalizeRadian( std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x)); - const double object_yaw = tier4_autoware_utils::normalizeRadian( + const double object_yaw = autoware::universe_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(twist_yaw - object_yaw); + const double diff_yaw = autoware::universe_utils::normalizeRadian(twist_yaw - object_yaw); if (std::abs(diff_yaw) < yaw_threshold) { return true; } else if (M_PI - yaw_threshold < std::abs(diff_yaw)) { @@ -174,10 +174,12 @@ RadarFusionToDetectedObject::filterRadarWithinObject( { std::vector outputs{}; - tier4_autoware_utils::Point2d object_size{object.shape.dimensions.x, object.shape.dimensions.y}; + autoware::universe_utils::Point2d object_size{ + object.shape.dimensions.x, object.shape.dimensions.y}; LinearRing2d object_box = createObject2dWithMargin(object_size, param_.bounding_box_margin); - object_box = tier4_autoware_utils::transformVector( - object_box, tier4_autoware_utils::pose2transform(object.kinematics.pose_with_covariance.pose)); + object_box = autoware::universe_utils::transformVector( + object_box, + autoware::universe_utils::pose2transform(object.kinematics.pose_with_covariance.pose)); for (const auto & radar : (*radars)) { Point2d radar_point{ @@ -212,10 +214,10 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( Eigen::Vector2d vec_min_distance(0.0, 0.0); if (param_.velocity_weight_min_distance > 0.0) { auto comp_func = [&](const RadarInput & a, const RadarInput & b) { - return tier4_autoware_utils::calcSquaredDistance2d( + return autoware::universe_utils::calcSquaredDistance2d( a.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.position) < - tier4_autoware_utils::calcSquaredDistance2d( + autoware::universe_utils::calcSquaredDistance2d( b.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.position); }; diff --git a/perception/radar_object_clustering/README.md b/perception/radar_object_clustering/README.md index 5133ddbdbb4ad..8f936ce61a1f3 100644 --- a/perception/radar_object_clustering/README.md +++ b/perception/radar_object_clustering/README.md @@ -83,13 +83,13 @@ These are used in `isSameObject` function as below. bool RadarObjectClusteringNode::isSameObject( const DetectedObject & object_1, const DetectedObject & object_2) { - const double angle_diff = std::abs(tier4_autoware_utils::normalizeRadian( + const double angle_diff = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(object_1.kinematics.pose_with_covariance.pose.orientation) - tf2::getYaw(object_2.kinematics.pose_with_covariance.pose.orientation))); const double velocity_diff = std::abs( object_1.kinematics.twist_with_covariance.twist.linear.x - object_2.kinematics.twist_with_covariance.twist.linear.x); - const double distance = tier4_autoware_utils::calcDistance2d( + const double distance = autoware::universe_utils::calcDistance2d( object_1.kinematics.pose_with_covariance.pose.position, object_2.kinematics.pose_with_covariance.pose.position); diff --git a/perception/radar_object_clustering/package.xml b/perception/radar_object_clustering/package.xml index 44d99032380cf..c7666d2c5701e 100644 --- a/perception/radar_object_clustering/package.xml +++ b/perception/radar_object_clustering/package.xml @@ -15,13 +15,13 @@ ament_cmake_auto autoware_perception_msgs + autoware_universe_utils geometry_msgs object_recognition_utils rclcpp rclcpp_components tf2 tf2_geometry_msgs - tier4_autoware_utils autoware_cmake ament_lint_auto diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp index a659a1411dbf9..339ab21741d29 100644 --- a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp +++ b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp @@ -14,9 +14,9 @@ #include "radar_object_clustering/radar_object_clustering_node.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -179,13 +179,13 @@ void RadarObjectClusteringNode::onObjects(const DetectedObjects::ConstSharedPtr bool RadarObjectClusteringNode::isSameObject( const DetectedObject & object_1, const DetectedObject & object_2) { - const double angle_diff = std::abs(tier4_autoware_utils::normalizeRadian( + const double angle_diff = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(object_1.kinematics.pose_with_covariance.pose.orientation) - tf2::getYaw(object_2.kinematics.pose_with_covariance.pose.orientation))); const double velocity_diff = std::abs( object_1.kinematics.twist_with_covariance.twist.linear.x - object_2.kinematics.twist_with_covariance.twist.linear.x); - const double distance = tier4_autoware_utils::calcDistance2d( + const double distance = autoware::universe_utils::calcDistance2d( object_1.kinematics.pose_with_covariance.pose.position, object_2.kinematics.pose_with_covariance.pose.position); diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp b/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp index aaffea3d5c3ab..e1c33c022e427 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp @@ -34,9 +34,9 @@ #include #endif +#include +#include #include -#include -#include #include diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 4eb09a1def6a2..c3c4b33946b56 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -15,6 +15,7 @@ eigen3_cmake_module autoware_perception_msgs + autoware_universe_utils eigen glog kalman_filter @@ -26,7 +27,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils tier4_perception_msgs unique_identifier_msgs yaml-cpp diff --git a/perception/radar_object_tracker/src/data_association/data_association.cpp b/perception/radar_object_tracker/src/data_association/data_association.cpp index bd1860e6b0136..b308f62a24dfa 100644 --- a/perception/radar_object_tracker/src/data_association/data_association.cpp +++ b/perception/radar_object_tracker/src/data_association/data_association.cpp @@ -55,8 +55,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & tracker_quat, const bool distinguish_front_or_back = true) { const double measurement_yaw = - tier4_autoware_utils::normalizeRadian(tf2::getYaw(measurement_quat)); - const double tracker_yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(tracker_quat)); + autoware::universe_utils::normalizeRadian(tf2::getYaw(measurement_quat)); + const double tracker_yaw = autoware::universe_utils::normalizeRadian(tf2::getYaw(tracker_quat)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed measurement_yaw to be in the range of +-90 or 180 degrees of X_t(IDX::YAW) @@ -203,7 +203,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double dist = tier4_autoware_utils::calcDistance2d( + const double dist = autoware::universe_utils::calcDistance2d( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position); @@ -221,7 +221,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double max_area = max_area_matrix_(tracker_label, measurement_label); const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = tier4_autoware_utils::getArea(measurement_object.shape); + const double area = autoware::universe_utils::getArea(measurement_object.shape); if (area < min_area || max_area < area) { passed_gate = false; } diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index c2088858b76c5..e1603fbfab702 100644 --- a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -20,8 +20,8 @@ #include "radar_object_tracker/utils/utils.hpp" -#include -#include +#include +#include #include #include @@ -209,7 +209,7 @@ void ConstantTurnRateMotionTracker::loadDefaultModelParameters(const std::string // limitation // (TODO): this may be written in another yaml file based on classify result const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] - max_vx_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [m/s] + max_vx_ = autoware::universe_utils::kmph2mps(max_speed_kmph); // [m/s] } bool ConstantTurnRateMotionTracker::predict(const rclcpp::Time & time) @@ -393,7 +393,7 @@ bool ConstantTurnRateMotionTracker::measureWithPose( Eigen::MatrixXd Y_yaw = Eigen::MatrixXd::Zero(1, 1); const auto yaw = [&] { - auto obj_yaw = tier4_autoware_utils::normalizeRadian( + auto obj_yaw = autoware::universe_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); while (M_PI_2 <= yaw_state - obj_yaw) { obj_yaw = obj_yaw + M_PI; @@ -617,7 +617,7 @@ bool ConstantTurnRateMotionTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index ce9ddef6ea993..1bc548fa7a951 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -20,8 +20,8 @@ #include "radar_object_tracker/utils/utils.hpp" -#include -#include +#include +#include #include #include @@ -237,8 +237,8 @@ void LinearMotionTracker::loadDefaultModelParameters(const std::string & path) // limitation // (TODO): this may be written in another yaml file based on classify result const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] - max_vx_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [m/s] - max_vy_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [rad/s] + max_vx_ = autoware::universe_utils::kmph2mps(max_speed_kmph); // [m/s] + max_vy_ = autoware::universe_utils::kmph2mps(max_speed_kmph); // [rad/s] } bool LinearMotionTracker::predict(const rclcpp::Time & time) @@ -682,7 +682,7 @@ bool LinearMotionTracker::getTrackedObject( const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = - tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); } return true; diff --git a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp index 57d7b5a21fa94..ba2500a60beff 100644 --- a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp +++ b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp @@ -81,10 +81,10 @@ bool checkCloseLaneletCondition( double object_motion_yaw = object_yaw; bool velocity_is_reverted = object.kinematics.twist_with_covariance.twist.linear.x < 0.0; if (velocity_is_reverted) { - object_motion_yaw = tier4_autoware_utils::normalizeRadian(object_yaw + M_PI); + object_motion_yaw = autoware::universe_utils::normalizeRadian(object_yaw + M_PI); } const double delta_yaw = object_motion_yaw - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); if (abs_norm_delta_yaw > max_angle_diff_from_lane) { @@ -134,14 +134,14 @@ bool hasValidVelocityDirectionToLanelet( const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y; const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); const double object_vel_yaw_global = - tier4_autoware_utils::normalizeRadian(object_yaw + object_vel_yaw); + autoware::universe_utils::normalizeRadian(object_yaw + object_vel_yaw); const double object_vel = std::hypot(object_vel_x, object_vel_y); for (const auto & lanelet : lanelets) { const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_vel_yaw_global - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double lane_vel = object_vel * std::sin(normalized_delta_yaw); if (std::fabs(lane_vel) < max_lateral_velocity) { diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp index 59c9e5e579772..30d02c90b0594 100644 --- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp +++ b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp @@ -15,8 +15,8 @@ #ifndef RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ #define RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_perception_msgs/msg/detected_objects.hpp" #include "autoware_perception_msgs/msg/object_classification.hpp" @@ -64,7 +64,7 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_radar_{}; rclcpp::Subscription::SharedPtr sub_odometry_{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; // Callback void onRadarTracks(const RadarTracks::ConstSharedPtr msg); diff --git a/perception/radar_tracks_msgs_converter/package.xml b/perception/radar_tracks_msgs_converter/package.xml index f4b8ea496897f..ad0257dbbd726 100644 --- a/perception/radar_tracks_msgs_converter/package.xml +++ b/perception/radar_tracks_msgs_converter/package.xml @@ -16,6 +16,7 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils geometry_msgs nav_msgs radar_msgs @@ -24,7 +25,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index 234f430d6fb81..36f4d3cd3da90 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -14,9 +14,9 @@ #include "radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include @@ -92,7 +92,7 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt sub_odometry_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, std::bind(&RadarTracksMsgsConverterNode::onTwist, this, _1)); - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); // Publisher pub_tracked_objects_ = create_publisher("~/output/radar_tracked_objects", 1); @@ -261,7 +261,7 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() } // yaw angle (vehicle heading) is obtained from ground velocity - double yaw = tier4_autoware_utils::normalizeRadian( + double yaw = autoware::universe_utils::normalizeRadian( std::atan2(compensated_velocity.y, compensated_velocity.x)); // kinematics setting @@ -273,7 +273,7 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() // velocity of object is defined in the object coordinate // heading is obtained from ground velocity kinematics.pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw); + autoware::universe_utils::createQuaternionFromYaw(yaw); // longitudinal velocity is the length of the velocity vector // lateral velocity is zero, use default value kinematics.twist_with_covariance.twist.linear.x = std::sqrt( @@ -334,8 +334,8 @@ geometry_msgs::msg::Vector3 RadarTracksMsgsConverterNode::compensateVelocityEgoM std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware::universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array pose_covariance{}; pose_covariance[POSE_IDX::X_X] = radar_track.position_covariance[RADAR_IDX::X_X]; pose_covariance[POSE_IDX::X_Y] = radar_track.position_covariance[RADAR_IDX::X_Y]; @@ -351,8 +351,8 @@ std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix std::array RadarTracksMsgsConverterNode::convertTwistCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware::universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array twist_covariance{}; twist_covariance[POSE_IDX::X_X] = radar_track.velocity_covariance[RADAR_IDX::X_X]; twist_covariance[POSE_IDX::X_Y] = radar_track.velocity_covariance[RADAR_IDX::X_Y]; @@ -368,8 +368,8 @@ std::array RadarTracksMsgsConverterNode::convertTwistCovarianceMatri std::array RadarTracksMsgsConverterNode::convertAccelerationCovarianceMatrix( const radar_msgs::msg::RadarTrack & radar_track) { - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = autoware::universe_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; std::array acceleration_covariance{}; acceleration_covariance[POSE_IDX::X_X] = radar_track.acceleration_covariance[RADAR_IDX::X_X]; acceleration_covariance[POSE_IDX::X_Y] = radar_track.acceleration_covariance[RADAR_IDX::X_Y]; diff --git a/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp b/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp index 86a790e208012..b3ebc4d916947 100644 --- a/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp +++ b/perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp @@ -17,9 +17,9 @@ #include "detected_object_validation/utils/utils.hpp" +#include +#include #include -#include -#include #include @@ -68,9 +68,9 @@ class LowIntensityClusterFilter : public rclcpp::Node utils::FilterTargetLabel filter_target_; // debugger - std::unique_ptr> stop_watch_ptr_{ + std::unique_ptr> stop_watch_ptr_{ nullptr}; - std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; }; } // namespace low_intensity_cluster_filter diff --git a/perception/raindrop_cluster_filter/package.xml b/perception/raindrop_cluster_filter/package.xml index 492b789239d54..bff3f71db6f04 100644 --- a/perception/raindrop_cluster_filter/package.xml +++ b/perception/raindrop_cluster_filter/package.xml @@ -12,6 +12,7 @@ ament_cmake autoware_cmake + autoware_universe_utils detected_object_validation lanelet2_extension message_filters @@ -24,7 +25,6 @@ tf2 tf2_eigen tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/raindrop_cluster_filter/raindrop_cluster_filter.md b/perception/raindrop_cluster_filter/raindrop_cluster_filter.md index 175bbd00a93ea..e92e645f7a3e8 100644 --- a/perception/raindrop_cluster_filter/raindrop_cluster_filter.md +++ b/perception/raindrop_cluster_filter/raindrop_cluster_filter.md @@ -26,22 +26,22 @@ Mainly this focuses on filtering out unknown objects with very low intensity poi ### Core Parameters -| Name | Type | Default Value | Description | -| --------------------------------- | ----- | ------------------------------------------------------- | --------------------------------------------- | -| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. | -| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. | -| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. | -| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. | -| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. | -| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. | -| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. | -| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. | -| `max_x` | float | 60.00 | Maximum of x of the filter effective range | -| `min_x` | float | -20.00 | Minimum of x of the filter effective range | -| `max_y` | float | 20.00 | Maximum of y of the filter effective range | -| `min_y` | float | -20.00 | Minium of y of the filter effective range | -| `intensity_threshold` | float | 1.0 | The threshold of average intensity for filter | -| `existence_probability_threshold` | float | The existence probability threshold to apply the filter | +| Name | Type | Default Value | Description | +| --------------------------------- | ----- | ------------- | ------------------------------------------------------- | +| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. | +| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. | +| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. | +| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. | +| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. | +| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. | +| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. | +| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. | +| `max_x` | float | 60.00 | Maximum of x of the filter effective range | +| `min_x` | float | -20.00 | Minimum of x of the filter effective range | +| `max_y` | float | 20.00 | Maximum of y of the filter effective range | +| `min_y` | float | -20.00 | Minium of y of the filter effective range | +| `intensity_threshold` | float | 1.0 | The threshold of average intensity for filter | +| `existence_probability_threshold` | float | 0.2 | The existence probability threshold to apply the filter | ## Assumptions / Known limits diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp index 5c47b61eaa0a3..4772c355ec0b6 100644 --- a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp +++ b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp @@ -14,8 +14,8 @@ #include "raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp" +#include #include -#include #include @@ -52,8 +52,8 @@ LowIntensityClusterFilter::LowIntensityClusterFilter(const rclcpp::NodeOptions & "output/objects", rclcpp::QoS{1}); // initialize debug tool { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ptr_ = std::make_unique(this, "low_intensity_cluster_filter_node"); @@ -85,8 +85,8 @@ void LowIntensityClusterFilter::objectCallback( geometry_msgs::msg::Pose max_pose; max_pose.position.x = max_x_; max_pose.position.y = max_y_; - auto min_ranged_transformed = tier4_autoware_utils::transformPose(min_range, transform_stamp); - auto max_range_transformed = tier4_autoware_utils::transformPose(max_pose, transform_stamp); + auto min_ranged_transformed = autoware::universe_utils::transformPose(min_range, transform_stamp); + auto max_range_transformed = autoware::universe_utils::transformPose(max_pose, transform_stamp); for (const auto & feature_object : input_msg->feature_objects) { const auto & object = feature_object.object; const auto & label = object.classification.front().label; diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/shape_estimation/lib/corrector/utils.cpp index af42400dcc065..1332647b0f1f4 100644 --- a/perception/shape_estimation/lib/corrector/utils.cpp +++ b/perception/shape_estimation/lib/corrector/utils.cpp @@ -14,7 +14,7 @@ #include "shape_estimation/corrector/utils.hpp" -#include +#include #include #include @@ -246,9 +246,9 @@ bool correctWithDefaultValue( // correct to set long length is x, short length is y if (shape.dimensions.x < shape.dimensions.y) { - geometry_msgs::msg::Vector3 rpy = tier4_autoware_utils::getRPY(pose.orientation); + geometry_msgs::msg::Vector3 rpy = autoware::universe_utils::getRPY(pose.orientation); rpy.z = rpy.z + M_PI_2; - pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z); + pose.orientation = autoware::universe_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z); double temp = shape.dimensions.x; shape.dimensions.x = shape.dimensions.y; shape.dimensions.y = temp; diff --git a/perception/shape_estimation/package.xml b/perception/shape_estimation/package.xml index e91a399baa076..8eab542036177 100644 --- a/perception/shape_estimation/package.xml +++ b/perception/shape_estimation/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_perception_msgs + autoware_universe_utils eigen libopencv-dev libpcl-all-dev @@ -21,7 +22,6 @@ tf2 tf2_eigen tf2_geometry_msgs - tier4_autoware_utils tier4_perception_msgs ament_cmake_gtest diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index c20308ca0b6d1..5719cd97b3ed5 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -14,8 +14,8 @@ #include "shape_estimation/shape_estimator.hpp" +#include #include -#include #include @@ -54,11 +54,13 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); processing_time_publisher_ = - std::make_unique(this, "shape_estimation"); - stop_watch_ptr_ = std::make_unique>(); + std::make_unique(this, "shape_estimation"); + stop_watch_ptr_ = + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } static autoware_perception_msgs::msg::ObjectClassification::_label_type get_label( @@ -112,7 +114,7 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared if (use_vehicle_reference_yaw_ && is_vehicle) { ref_yaw_info = ReferenceYawInfo{ static_cast(tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)), - tier4_autoware_utils::deg2rad(10)}; + autoware::universe_utils::deg2rad(10)}; } if (use_vehicle_reference_shape_size_ && is_vehicle) { ref_shape_size_info = ReferenceShapeSizeInfo{object.shape, ReferenceShapeSizeInfo::Mode::Min}; diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp index a3788668ef6ae..807f12e39b025 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/node.hpp @@ -17,10 +17,10 @@ #include "shape_estimation/shape_estimator.hpp" +#include +#include +#include #include -#include -#include -#include #include #include @@ -35,11 +35,11 @@ class ShapeEstimationNode : public rclcpp::Node // ros rclcpp::Publisher::SharedPtr pub_; rclcpp::Subscription::SharedPtr sub_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // debug publisher - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; void callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg); diff --git a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp index 223272b64d716..d25eef20cc676 100644 --- a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp +++ b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp @@ -15,8 +15,8 @@ #ifndef SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ #define SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ +#include "autoware/universe_utils/ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_perception_msgs/msg/detected_objects.hpp" @@ -47,7 +47,7 @@ class SimpleObjectMergerNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_objects_{}; std::vector::SharedPtr> sub_objects_array{}; - std::shared_ptr transform_listener_; + std::shared_ptr transform_listener_; // Callback void onData(const DetectedObjects::ConstSharedPtr msg, size_t array_number); diff --git a/perception/simple_object_merger/package.xml b/perception/simple_object_merger/package.xml index 4586efe53f2e7..487155bffd102 100644 --- a/perception/simple_object_merger/package.xml +++ b/perception/simple_object_merger/package.xml @@ -13,10 +13,10 @@ ament_cmake_auto autoware_perception_msgs + autoware_universe_utils geometry_msgs rclcpp rclcpp_components - tier4_autoware_utils autoware_cmake ament_lint_auto diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index fabf06595970c..faa531560ba09 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -98,7 +98,7 @@ SimpleObjectMergerNode::SimpleObjectMergerNode(const rclcpp::NodeOptions & node_ } // Subscriber - transform_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); sub_objects_array.resize(input_topic_size); objects_data_.resize(input_topic_size); diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp index fc396ae4b3473..6044148a932ae 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp @@ -15,12 +15,12 @@ #ifndef TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ #define TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ +#include +#include #include #include #include #include -#include -#include #include #include @@ -63,8 +63,8 @@ class TrtYoloXNode : public rclcpp::Node LabelMap label_map_; std::unique_ptr trt_yolox_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace tensorrt_yolox diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp index ea7857c3768aa..11f68f580acc2 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp @@ -956,7 +956,7 @@ void TrtYoloX::generateYoloxProposals( objects.push_back(obj); } } // class loop - } // point anchor loop + } // point anchor loop } void TrtYoloX::qsortDescentInplace(ObjectArray & face_objects, int left, int right) const diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp index c1c4c42741cee..4ee18b99e4bc6 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -31,9 +31,9 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) { { stop_watch_ptr_ = - std::make_unique>(); + std::make_unique>(); debug_publisher_ = - std::make_unique(this, "tensorrt_yolox"); + std::make_unique(this, "tensorrt_yolox"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp index 7fe4d3a06decc..3683ba02b38cd 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -19,10 +19,10 @@ #include "tracking_object_merger/utils/tracker_state.hpp" #include "tracking_object_merger/utils/utils.hpp" +#include +#include +#include #include -#include -#include -#include #include #include @@ -86,8 +86,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node // debug object publisher rclcpp::Publisher::SharedPtr debug_object_pub_; bool publish_interpolated_sub_objects_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; /* handle objects */ std::unordered_map> @@ -106,7 +106,7 @@ class DecorativeTrackerMergerNode : public rclcpp::Node // tracker default settings TrackerStateParameter tracker_state_parameter_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // merge policy (currently not used) struct diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp index be53f69ff74cb..1778e09da5cd9 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp @@ -17,8 +17,8 @@ #ifndef TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ #define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ -// #include -#include "tier4_autoware_utils/geometry/geometry.hpp" +// #include +#include "autoware/universe_utils/geometry/geometry.hpp" #include diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml index d5904571cec6e..370f5beb3e3b3 100644 --- a/perception/tracking_object_merger/package.xml +++ b/perception/tracking_object_merger/package.xml @@ -14,6 +14,7 @@ eigen3_cmake_module autoware_perception_msgs + autoware_universe_utils eigen glog mussp @@ -22,7 +23,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/tracking_object_merger/src/data_association/data_association.cpp b/perception/tracking_object_merger/src/data_association/data_association.cpp index 21bb1b46cb14a..4d10a415df7da 100644 --- a/perception/tracking_object_merger/src/data_association/data_association.cpp +++ b/perception/tracking_object_merger/src/data_association/data_association.cpp @@ -14,8 +14,8 @@ #include "tracking_object_merger/data_association/data_association.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "tracking_object_merger/data_association/solver/gnn_solver.hpp" #include "tracking_object_merger/utils/utils.hpp" @@ -31,8 +31,8 @@ double getFormedYawAngle( const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, const bool distinguish_front_or_back = true) { - const double yaw0 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat0)); - const double yaw1 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat1)); + const double yaw0 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat0)); + const double yaw1 = autoware::universe_utils::normalizeRadian(tf2::getYaw(quat1)); const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 @@ -183,7 +183,7 @@ double DataAssociation::calcScoreBetweenObjects( double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { const double max_dist = max_dist_matrix_(object1_label, object0_label); - const double dist = tier4_autoware_utils::calcDistance2d( + const double dist = autoware::universe_utils::calcDistance2d( object0.kinematics.pose_with_covariance.pose.position, object1.kinematics.pose_with_covariance.pose.position); diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index 30762d4a21231..99dccab3ff72c 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -155,12 +155,14 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio set3dDataAssociation("radar-radar", data_association_map_); // debug publisher - processing_time_publisher_ = - std::make_unique(this, "decorative_object_merger_node"); - stop_watch_ptr_ = std::make_unique>(); + processing_time_publisher_ = std::make_unique( + this, "decorative_object_merger_node"); + stop_watch_ptr_ = + std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void DecorativeTrackerMergerNode::set3dDataAssociation( diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 343c9d9711032..e981ef09a21b0 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -280,7 +280,7 @@ bool objectsHaveSameMotionDirections(const TrackedObject & main_obj, const Track // diff of motion yaw angle const auto motion_yaw_diff = std::fabs(main_motion_yaw - sub_motion_yaw); const auto normalized_motion_yaw_diff = - tier4_autoware_utils::normalizeRadian(motion_yaw_diff); // -pi ~ pi + autoware::universe_utils::normalizeRadian(motion_yaw_diff); // -pi ~ pi // evaluate if motion yaw angle is same constexpr double yaw_threshold = M_PI / 4.0; // 45 deg if (std::abs(normalized_motion_yaw_diff) < yaw_threshold) { @@ -305,7 +305,7 @@ bool objectsYawIsReverted(const TrackedObject & main_obj, const TrackedObject & const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); // calc yaw diff const auto yaw_diff = std::fabs(main_yaw - sub_yaw); - const auto normalized_yaw_diff = tier4_autoware_utils::normalizeRadian(yaw_diff); // -pi ~ pi + const auto normalized_yaw_diff = autoware::universe_utils::normalizeRadian(yaw_diff); // -pi ~ pi // evaluate if yaw is reverted constexpr double yaw_threshold = M_PI / 2.0; // 90 deg if (std::abs(normalized_yaw_diff) >= yaw_threshold) { diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/nodelet.cpp index 89ba30a8b71c2..6378a98fdc8cc 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/traffic_light_classifier/src/nodelet.cpp @@ -157,6 +157,9 @@ void TrafficLightClassifierNodelet::imageRoiCallback( bool TrafficLightClassifierNodelet::is_harsh_backlight(const cv::Mat & img) const { + if (img.empty()) { + return false; + } cv::Mat y_cr_cb; cv::cvtColor(img, y_cr_cb, cv::COLOR_RGB2YCrCb); diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml index de0f62829cf18..e72cac75cd809 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/traffic_light_map_based_detector/package.xml @@ -15,6 +15,7 @@ autoware_map_msgs autoware_planning_msgs + autoware_universe_utils geometry_msgs image_geometry lanelet2_extension @@ -25,7 +26,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_perception_msgs ament_lint_auto diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index 465ab5f85d9a7..7750bf642516a 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -14,11 +14,11 @@ #include "traffic_light_map_based_detector/node.hpp" +#include +#include #include #include #include -#include -#include #include #include @@ -512,9 +512,10 @@ void MapBasedDetector::getVisibleTrafficLights( double max_angle_range; if (pedestrian_tl_id_.find(traffic_light.id()) != pedestrian_tl_id_.end()) { max_angle_range = - tier4_autoware_utils::deg2rad(config_.pedestrian_traffic_light_max_angle_range); + autoware::universe_utils::deg2rad(config_.pedestrian_traffic_light_max_angle_range); } else { - max_angle_range = tier4_autoware_utils::deg2rad(config_.car_traffic_light_max_angle_range); + max_angle_range = + autoware::universe_utils::deg2rad(config_.car_traffic_light_max_angle_range); } // traffic light bottom left const auto & tl_bl = traffic_light.front(); @@ -530,7 +531,7 @@ void MapBasedDetector::getVisibleTrafficLights( } // check angle range - const double tl_yaw = tier4_autoware_utils::normalizeRadian( + const double tl_yaw = autoware::universe_utils::normalizeRadian( std::atan2(tl_br.y() - tl_bl.y(), tl_br.x() - tl_bl.x()) + M_PI_2); // get direction of z axis @@ -538,7 +539,7 @@ void MapBasedDetector::getVisibleTrafficLights( tf2::Matrix3x3 camera_rotation_matrix(tf_map2camera.getRotation()); camera_z_dir = camera_rotation_matrix * camera_z_dir; double camera_yaw = std::atan2(camera_z_dir.y(), camera_z_dir.x()); - camera_yaw = tier4_autoware_utils::normalizeRadian(camera_yaw); + camera_yaw = autoware::universe_utils::normalizeRadian(camera_yaw); if (!isInAngleRange(tl_yaw, camera_yaw, max_angle_range)) { continue; } diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp index 7e2825197f3a7..c14a5d56dc5d4 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp +++ b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp @@ -16,10 +16,10 @@ #ifndef TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__OCCLUSION_PREDICTOR_HPP_ #define TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__OCCLUSION_PREDICTOR_HPP_ +#include +#include #include #include -#include -#include #include #include diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/traffic_light_occlusion_predictor/package.xml index 47077ce342621..9a701548092d9 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/traffic_light_occlusion_predictor/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_map_msgs + autoware_universe_utils geometry_msgs image_geometry lanelet2_extension @@ -24,7 +25,6 @@ tf2 tf2_eigen tf2_ros - tier4_autoware_utils tier4_perception_msgs traffic_light_utils diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index ddd4e52d5248a..1a2eaa6c4d941 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -14,11 +14,11 @@ #include "traffic_light_occlusion_predictor/nodelet.hpp" +#include #include #include #include #include -#include #include #include diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 6f96913420bad..6515a515397da 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -102,7 +102,7 @@ void CloudOcclusionPredictor::predict( pcl::PointCloud cloud_camera; // points within roi pcl::PointCloud cloud_roi; - tier4_autoware_utils::transformPointCloudFromROSMsg(*cloud_msg, cloud_camera, camera2cloud); + autoware::universe_utils::transformPointCloudFromROSMsg(*cloud_msg, cloud_camera, camera2cloud); filterCloud(cloud_camera, roi_tls, roi_brs, cloud_roi); diff --git a/planning/.pages b/planning/.pages index 5fe671a3629cc..3c868628198a8 100644 --- a/planning/.pages +++ b/planning/.pages @@ -1,40 +1,39 @@ nav: - 'Introduction': planning - 'Behavior Path Planner': - - 'About Behavior Path': planning/autoware_behavior_path_planner + - 'About Behavior Path': planning/behavior_path_planner/autoware_behavior_path_planner - 'Concept': - - 'Planner Manager': planning/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design - - 'Scene Module Interface': planning/autoware_behavior_path_planner/docs/behavior_path_planner_interface_design - - 'Path Generation': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design - - 'Collision Assessment/Safety Check': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check - - 'Dynamic Drivable Area': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design - - 'Turn Signal': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design + - 'Planner Manager': planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design + - 'Scene Module Interface': planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_interface_design + - 'Path Generation': planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design + - 'Collision Assessment/Safety Check': planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check + - 'Dynamic Drivable Area': planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design + - 'Turn Signal': planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design - 'Scene Module': - - 'Avoidance by Lane Change': planning/autoware_behavior_path_avoidance_by_lane_change_module - - 'Dynamic Obstacle Avoidance': planning/autoware_behavior_path_dynamic_obstacle_avoidance_module - - 'Goal Planner': planning/autoware_behavior_path_goal_planner_module - - 'Lane Change': planning/autoware_behavior_path_lane_change_module - - 'Side Shift': planning/autoware_behavior_path_side_shift_module - - 'Start Planner': planning/autoware_behavior_path_start_planner_module - - 'Static Obstacle Avoidance': planning/autoware_behavior_path_static_obstacle_avoidance_module + - 'Avoidance by Lane Change': planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module + - 'Dynamic Obstacle Avoidance': planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module + - 'Goal Planner': planning/behavior_path_planner/autoware_behavior_path_goal_planner_module + - 'Lane Change': planning/behavior_path_planner/autoware_behavior_path_lane_change_module + - 'Side Shift': planning/behavior_path_planner/autoware_behavior_path_side_shift_module + - 'Start Planner': planning/behavior_path_planner/autoware_behavior_path_start_planner_module + - 'Static Obstacle Avoidance': planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module - 'Behavior Velocity Planner': - - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner - - 'Template for Custom Module': planning/autoware_behavior_velocity_template_module + - 'About Behavior Velocity': planning/behavior_velocity_planner/autoware_behavior_velocity_planner + - 'Template for Custom Module': planning/behavior_velocity_planner/autoware_behavior_velocity_template_module - 'Available Module': - - 'Blind Spot': planning/autoware_behavior_velocity_blind_spot_module - - 'Crosswalk': planning/autoware_behavior_velocity_crosswalk_module + - 'Blind Spot': planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module + - 'Crosswalk': planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module - 'Detection Area': planning/behavior_velocity_detection_area_module - - 'Dynamic Obstacle Stop': planning/behavior_velocity_dynamic_obstacle_stop_module - - 'Intersection': planning/autoware_behavior_velocity_intersection_module - - 'No Drivable Lane': planning/behavior_velocity_no_drivable_lane_module - - 'No Stopping Area': planning/autoware_behavior_velocity_no_stopping_area_module - - 'Occlusion Spot': planning/autoware_behavior_velocity_occlusion_spot_module - - 'Run Out': planning/autoware_behavior_velocity_run_out_module - - 'Speed Bump': planning/behavior_velocity_speed_bump_module - - 'Stop Line': planning/autoware_behavior_velocity_stop_line_module - - 'Traffic Light': planning/autoware_behavior_velocity_traffic_light_module - - 'Virtual Traffic Light': planning/autoware_behavior_velocity_virtual_traffic_light_module - - 'Walkway': planning/autoware_behavior_velocity_walkway_module + - 'Intersection': planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module + - 'No Drivable Lane': planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module + - 'No Stopping Area': planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module + - 'Occlusion Spot': planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module + - 'Run Out': planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module + - 'Speed Bump': planning/behavior_velocity_planner/behavior_velocity_speed_bump_module + - 'Stop Line': planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module + - 'Traffic Light': planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module + - 'Virtual Traffic Light': planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module + - 'Walkway': planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module - 'Parking': - 'Freespace Planner': - 'About Freespace Planner': planning/autoware_freespace_planner @@ -64,6 +63,7 @@ nav: - 'Motion Velocity Planner': - 'About Motion Velocity Planner': planning/motion_velocity_planner/autoware_motion_velocity_planner_node/ - 'Available Modules': + - 'Dynamic Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/ - 'Out of Lane': planning/motion_velocity_planner/autoware_motion_velocity_planner_out_of_lane_module/ - 'Obstacle Velocity Limiter': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/ - 'Velocity Smoother': diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_design.fig.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_design.fig.drawio.svg deleted file mode 100644 index d0563e9f2f542..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_design.fig.drawio.svg +++ /dev/null @@ -1,668 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Calculate avoidance shift -
- from target obstacles -
-
-
-
- Calculate avoidance shift... -
-
- - - - - - - - - - - - - - - - - -
-
-
- shift length = avoid margin -
- + ego vehicle width / 2 -
-
-
-
- shift length = avoid marg... -
-
- - - - - - -
-
-
- The shift gets sharp -
- if it is close to ego -
- up to the maximum jerk. -
-
-
-
- The shift gets sharp... -
-
- - - - - - -
-
-
- The avoidance shift is basically calculated from nominal jerk ego vehicle speed. -
-
-
-
- The avoidance shift is ba... -
-
- - - - - - - -
-
-
- This is not a target vehicle -
- since it is on the center line. -
-
-
-
- This is not a target vehi... -
-
- - - - -
-
-
- The direction of avoidance is determined by the position of obstacles, whether it is on the left or right of the center line. -
-
-
-
- The direction of avoidanc... -
-
- - - - - - -
-
-
- Merge all shifts -
-
-
-
- Merge all shifts -
-
- - - - - - - - - - - - - - -
-
-
- For multiple avoidance shifts in the same direction, take the largest one. -
-
-
-
- For multiple avoidance sh... -
-
- - - - - -
-
-
- For multiple avoidance shifts in the different direction, take the sum of both. -
-
-
-
- For multiple avoidance... -
-
- - - - - - - - - - - - -
-
-
- Pick up the shift points where the gradient of the shift changes. -
-
-
-
- Pick up the shift points... -
-
- - - - - - - - - - - - - -
-
-
- Trim shift points -
-
-
-
- Trim shift points -
-
- - - - - - -
-
-
- Start avoiding as soon as getting back to the center line. Remove these points to avoid obstacles at the same shift. -
-
-
-
- Start avoiding as soon a... -
-
- - - - - - -
-
-
- Points that do not have a large effect on the overall shift will be deleted. -
-
-
-
- Points that do not have... -
-
- - - - - - - - - - - - - - - - - - - - - -
-
-
- Generate path -
-
-
-
- Generate path -
-
- - - - -
-
-
- Each shift points are connected by a Clothoid-like curve. -
-
-
-
- Each shift points... -
-
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- - - - Text is not SVG - cannot display - - -
diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/intersection_problem.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/intersection_problem.drawio.svg deleted file mode 100644 index 67d21dfd78a09..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/intersection_problem.drawio.svg +++ /dev/null @@ -1,179 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
Parked vehicle
-
-
-
- Parked vehicle -
-
- - - -
-
-
- Normal avoidance will block the opposite lane -
-
-
-
- Normal avoidance will block the opposite lane -
-
- - - -
-
-
- So, it should avoid inside the ego lane -
-
-
-
- So, it should avoid inside the ego lan... -
-
- - - -
-
-
- Or, wait behind the object -
- until the traffic light turns green. -
-
-
-
- Or, wait behind the object... -
-
-
- - - - Viewer does not support full SVG 1.1 - - -
diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/lateral.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/lateral.png deleted file mode 100644 index 8d8512f042922..0000000000000 Binary files a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/lateral.png and /dev/null differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/margin.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/margin.png deleted file mode 100644 index 8920a7be56ff9..0000000000000 Binary files a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/margin.png and /dev/null differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_vehicle_selection.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_vehicle_selection.svg deleted file mode 100644 index 00802b074cdd7..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_vehicle_selection.svg +++ /dev/null @@ -1,296 +0,0 @@ - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
-
- Ego -
-
- - - - -
-
-
- road shoulder -
-
-
-
- road shoulder -
-
- - - - - - -
-
-
- center line -
-
-
-
- center line -
-
- - - - - - - -
-
-
- threshold_distance_object_is_on_center -
-
-
-
- thresh... -
-
- - - - - - - -
-
-
- Object -
-
-
-
- Object -
-
- - - - -
-
-
- object -
- center -
-
-
-
- object... -
-
- - - - - - - -
-
-
- shiftable_length -
-
-
-
- shiftable_l... -
-
- - - - -
-
-
- shift_length -
-
-
-
- shift_length -
-
- - - - -
-
-
- ego lane -
-
-
-
- ego lane -
-
- - - - - - - -
-
-
- detection_area_left_expand_dist -
-
-
-
- detection_a... -
-
- - - - - - - - -
-
-
- object_check_forward_distance -
-
-
-
- object... -
-
- - - - -
-
-
- object_check_backward_distance -
-
-
-
- object... -
-
- - - - - - - - -
-
-
- detection area -
-
-
-
- detect... -
-
-
- - - - Text is not SVG - cannot display - - -
diff --git a/planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp similarity index 91% rename from planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp rename to planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp index e4043f1aeb986..ab27f3727e847 100644 --- a/planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp +++ b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ -#define AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#ifndef AUTOWARE__EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#define AUTOWARE__EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ #include @@ -82,4 +82,4 @@ class ExternalVelocityLimitSelectorNode : public rclcpp::Node }; } // namespace autoware::external_velocity_limit_selector -#endif // AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#endif // AUTOWARE__EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ diff --git a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index 008b58df303b1..f2038a11362b5 100644 --- a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" +#include "autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" #include #include diff --git a/planning/autoware_freespace_planner/include/autoware_freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp similarity index 91% rename from planning/autoware_freespace_planner/include/autoware_freespace_planner/freespace_planner_node.hpp rename to planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp index 0606d1d2bb369..70167f0833586 100644 --- a/planning/autoware_freespace_planner/include/autoware_freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp @@ -28,13 +28,13 @@ * limitations under the License. */ -#ifndef AUTOWARE_FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ -#define AUTOWARE_FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ +#ifndef AUTOWARE__FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ +#define AUTOWARE__FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" -#include -#include +#include +#include #include #include @@ -52,7 +52,7 @@ #include #endif -#include +#include #include #include @@ -163,8 +163,8 @@ class FreespacePlannerNode : public rclcpp::Node TransformStamped getTransform(const std::string & from, const std::string & to); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; } // namespace autoware::freespace_planner -#endif // AUTOWARE_FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ +#endif // AUTOWARE__FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ diff --git a/planning/autoware_freespace_planner/package.xml b/planning/autoware_freespace_planner/package.xml index a5ed76d7d27b6..ebc8d9df654f7 100644 --- a/planning/autoware_freespace_planner/package.xml +++ b/planning/autoware_freespace_planner/package.xml @@ -16,19 +16,19 @@ autoware_cmake autoware_freespace_planning_algorithms + autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - motion_utils nav_msgs rclcpp rclcpp_components tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index 797c140aa9dd9..55068263f5d5b 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -28,12 +28,12 @@ * limitations under the License. */ -#include "autoware_freespace_planner/freespace_planner_node.hpp" +#include "autoware/freespace_planner/freespace_planner_node.hpp" -#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" -#include -#include +#include +#include #include #include @@ -142,8 +142,8 @@ Trajectory getPartialTrajectory( double calcDistance2d(const Trajectory & trajectory, const Pose & pose) { - const auto idx = motion_utils::findNearestIndex(trajectory.points, pose.position); - return tier4_autoware_utils::calcDistance2d(trajectory.points.at(idx), pose); + const auto idx = autoware::motion_utils::findNearestIndex(trajectory.points, pose.position); + return autoware::universe_utils::calcDistance2d(trajectory.points.at(idx), pose); } Pose transformPose(const Pose & pose, const TransformStamped & transform) @@ -287,7 +287,7 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti this, get_clock(), period_ns, std::bind(&FreespacePlannerNode::onTimer, this)); } - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam() @@ -365,8 +365,8 @@ bool FreespacePlannerNode::isPlanRequired() if (node_param_.replan_when_obstacle_found) { algo_->setMap(*occupancy_grid_); - const size_t nearest_index_partial = - motion_utils::findNearestIndex(partial_trajectory_.points, current_pose_.pose.position); + const size_t nearest_index_partial = autoware::motion_utils::findNearestIndex( + partial_trajectory_.points, current_pose_.pose.position); const size_t end_index_partial = partial_trajectory_.points.size() - 1; const auto forward_trajectory = @@ -395,7 +395,7 @@ bool FreespacePlannerNode::isPlanRequired() void FreespacePlannerNode::updateTargetIndex() { const auto is_near_target = - tier4_autoware_utils::calcDistance2d(trajectory_.points.at(target_index_), current_pose_) < + autoware::universe_utils::calcDistance2d(trajectory_.points.at(target_index_), current_pose_) < node_param_.th_arrived_distance_m; const auto is_stopped = isStopped(odom_buffer_, node_param_.th_stopped_velocity_mps); diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp index 319786dc722dd..7b4c8e3856b38 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_freespace_planner/freespace_planner_node.hpp" +#include "autoware/freespace_planner/freespace_planner_node.hpp" #include #include diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/abstract_algorithm.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp similarity index 95% rename from planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/abstract_algorithm.hpp rename to planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp index 394900c9a704c..bca08f0d11c58 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ -#define AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ +#ifndef AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ +#define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ +#include #include -#include #include #include @@ -189,4 +189,4 @@ class AbstractPlanningAlgorithm } // namespace autoware::freespace_planning_algorithms -#endif // AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ +#endif // AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/astar_search.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp similarity index 93% rename from planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/astar_search.hpp rename to planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp index 99cf3fc743a82..a746ceea6f838 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/astar_search.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ -#define AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ +#ifndef AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ +#define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ -#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" -#include "autoware_freespace_planning_algorithms/reeds_shepp.hpp" +#include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware/freespace_planning_algorithms/reeds_shepp.hpp" #include @@ -171,4 +171,4 @@ class AstarSearch : public AbstractPlanningAlgorithm }; } // namespace autoware::freespace_planning_algorithms -#endif // AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ +#endif // AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/reeds_shepp.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/reeds_shepp.hpp similarity index 96% rename from planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/reeds_shepp.hpp rename to planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/reeds_shepp.hpp index ece30175fad86..26125f08cd667 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/reeds_shepp.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/reeds_shepp.hpp @@ -78,8 +78,8 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#ifndef AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ -#define AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ +#ifndef AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ +#define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ #include #include @@ -139,4 +139,4 @@ class ReedsSheppStateSpace }; } // namespace autoware::freespace_planning_algorithms -#endif // AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ +#endif // AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp similarity index 88% rename from planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar.hpp rename to planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp index 6a203d84a47c9..17e7f1f1ebf56 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ -#define AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ +#ifndef AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ +#define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ -#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" -#include "autoware_freespace_planning_algorithms/rrtstar_core.hpp" +#include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware/freespace_planning_algorithms/rrtstar_core.hpp" #include @@ -71,4 +71,4 @@ class RRTStar : public AbstractPlanningAlgorithm } // namespace autoware::freespace_planning_algorithms -#endif // AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ +#endif // AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar_core.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar_core.hpp similarity index 95% rename from planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar_core.hpp rename to planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar_core.hpp index 633da7a86a8c6..54568a1004af7 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar_core.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar_core.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ -#define AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ +#ifndef AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ +#define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ -#include "autoware_freespace_planning_algorithms/reeds_shepp.hpp" +#include "autoware/freespace_planning_algorithms/reeds_shepp.hpp" #include @@ -158,4 +158,4 @@ class RRTStar } // namespace autoware::freespace_planning_algorithms::rrtstar_core -#endif // AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ +#endif // AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ diff --git a/planning/autoware_freespace_planning_algorithms/package.xml b/planning/autoware_freespace_planning_algorithms/package.xml index 61173f0a877e4..05e906c998096 100644 --- a/planning/autoware_freespace_planning_algorithms/package.xml +++ b/planning/autoware_freespace_planning_algorithms/package.xml @@ -16,6 +16,7 @@ autoware_cmake python_cmake_module + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs nav_msgs @@ -25,7 +26,6 @@ std_msgs tf2 tf2_geometry_msgs - tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index b872ee993c5e8..0a76067bc77f8 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" -#include "autoware_freespace_planning_algorithms/astar_search.hpp" +#include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware/freespace_planning_algorithms/astar_search.hpp" #include #include diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index d946ea87cff81..c434635ff1401 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" -#include -#include +#include +#include #include namespace autoware::freespace_planning_algorithms { -using tier4_autoware_utils::createQuaternionFromYaw; -using tier4_autoware_utils::normalizeRadian; +using autoware::universe_utils::createQuaternionFromYaw; +using autoware::universe_utils::normalizeRadian; geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) @@ -91,13 +91,13 @@ geometry_msgs::msg::Pose local2global( double PlannerWaypoints::compute_length() const { if (waypoints.empty()) { - std::runtime_error("cannot compute cost because waypoint has size 0"); + throw std::runtime_error("cannot compute cost because waypoint has size 0"); } double total_cost = 0.0; for (size_t i = 0; i < waypoints.size() - 1; ++i) { const auto pose_a = waypoints.at(i); const auto pose_b = waypoints.at(i + 1); - total_cost += tier4_autoware_utils::calcDistance2d(pose_a.pose, pose_b.pose); + total_cost += autoware::universe_utils::calcDistance2d(pose_a.pose, pose_b.pose); } return total_cost; } diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 1a7b23251111c..6b04ca87bfb7d 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_freespace_planning_algorithms/astar_search.hpp" +#include "autoware/freespace_planning_algorithms/astar_search.hpp" -#include -#include +#include +#include #include @@ -42,7 +42,7 @@ double calcReedsSheppDistance( void setYaw(geometry_msgs::msg::Quaternion * orientation, const double yaw) { - *orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + *orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); } geometry_msgs::msg::Pose calcRelativePose( @@ -86,8 +86,9 @@ AstarSearch::TransitionTable createTransitionTable( for (int i = 0; i < turning_radius_size; ++i) { double R = R_min + i * dR; double step = R * dtheta; - const NodeUpdate forward_left{ - R * sin(dtheta), R * (1 - cos(dtheta)), dtheta, step, true, false}; + const double shift_x = R * sin(dtheta); + const double shift_y = R * (1 - cos(dtheta)); + const NodeUpdate forward_left{shift_x, shift_y, dtheta, step, true, false}; const NodeUpdate forward_right = forward_left.flipped(); forward_node_candidates.push_back(forward_left); forward_node_candidates.push_back(forward_right); @@ -209,7 +210,7 @@ double AstarSearch::estimateCost(const geometry_msgs::msg::Pose & pose) const total_cost += calcReedsSheppDistance(pose, goal_pose_, radius) * astar_param_.distance_heuristic_weight; } else { - total_cost += tier4_autoware_utils::calcDistance2d(pose, goal_pose_) * + total_cost += autoware::universe_utils::calcDistance2d(pose, goal_pose_) * astar_param_.distance_heuristic_weight; } return total_cost; @@ -332,7 +333,7 @@ bool AstarSearch::isGoal(const AstarNode & node) const const double lateral_goal_range = planner_common_param_.lateral_goal_range / 2.0; const double longitudinal_goal_range = planner_common_param_.longitudinal_goal_range / 2.0; const double goal_angle = - tier4_autoware_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); + autoware::universe_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); const auto relative_pose = calcRelativePose(goal_pose_, node2pose(node)); @@ -348,7 +349,7 @@ bool AstarSearch::isGoal(const AstarNode & node) const } const auto angle_diff = - tier4_autoware_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); + autoware::universe_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); if (std::abs(angle_diff) > goal_angle) { return false; } @@ -363,7 +364,7 @@ geometry_msgs::msg::Pose AstarSearch::node2pose(const AstarNode & node) const pose_local.position.x = node.x; pose_local.position.y = node.y; pose_local.position.z = goal_pose_.position.z; - pose_local.orientation = tier4_autoware_utils::createQuaternionFromYaw(node.theta); + pose_local.orientation = autoware::universe_utils::createQuaternionFromYaw(node.theta); return pose_local; } diff --git a/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp b/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp index e8e846dde5681..82d8a5abcee8c 100644 --- a/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp @@ -78,7 +78,7 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include "autoware_freespace_planning_algorithms/reeds_shepp.hpp" +#include "autoware/freespace_planning_algorithms/reeds_shepp.hpp" #include #include diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp index 587771c35d4df..6b0018c517d4a 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_freespace_planning_algorithms/rrtstar.hpp" +#include "autoware/freespace_planning_algorithms/rrtstar.hpp" namespace autoware::freespace_planning_algorithms { diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp index 8f6405bab80cf..7403afd37e295 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_freespace_planning_algorithms/rrtstar_core.hpp" +#include "autoware/freespace_planning_algorithms/rrtstar_core.hpp" #include diff --git a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 5cfb004a3b1c3..79898ed57327b 100644 --- a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" -#include "autoware_freespace_planning_algorithms/astar_search.hpp" -#include "autoware_freespace_planning_algorithms/rrtstar.hpp" +#include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware/freespace_planning_algorithms/astar_search.hpp" +#include "autoware/freespace_planning_algorithms/rrtstar.hpp" #include #include diff --git a/planning/autoware_freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp index e08afdb236192..40181884d36c7 100644 --- a/planning/autoware_freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp +++ b/planning/autoware_freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_freespace_planning_algorithms/rrtstar_core.hpp" +#include "autoware/freespace_planning_algorithms/rrtstar_core.hpp" #include diff --git a/planning/autoware_mission_planner/include/mission_planner/mission_planner_plugin.hpp b/planning/autoware_mission_planner/include/autoware/mission_planner/mission_planner_plugin.hpp similarity index 89% rename from planning/autoware_mission_planner/include/mission_planner/mission_planner_plugin.hpp rename to planning/autoware_mission_planner/include/autoware/mission_planner/mission_planner_plugin.hpp index 5933d8879a5f6..837ea15a55486 100644 --- a/planning/autoware_mission_planner/include/mission_planner/mission_planner_plugin.hpp +++ b/planning/autoware_mission_planner/include/autoware/mission_planner/mission_planner_plugin.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MISSION_PLANNER__MISSION_PLANNER_PLUGIN_HPP_ -#define MISSION_PLANNER__MISSION_PLANNER_PLUGIN_HPP_ +#ifndef AUTOWARE__MISSION_PLANNER__MISSION_PLANNER_PLUGIN_HPP_ +#define AUTOWARE__MISSION_PLANNER__MISSION_PLANNER_PLUGIN_HPP_ #include @@ -47,4 +47,4 @@ class PlannerPlugin } // namespace autoware::mission_planner -#endif // MISSION_PLANNER__MISSION_PLANNER_PLUGIN_HPP_ +#endif // AUTOWARE__MISSION_PLANNER__MISSION_PLANNER_PLUGIN_HPP_ diff --git a/planning/autoware_mission_planner/package.xml b/planning/autoware_mission_planner/package.xml index 0a0491e75dd95..deaf9b8e942a2 100644 --- a/planning/autoware_mission_planner/package.xml +++ b/planning/autoware_mission_planner/package.xml @@ -19,18 +19,18 @@ autoware_adapi_v1_msgs autoware_map_msgs + autoware_motion_utils autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs lanelet2_extension - motion_utils pluginlib rclcpp rclcpp_components tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs ament_lint_auto diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp index a1ef66f5757f5..69c52b9c2eafb 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -16,16 +16,16 @@ #include "utility_functions.hpp" +#include +#include +#include +#include +#include #include #include #include #include #include -#include -#include -#include -#include -#include #include #include @@ -39,20 +39,6 @@ namespace { using RouteSections = std::vector; -RouteSections combine_consecutive_route_sections( - const RouteSections & route_sections1, const RouteSections & route_sections2) -{ - RouteSections route_sections; - route_sections.reserve(route_sections1.size() + route_sections2.size()); - if (!route_sections1.empty()) { - // remove end route section because it is overlapping with first one in next route_section - route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1); - } - if (!route_sections2.empty()) { - route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end()); - } - return route_sections; -} bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point) { @@ -122,8 +108,8 @@ geometry_msgs::msg::Pose get_closest_centerline_pose( const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, point.position); - const auto nearest_idx = - motion_utils::findNearestIndex(convertCenterlineToPoints(closest_lanelet), point.position); + const auto nearest_idx = autoware::motion_utils::findNearestIndex( + convertCenterlineToPoints(closest_lanelet), point.position); const auto nearest_point = closest_lanelet.centerline()[nearest_idx]; // shift nearest point on its local y axis so that vehicle's right and left edges @@ -204,13 +190,13 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) } const std_msgs::msg::ColorRGBA cl_route = - tier4_autoware_utils::createMarkerColor(0.8, 0.99, 0.8, 0.15); + autoware::universe_utils::createMarkerColor(0.8, 0.99, 0.8, 0.15); const std_msgs::msg::ColorRGBA cl_ll_borders = - tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); + autoware::universe_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); const std_msgs::msg::ColorRGBA cl_end = - tier4_autoware_utils::createMarkerColor(0.2, 0.2, 0.4, 0.05); + autoware::universe_utils::createMarkerColor(0.2, 0.2, 0.4, 0.05); const std_msgs::msg::ColorRGBA cl_goal = - tier4_autoware_utils::createMarkerColor(0.2, 0.4, 0.4, 0.05); + autoware::universe_utils::createMarkerColor(0.2, 0.4, 0.4, 0.05); visualization_msgs::msg::MarkerArray route_marker_array; insert_marker_array( @@ -230,27 +216,27 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) } visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( - tier4_autoware_utils::LinearRing2d goal_footprint) const + autoware::universe_utils::LinearRing2d goal_footprint) const { visualization_msgs::msg::MarkerArray msg; - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock().now(), "goal_footprint", 0, visualization_msgs::msg::Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0)); + autoware::universe_utils::createMarkerScale(0.05, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0)); marker.lifetime = rclcpp::Duration::from_seconds(2.5); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0)); + autoware::universe_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0)); marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); @@ -261,7 +247,7 @@ visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( bool DefaultPlanner::check_goal_footprint_inside_lanes( const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelet & combined_prev_lanelet, - const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length, + const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length, const double search_margin) { // check if goal footprint is in current lane @@ -316,8 +302,8 @@ bool DefaultPlanner::is_goal_valid( shoulder_lanelets, goal, &closest_shoulder_lanelet)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); + const auto angle_diff = autoware::universe_utils::normalizeRadian(lane_yaw - goal_yaw); + const double th_angle = autoware::universe_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } @@ -346,8 +332,8 @@ bool DefaultPlanner::is_goal_valid( } const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - tier4_autoware_utils::LinearRing2d goal_footprint = - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(goal)); + autoware::universe_utils::LinearRing2d goal_footprint = + transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(goal)); pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint)); const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint); @@ -371,9 +357,9 @@ bool DefaultPlanner::is_goal_valid( if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); + const auto angle_diff = autoware::universe_utils::normalizeRadian(lane_yaw - goal_yaw); - const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); + const double th_angle = autoware::universe_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } @@ -422,15 +408,14 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) RCLCPP_WARN(logger, "Failed to plan route."); return route_msg; } + for (const auto & lane : path_lanelets) { + if (!all_route_lanelets.empty() && lane.id() == all_route_lanelets.back().id()) continue; all_route_lanelets.push_back(lane); } - // create local route sections - route_handler_.setRouteLanelets(path_lanelets); - const auto local_route_sections = route_handler_.createMapSegments(path_lanelets); - route_sections = combine_consecutive_route_sections(route_sections, local_route_sections); } route_handler_.setRouteLanelets(all_route_lanelets); + route_sections = route_handler_.createMapSegments(all_route_lanelets); auto goal_pose = points.back(); if (param_.enable_correct_goal_pose) { diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp index cf69475e89869..60c2a53b82123 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -15,9 +15,9 @@ #ifndef LANELET2_PLUGINS__DEFAULT_PLANNER_HPP_ #define LANELET2_PLUGINS__DEFAULT_PLANNER_HPP_ -#include +#include +#include #include -#include #include #include @@ -51,7 +51,8 @@ class DefaultPlanner : public mission_planner::PlannerPlugin void updateRoute(const PlannerPlugin::LaneletRoute & route) override; void clearRoute() override; MarkerArray visualize(const LaneletRoute & route) const override; - MarkerArray visualize_debug_footprint(tier4_autoware_utils::LinearRing2d goal_footprint_) const; + MarkerArray visualize_debug_footprint( + autoware::universe_utils::LinearRing2d goal_footprint_) const; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; private: @@ -85,7 +86,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin bool check_goal_footprint_inside_lanes( const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelet & combined_prev_lanelet, - const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length, + const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length, const double search_margin = 2.0); /** diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp index 1f3b2077cc5eb..5046b81c72b96 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -21,10 +21,10 @@ #include #include -tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon( - tier4_autoware_utils::LinearRing2d footprint) +autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( + autoware::universe_utils::LinearRing2d footprint) { - tier4_autoware_utils::Polygon2d footprint_polygon; + autoware::universe_utils::Polygon2d footprint_polygon; boost::geometry::append(footprint_polygon.outer(), footprint[0]); boost::geometry::append(footprint_polygon.outer(), footprint[1]); boost::geometry::append(footprint_polygon.outer(), footprint[2]); @@ -120,7 +120,7 @@ geometry_msgs::msg::Pose convertBasicPoint3dToPose( pose.position.y = point.y(); pose.position.z = point.z(); - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); + pose.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); return pose; } diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp index c3bed03ac6512..c8812e2c76dd6 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -15,10 +15,10 @@ #ifndef LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ -#include +#include +#include #include #include -#include #include #include @@ -42,8 +42,8 @@ bool exists(const std::vector & vectors, const T & item) return false; } -tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon( - tier4_autoware_utils::LinearRing2d footprint); +autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( + autoware::universe_utils::LinearRing2d footprint); void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); diff --git a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp b/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp index 54285bd1491f9..da14712dd6c64 100644 --- a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp @@ -14,9 +14,9 @@ #include "arrival_checker.hpp" -#include -#include -#include +#include +#include +#include #include @@ -26,7 +26,7 @@ namespace autoware::mission_planner ArrivalChecker::ArrivalChecker(rclcpp::Node * node) : vehicle_stop_checker_(node) { const double angle_deg = node->declare_parameter("arrival_check_angle_deg"); - angle_ = tier4_autoware_utils::deg2rad(angle_deg); + angle_ = autoware::universe_utils::deg2rad(angle_deg); distance_ = node->declare_parameter("arrival_check_distance"); duration_ = node->declare_parameter("arrival_check_duration"); } @@ -67,14 +67,14 @@ bool ArrivalChecker::is_arrived(const PoseStamped & pose) const } // Check distance. - if (distance_ < tier4_autoware_utils::calcDistance2d(pose.pose, goal.pose)) { + if (distance_ < autoware::universe_utils::calcDistance2d(pose.pose, goal.pose)) { return false; } // Check angle. const double yaw_pose = tf2::getYaw(pose.pose.orientation); const double yaw_goal = tf2::getYaw(goal.pose.orientation); - const double yaw_diff = tier4_autoware_utils::normalizeRadian(yaw_pose - yaw_goal); + const double yaw_diff = autoware::universe_utils::normalizeRadian(yaw_pose - yaw_goal); if (angle_ < std::fabs(yaw_diff)) { return false; } diff --git a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp b/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp index 55790702d5ef1..708b3de40bdab 100644 --- a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp @@ -15,7 +15,7 @@ #ifndef MISSION_PLANNER__ARRIVAL_CHECKER_HPP_ #define MISSION_PLANNER__ARRIVAL_CHECKER_HPP_ -#include +#include #include #include @@ -42,7 +42,7 @@ class ArrivalChecker double duration_; std::optional goal_with_uuid_; rclcpp::Subscription::SharedPtr sub_goal_; - motion_utils::VehicleStopChecker vehicle_stop_checker_; + autoware::motion_utils::VehicleStopChecker vehicle_stop_checker_; }; } // namespace autoware::mission_planner diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp index 52f6e4ca191f5..e484b9915aa87 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp @@ -52,18 +52,11 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) plugin_loader_.createSharedInstance("autoware::mission_planner::lanelet2::DefaultPlanner"); planner_->initialize(this); - const auto reroute_availability = std::make_shared(); - reroute_availability->availability = false; - reroute_availability_ = reroute_availability; - const auto durable_qos = rclcpp::QoS(1).transient_local(); sub_odometry_ = create_subscription( "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1)); sub_vector_map_ = create_subscription( "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1)); - sub_reroute_availability_ = create_subscription( - "~/input/reroute_availability", rclcpp::QoS(1), - std::bind(&MissionPlanner::on_reroute_availability, this, _1)); pub_marker_ = create_publisher("~/debug/route_marker", durable_qos); // NOTE: The route interface should be mutually exclusive by callback group. @@ -85,7 +78,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) const auto period = rclcpp::Rate(10).period(); data_check_timer_ = create_wall_timer(period, [this] { check_initialization(); }); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } void MissionPlanner::publish_pose_log(const Pose & pose, const std::string & pose_type) @@ -137,11 +130,6 @@ void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) } } -void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg) -{ - reroute_availability_ = msg; -} - void MissionPlanner::on_map(const LaneletMapBin::ConstSharedPtr msg) { map_ptr_ = msg; @@ -234,7 +222,8 @@ void MissionPlanner::on_set_lanelet_route( throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - if (is_reroute && !reroute_availability_->availability) { + const auto reroute_availability = sub_reroute_availability_.takeData(); + if (is_reroute && (!reroute_availability || !reroute_availability->availability)) { throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); } @@ -282,7 +271,8 @@ void MissionPlanner::on_set_waypoint_route( throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - if (is_reroute && !reroute_availability_->availability) { + const auto reroute_availability = sub_reroute_availability_.takeData(); + if (is_reroute && (!reroute_availability || !reroute_availability->availability)) { throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); } diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp index 9f3dc7d98c41c..1a04a91c14ba3 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -16,12 +16,13 @@ #define MISSION_PLANNER__MISSION_PLANNER_HPP_ #include "arrival_checker.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include -#include +#include +#include +#include #include #include -#include #include #include @@ -84,13 +85,13 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; + autoware::universe_utils::InterProcessPollingSubscriber + sub_reroute_availability_{this, "~/input/reroute_availability"}; + rclcpp::Subscription::SharedPtr sub_vector_map_; - rclcpp::Subscription::SharedPtr sub_reroute_availability_; rclcpp::Publisher::SharedPtr pub_marker_; - Odometry::ConstSharedPtr odometry_; LaneletMapBin::ConstSharedPtr map_ptr_; - RerouteAvailability::ConstSharedPtr reroute_availability_; RouteState state_; LaneletRoute::ConstSharedPtr current_route_; lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr}; @@ -131,7 +132,7 @@ class MissionPlanner : public rclcpp::Node double minimum_reroute_length_; bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; }; } // namespace autoware::mission_planner diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/coloring.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp similarity index 74% rename from planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/coloring.hpp rename to planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp index 77e0e5db2f157..e228b34cb9f5d 100644 --- a/planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/coloring.hpp +++ b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ -#define AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ -#include "autoware_objects_of_interest_marker_interface/marker_data.hpp" +#ifndef AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ +#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ +#include "autoware/objects_of_interest_marker_interface/marker_data.hpp" -#include +#include #include @@ -28,4 +28,4 @@ std_msgs::msg::ColorRGBA getRed(const float alpha); std_msgs::msg::ColorRGBA getGray(const float alpha); } // namespace autoware::objects_of_interest_marker_interface::coloring -#endif // AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ +#endif // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/marker_data.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp similarity index 82% rename from planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/marker_data.hpp rename to planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp index 1ce530343172e..53a5cb091d1ea 100644 --- a/planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/marker_data.hpp +++ b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ -#define AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ +#ifndef AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ +#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ #include #include @@ -31,4 +31,4 @@ struct ObjectMarkerData enum class ColorName { GRAY, GREEN, AMBER, RED }; } // namespace autoware::objects_of_interest_marker_interface -#endif // AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ +#endif // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/marker_utils.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp similarity index 81% rename from planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/marker_utils.hpp rename to planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp index 5c41e7d33e8b2..ace32e91287a4 100644 --- a/planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/marker_utils.hpp +++ b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ -#define AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ -#include "autoware_objects_of_interest_marker_interface/coloring.hpp" -#include "autoware_objects_of_interest_marker_interface/marker_data.hpp" +#ifndef AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ +#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ +#include "autoware/objects_of_interest_marker_interface/coloring.hpp" +#include "autoware/objects_of_interest_marker_interface/marker_data.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -83,4 +83,4 @@ visualization_msgs::msg::MarkerArray createTargetMarker( const double height_offset, const double arrow_length = 1.0, const double line_width = 0.1); } // namespace autoware::objects_of_interest_marker_interface::marker_utils -#endif // AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ +#endif // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp similarity index 85% rename from planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp rename to planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp index dfb62198e1607..619df5f719df9 100644 --- a/planning/autoware_objects_of_interest_marker_interface/include/autoware_objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp +++ b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ -#define AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ -#include "autoware_objects_of_interest_marker_interface/coloring.hpp" -#include "autoware_objects_of_interest_marker_interface/marker_data.hpp" -#include "autoware_objects_of_interest_marker_interface/marker_utils.hpp" +#ifndef AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ +#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ +#include "autoware/objects_of_interest_marker_interface/coloring.hpp" +#include "autoware/objects_of_interest_marker_interface/marker_data.hpp" +#include "autoware/objects_of_interest_marker_interface/marker_utils.hpp" #include @@ -100,4 +100,4 @@ class ObjectsOfInterestMarkerInterface } // namespace autoware::objects_of_interest_marker_interface -#endif // AUTOWARE_OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ +#endif // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ diff --git a/planning/autoware_objects_of_interest_marker_interface/package.xml b/planning/autoware_objects_of_interest_marker_interface/package.xml index a7701d159e824..8b62865ddace5 100644 --- a/planning/autoware_objects_of_interest_marker_interface/package.xml +++ b/planning/autoware_objects_of_interest_marker_interface/package.xml @@ -15,10 +15,10 @@ ament_cmake_auto autoware_perception_msgs + autoware_universe_utils geometry_msgs rclcpp std_msgs - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp b/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp index 6bea6b577a61e..038006517f45a 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_objects_of_interest_marker_interface/coloring.hpp" +#include "autoware/objects_of_interest_marker_interface/coloring.hpp" namespace { @@ -22,7 +22,7 @@ std_msgs::msg::ColorRGBA convertFromColorCode(const uint64_t code, const float a const float g = static_cast((code << 48) >> 56) / 255.0; const float b = static_cast((code << 56) >> 56) / 255.0; - return tier4_autoware_utils::createMarkerColor(r, g, b, alpha); + return autoware::universe_utils::createMarkerColor(r, g, b, alpha); } } // namespace diff --git a/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp index 8c9478ab97a99..42d92bd0c91da 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_objects_of_interest_marker_interface/marker_utils.hpp" +#include "autoware/objects_of_interest_marker_interface/marker_utils.hpp" namespace autoware::objects_of_interest_marker_interface::marker_utils { @@ -20,8 +20,8 @@ using geometry_msgs::msg::Point; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerScale; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerScale; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -66,9 +66,9 @@ Marker createCircleMarker( for (size_t i = 0; i < num_points; ++i) { Point point; const double ratio = static_cast(i) / static_cast(num_points); - const double theta = 2 * tier4_autoware_utils::pi * ratio; - point.x = data.pose.position.x + radius * tier4_autoware_utils::cos(theta); - point.y = data.pose.position.y + radius * tier4_autoware_utils::sin(theta); + const double theta = 2 * autoware::universe_utils::pi * ratio; + point.x = data.pose.position.x + radius * autoware::universe_utils::cos(theta); + point.y = data.pose.position.y + radius * autoware::universe_utils::sin(theta); point.z = data.pose.position.z + height + height_offset; marker.points.push_back(point); } diff --git a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp index 42fea55a46cf2..253d2ec05c246 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp" +#include "autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp" -#include -#include -#include +#include +#include +#include namespace autoware::objects_of_interest_marker_interface { diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp similarity index 79% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/common_structs.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index 9b5c277589e67..e4ae670ef6b97 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ -#include "autoware_obstacle_cruise_planner/type_alias.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/ros/uuid_helper.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/uuid_helper.hpp" #include @@ -63,7 +63,7 @@ struct Obstacle twist(object.kinematics.initial_twist_with_covariance.twist), twist_reliable(true), classification(object.classification.at(0)), - uuid(tier4_autoware_utils::toHexString(object.object_id)), + uuid(autoware::universe_utils::toHexString(object.object_id)), shape(object.shape), ego_to_obstacle_distance(ego_to_obstacle_distance), lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) @@ -74,7 +74,7 @@ struct Obstacle } } - Polygon2d toPolygon() const { return tier4_autoware_utils::toPolygon2d(pose, shape); } + Polygon2d toPolygon() const { return autoware::universe_utils::toPolygon2d(pose, shape); } rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. geometry_msgs::msg::Pose pose; // interpolated with the current stamp @@ -198,33 +198,33 @@ struct LongitudinalInfo void onParam(const std::vector & parameters) { - tier4_autoware_utils::updateParam(parameters, "normal.max_accel", max_accel); - tier4_autoware_utils::updateParam(parameters, "normal.min_accel", min_accel); - tier4_autoware_utils::updateParam(parameters, "normal.max_jerk", max_jerk); - tier4_autoware_utils::updateParam(parameters, "normal.min_jerk", min_jerk); - tier4_autoware_utils::updateParam(parameters, "limit.max_accel", limit_max_accel); - tier4_autoware_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); - tier4_autoware_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); - tier4_autoware_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam(parameters, "normal.max_accel", max_accel); + autoware::universe_utils::updateParam(parameters, "normal.min_accel", min_accel); + autoware::universe_utils::updateParam(parameters, "normal.max_jerk", max_jerk); + autoware::universe_utils::updateParam(parameters, "normal.min_jerk", min_jerk); + autoware::universe_utils::updateParam(parameters, "limit.max_accel", limit_max_accel); + autoware::universe_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); + autoware::universe_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); + autoware::universe_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); + autoware::universe_utils::updateParam( parameters, "common.slow_down_min_accel", slow_down_min_accel); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.slow_down_min_jerk", slow_down_min_jerk); - tier4_autoware_utils::updateParam(parameters, "common.idling_time", idling_time); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam(parameters, "common.idling_time", idling_time); + autoware::universe_utils::updateParam( parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.safe_distance_margin", safe_distance_margin); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.hold_stop_velocity_threshold", hold_stop_velocity_threshold); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.hold_stop_distance_threshold", hold_stop_distance_threshold); } @@ -265,7 +265,7 @@ struct DebugData MarkerArray stop_wall_marker; MarkerArray cruise_wall_marker; MarkerArray slow_down_wall_marker; - std::vector detection_polygons; + std::vector detection_polygons; }; struct EgoNearestParam @@ -280,21 +280,22 @@ struct EgoNearestParam TrajectoryPoint calcInterpolatedPoint( const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const { - return motion_utils::calcInterpolatedPoint( - motion_utils::convertToTrajectory(traj_points), pose, dist_threshold, yaw_threshold); + return autoware::motion_utils::calcInterpolatedPoint( + autoware::motion_utils::convertToTrajectory(traj_points), pose, dist_threshold, + yaw_threshold); } size_t findIndex( const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const { - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( traj_points, pose, dist_threshold, yaw_threshold); } size_t findSegmentIndex( const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const { - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( traj_points, pose, dist_threshold, yaw_threshold); } @@ -302,4 +303,4 @@ struct EgoNearestParam double yaw_threshold; }; -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp similarity index 90% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/node.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index dcea7e1dc257b..dd9a6c3880672 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__NODE_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__NODE_HPP_ - -#include "autoware_obstacle_cruise_planner/common_structs.hpp" -#include "autoware_obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" -#include "autoware_obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" -#include "autoware_obstacle_cruise_planner/type_alias.hpp" +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__NODE_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__NODE_HPP_ + +#include "autoware/obstacle_cruise_planner/common_structs.hpp" +#include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" +#include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "signal_processing/lowpass_filter_1d.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include #include -#include #include #include @@ -145,11 +145,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr traj_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber ego_odom_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber ego_odom_sub_{ this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber objects_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; - tier4_autoware_utils::InterProcessPollingSubscriber acc_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber acc_sub_{ this, "~/input/acceleration"}; // Vehicle Parameters @@ -161,7 +161,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node PlanningAlgorithm planning_algorithm_; // stop watch - mutable tier4_autoware_utils::StopWatch< + mutable autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; mutable std::shared_ptr debug_data_ptr_{nullptr}; @@ -274,10 +274,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::vector prev_closest_stop_obstacles_{}; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_planning -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__NODE_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__NODE_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp similarity index 88% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index db807608caca4..f08ff8fef369c 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT -#include "autoware_obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" -#include "autoware_obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" -#include "autoware_obstacle_cruise_planner/planner_interface.hpp" -#include "autoware_obstacle_cruise_planner/type_alias.hpp" +#include "autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" +#include "autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" +#include "autoware/obstacle_cruise_planner/planner_interface.hpp" +#include "autoware/obstacle_cruise_planner/type_alias.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include @@ -118,5 +118,5 @@ class OptimizationBasedPlanner : public PlannerInterface double stop_dist_to_prohibit_engage_; }; // clang-format off -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT // clang-format on diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp similarity index 78% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp index d033e4e35aef0..4224f35fbc6e1 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ #include #include @@ -30,4 +30,4 @@ class SBoundary using SBoundaries = std::vector; -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp similarity index 84% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp index 49cf96ce81e0b..a2ac86ee6c86b 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp @@ -11,10 +11,10 @@ // 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 AUTOWARE_OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ -#include "autoware_obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" +#include "autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" #include "osqp_interface/osqp_interface.hpp" #include @@ -72,4 +72,4 @@ class VelocityOptimizer autoware::common::osqp::OSQPInterface qp_solver_; }; -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp similarity index 90% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp index 98679b5cbb515..f2269aeaa2967 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ #include @@ -94,4 +94,4 @@ class CruisePlanningDebugInfo std::array(TYPE::SIZE)> info_; }; -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp similarity index 91% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index 5fac88e909764..c048c3278e2ec 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ -#include "autoware_obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp" -#include "autoware_obstacle_cruise_planner/pid_based_planner/pid_controller.hpp" -#include "autoware_obstacle_cruise_planner/planner_interface.hpp" +#include "autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp" +#include "autoware/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp" +#include "autoware/obstacle_cruise_planner/planner_interface.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -137,4 +137,4 @@ class PIDBasedPlanner : public PlannerInterface std::function error_func_; }; -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/pid_based_planner/pid_controller.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp similarity index 85% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/pid_based_planner/pid_controller.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp index f8f7a483df2a1..c5d93b30fa6e0 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/pid_based_planner/pid_controller.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ #include @@ -59,4 +59,4 @@ class PIDController std::optional prev_error_; }; -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp similarity index 92% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/planner_interface.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index 2685b3d061ff1..0641d076bb3ff 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ -#include "autoware_obstacle_cruise_planner/common_structs.hpp" -#include "autoware_obstacle_cruise_planner/stop_planning_debug_info.hpp" -#include "autoware_obstacle_cruise_planner/type_alias.hpp" -#include "autoware_obstacle_cruise_planner/utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/obstacle_cruise_planner/common_structs.hpp" +#include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp" +#include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -125,7 +125,7 @@ class PlannerInterface bool suppress_sudden_obstacle_stop_; // stop watch - tier4_autoware_utils::StopWatch< + autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; @@ -172,7 +172,7 @@ class PlannerInterface const geometry_msgs::msg::Pose & ego_pose) const { const auto & p = ego_nearest_param_; - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( traj_points, ego_pose, p.dist_threshold, p.yaw_threshold); } @@ -181,7 +181,7 @@ class PlannerInterface const geometry_msgs::msg::Pose & ego_pose) const { const auto & p = ego_nearest_param_; - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( traj_points, ego_pose, p.dist_threshold, p.yaw_threshold); } @@ -303,29 +303,29 @@ class PlannerInterface if (obstacle_to_param_struct_map.count(label + "." + movement_postfix) < 1) continue; auto & param_by_obstacle_label = obstacle_to_param_struct_map.at(label + "." + movement_postfix); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".max_lat_margin", param_by_obstacle_label.max_lat_margin); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".min_lat_margin", param_by_obstacle_label.min_lat_margin); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".max_ego_velocity", param_by_obstacle_label.max_ego_velocity); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down." + label + "." + movement_postfix + ".min_ego_velocity", param_by_obstacle_label.min_ego_velocity); } } // common parameters - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down.lpf_gain_slow_down_vel", lpf_gain_slow_down_vel); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down.lpf_gain_lat_dist", lpf_gain_lat_dist); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down); } @@ -385,15 +385,15 @@ class PlannerInterface if (type_str == "default") { continue; } - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, param_prefix + type_str + ".sudden_object_acc_threshold", param.sudden_object_acc_threshold); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, param_prefix + type_str + ".sudden_object_dist_threshold", param.sudden_object_dist_threshold); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop); param.sudden_object_acc_threshold = @@ -425,4 +425,4 @@ class PlannerInterface std::nullopt}; }; -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/polygon_utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp similarity index 80% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/polygon_utils.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp index 621e4decbbf1e..fa6ee17d117be 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ -#include "autoware_obstacle_cruise_planner/common_structs.hpp" -#include "autoware_obstacle_cruise_planner/type_alias.hpp" +#include "autoware/obstacle_cruise_planner/common_structs.hpp" +#include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include @@ -30,8 +30,8 @@ namespace polygon_utils { namespace bg = boost::geometry; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; Polygon2d createOneStepPolygon( const std::vector & last_poses, @@ -52,4 +52,4 @@ std::vector getCollisionPoints( } // namespace polygon_utils -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/stop_planning_debug_info.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp similarity index 91% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/stop_planning_debug_info.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp index 7167ddbdf6ad1..ef4f5d0eba808 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/stop_planning_debug_info.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ #include @@ -85,4 +85,4 @@ class StopPlanningDebugInfo std::array(TYPE::SIZE)> info_; }; -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp similarity index 91% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/type_alias.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index 7b435acb5f13e..5721efd051f63 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" @@ -60,7 +60,7 @@ using tier4_planning_msgs::msg::VelocityLimitClearCommand; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; namespace bg = boost::geometry; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp similarity index 85% rename from planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/utils.hpp rename to planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp index 3b8093aea76a1..8af9a63f3a1fa 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ -#define AUTOWARE_OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ -#include "autoware_obstacle_cruise_planner/type_alias.hpp" +#include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "common_structs.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -62,7 +62,7 @@ size_t getIndexWithLongitudinalOffset( if (longitudinal_offset > 0) { for (size_t i = *start_idx; i < points.size() - 1; ++i) { const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); sum_length += segment_length; if (sum_length >= longitudinal_offset) { const double back_length = sum_length - longitudinal_offset; @@ -79,7 +79,7 @@ size_t getIndexWithLongitudinalOffset( for (size_t i = *start_idx; 0 < i; --i) { const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)); + autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)); sum_length += segment_length; if (sum_length >= -longitudinal_offset) { const double back_length = sum_length + longitudinal_offset; @@ -95,4 +95,4 @@ size_t getIndexWithLongitudinalOffset( } } // namespace obstacle_cruise_utils -#endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 2d3a96abfd81d..6055327a906e1 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -18,14 +18,15 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation lanelet2_extension - motion_utils nav_msgs object_recognition_utils osqp_interface @@ -35,7 +36,6 @@ std_msgs tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 18ea1243d9101..f6e7c2bb43b38 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_obstacle_cruise_planner/node.hpp" - -#include "autoware_obstacle_cruise_planner/polygon_utils.hpp" -#include "autoware_obstacle_cruise_planner/utils.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" +#include "autoware/obstacle_cruise_planner/node.hpp" + +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/obstacle_cruise_planner/polygon_utils.hpp" +#include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -58,9 +58,9 @@ std::optional calcDistanceToFrontVehicle( const std::vector & traj_points, const size_t ego_idx, const geometry_msgs::msg::Point & obstacle_pos) { - const size_t obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle_pos); + const size_t obstacle_idx = autoware::motion_utils::findNearestIndex(traj_points, obstacle_pos); const auto ego_to_obstacle_distance = - motion_utils::calcSignedArcLength(traj_points, ego_idx, obstacle_idx); + autoware::motion_utils::calcSignedArcLength(traj_points, ego_idx, obstacle_idx); if (ego_to_obstacle_distance < 0.0) return std::nullopt; return ego_to_obstacle_distance; } @@ -82,19 +82,21 @@ PredictedPath resampleHighestConfidencePredictedPath( double calcDiffAngleAgainstTrajectory( const std::vector & traj_points, const geometry_msgs::msg::Pose & target_pose) { - const size_t nearest_idx = motion_utils::findNearestIndex(traj_points, target_pose.position); + const size_t nearest_idx = + autoware::motion_utils::findNearestIndex(traj_points, target_pose.position); const double traj_yaw = tf2::getYaw(traj_points.at(nearest_idx).pose.orientation); const double target_yaw = tf2::getYaw(target_pose.orientation); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(target_yaw - traj_yaw); + const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); return diff_yaw; } std::pair projectObstacleVelocityToTrajectory( const std::vector & traj_points, const Obstacle & obstacle) { - const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); + const size_t object_idx = + autoware::motion_utils::findNearestIndex(traj_points, obstacle.pose.position); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); const double obstacle_yaw = tf2::getYaw(obstacle.pose.orientation); @@ -130,7 +132,7 @@ TrajectoryPoint getExtendTrajectoryPoint( const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) { TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = tier4_autoware_utils::calcOffsetPose( + extend_trajectory_point.pose = autoware::universe_utils::calcOffsetPose( goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; @@ -143,7 +145,8 @@ std::vector extendTrajectoryPoints( const double step_length) { auto output_points = input_points; - const auto is_driving_forward_opt = motion_utils::isDrivingForwardWithTwist(input_points); + const auto is_driving_forward_opt = + autoware::motion_utils::isDrivingForwardWithTwist(input_points); const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (extend_distance < std::numeric_limits::epsilon()) { @@ -186,12 +189,12 @@ std::vector getTargetObjectType(rclcpp::Node & node, const std::string & pa std::vector resampleTrajectoryPoints( const std::vector & traj_points, const double interval) { - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory(traj, interval); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); } -geometry_msgs::msg::Point toGeomPoint(const tier4_autoware_utils::Point2d & point) +geometry_msgs::msg::Point toGeomPoint(const autoware::universe_utils::Point2d & point) { geometry_msgs::msg::Point geom_point; geom_point.x = point.x(); @@ -278,75 +281,75 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( const std::vector & parameters) { // behavior determination - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.decimate_trajectory_step_length", decimate_trajectory_step_length); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.crossing_obstacle.obstacle_velocity_threshold", crossing_obstacle_velocity_threshold); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold", crossing_obstacle_traj_angle_threshold); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.stop.crossing_obstacle.collision_time_margin", collision_time_margin); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold", outside_obstacle_min_velocity_threshold); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold", ego_obstacle_overlap_time_threshold); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.outside_obstacle.max_prediction_time_for_collision_check", max_prediction_time_for_collision_check); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.stop_obstacle_hold_time_threshold", stop_obstacle_hold_time_threshold); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.prediction_resampling_time_interval", prediction_resampling_time_interval); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.prediction_resampling_time_horizon", prediction_resampling_time_horizon); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.stop.max_lat_margin_against_unknown", max_lat_margin_for_stop_against_unknown); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.enable_yield", enable_yield); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.lat_distance_threshold", yield_lat_distance_threshold); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.max_lat_dist_between_obstacles", max_lat_dist_between_obstacles); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold", stopped_obstacle_velocity_threshold); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.cruise.yield.max_obstacles_collision_time", max_obstacles_collision_time); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.slow_down.max_lat_margin", max_lat_margin_for_slow_down); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.slow_down.lat_hysteresis_margin", lat_hysteresis_margin_for_slow_down); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition", successive_num_to_entry_slow_down_condition); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition", successive_num_to_exit_slow_down_condition); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose", enable_to_consider_current_pose); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "behavior_determination.consider_current_pose.time_to_convergence", time_to_convergence); } @@ -406,7 +409,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & planner_ptr_ = std::make_unique( *this, longitudinal_info, vehicle_info_, ego_nearest_param_, debug_data_ptr_); } else { - std::logic_error("Designated algorithm is not supported."); + throw std::logic_error("Designated algorithm is not supported."); } min_behavior_stop_margin_ = declare_parameter("common.min_behavior_stop_margin"); @@ -437,8 +440,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( @@ -457,17 +461,17 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( { planner_ptr_->onParam(parameters); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.enable_debug_info", enable_debug_info_); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.stop_on_curve.additional_safe_distance_margin", additional_safe_distance_margin_on_curve_); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.stop_on_curve.min_safe_distance_margin", min_safe_distance_margin_on_curve_); @@ -476,7 +480,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); behavior_determination_param_.onParam(parameters); @@ -500,7 +504,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto & objects = *objects_ptr; const auto & acc = *acc_ptr; - const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg); + const auto traj_points = autoware::motion_utils::convertToTrajectoryPointArray(*msg); // check if subscribed variables are ready if (traj_points.empty()) { return; @@ -509,7 +513,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms stop_watch_.tic(__func__); *debug_data_ptr_ = DebugData(); - const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(traj_points); + const auto is_driving_forward = autoware::motion_utils::isDrivingForwardWithTwist(traj_points); is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; // 1. Convert predicted objects to obstacles which are @@ -541,7 +545,8 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms publishVelocityLimit(slow_down_vel_limit, "slow_down"); // 7. Publish trajectory - const auto output_traj = motion_utils::convertToTrajectory(slow_down_traj_points, msg->header); + const auto output_traj = + autoware::motion_utils::convertToTrajectory(slow_down_traj_points, msg->header); trajectory_pub_->publish(output_traj); // 8. Publish debug data @@ -568,10 +573,10 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( const double vehicle_width = vehicle_info.vehicle_width_m; const size_t nearest_idx = - motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); + autoware::motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); const auto nearest_pose = traj_points.at(nearest_idx).pose; const auto current_ego_pose_error = - tier4_autoware_utils::inverseTransformPose(current_ego_pose, nearest_pose); + autoware::universe_utils::inverseTransformPose(current_ego_pose, nearest_pose); const double current_ego_lat_error = current_ego_pose_error.position.y; const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation); double time_elapsed{0.0}; @@ -587,11 +592,11 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( const double rem_ratio = (p.time_to_convergence - time_elapsed) / p.time_to_convergence; geometry_msgs::msg::Pose indexed_pose_err; indexed_pose_err.set__orientation( - tier4_autoware_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); + autoware::universe_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); indexed_pose_err.set__position( - tier4_autoware_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); + autoware::universe_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); current_poses.push_back( - tier4_autoware_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); + autoware::universe_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { time_elapsed += p.decimate_trajectory_step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps); @@ -605,23 +610,23 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) { boost::geometry::append( idx_poly, - tier4_autoware_utils::toFootprint(pose, front_length, rear_length, vehicle_width) + autoware::universe_utils::toFootprint(pose, front_length, rear_length, vehicle_width) .outer()); boost::geometry::append( - idx_poly, - tier4_autoware_utils::fromMsg(tier4_autoware_utils::calcOffsetPose( - pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) - .position) - .to_2d()); + idx_poly, autoware::universe_utils::fromMsg( + autoware::universe_utils::calcOffsetPose( + pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) + .position) + .to_2d()); boost::geometry::append( - idx_poly, tier4_autoware_utils::fromMsg( - tier4_autoware_utils::calcOffsetPose( + idx_poly, autoware::universe_utils::fromMsg( + autoware::universe_utils::calcOffsetPose( pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0) .position) .to_2d()); } else { boost::geometry::append( - idx_poly, tier4_autoware_utils::toFootprint( + idx_poly, autoware::universe_utils::toFootprint( pose, front_length, rear_length, vehicle_width + lat_margin * 2.0) .outer()); } @@ -650,7 +655,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( std::vector target_obstacles; for (const auto & predicted_object : objects.objects) { const auto & object_id = - tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4); + autoware::universe_utils::toHexString(predicted_object.object_id).substr(0, 4); const auto & current_obstacle_pose = obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); @@ -678,7 +683,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( // 3. Check if rough lateral distance is smaller than the threshold const double lat_dist_from_obstacle_to_traj = - motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); + autoware::motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); const double min_lat_dist_to_traj_poly = [&]() { const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape); @@ -743,10 +748,11 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle( const std::vector & traj_points, const Obstacle & obstacle, const size_t first_collision_idx) const { - const auto obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); + const auto obstacle_idx = + autoware::motion_utils::findNearestIndex(traj_points, obstacle.pose.position); const double obstacle_to_col_points_distance = - motion_utils::calcSignedArcLength(traj_points, obstacle_idx, first_collision_idx); + autoware::motion_utils::calcSignedArcLength(traj_points, obstacle_idx, first_collision_idx); const double obstacle_max_length = calcObstacleMaxLength(obstacle.shape); // If the obstacle is far in front of the collision point, the obstacle is behind the ego. @@ -938,7 +944,7 @@ std::optional ObstacleCruisePlannerNode::createYieldCruiseObstac } const auto collision_points = [&]() -> std::optional> { - const auto obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose); + const auto obstacle_idx = autoware::motion_utils::findNearestIndex(traj_points, obstacle.pose); if (!obstacle_idx) return std::nullopt; const auto collision_traj_point = traj_points.at(obstacle_idx.value()); const auto object_time = now() + traj_points.at(obstacle_idx.value()).time_from_start; @@ -1203,8 +1209,8 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, // and the collision between ego and obstacles are within the margin threshold. const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); - const double has_high_speed = p.crossing_obstacle_velocity_threshold < - std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); + const bool has_high_speed = p.crossing_obstacle_velocity_threshold < + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); if (is_obstacle_crossing && has_high_speed) { // Get highest confidence predicted path const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( @@ -1352,7 +1358,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl for (const auto & collision_poly : front_collision_polygons) { for (const auto & collision_point : collision_poly.outer()) { const auto collision_geom_point = toGeomPoint(collision_point); - const double dist = motion_utils::calcLongitudinalOffsetToSegment( + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, front_seg_idx, collision_geom_point); if (dist < front_min_dist) { front_min_dist = dist; @@ -1367,7 +1373,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl for (const auto & collision_poly : back_collision_polygons) { for (const auto & collision_point : collision_poly.outer()) { const auto collision_geom_point = toGeomPoint(collision_point); - const double dist = motion_utils::calcLongitudinalOffsetToSegment( + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, back_seg_idx, collision_geom_point); if (back_max_dist < dist) { back_max_dist = dist; @@ -1390,7 +1396,8 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&prev_closest_stop_obstacle](const PredictedObject & po) { - return tier4_autoware_utils::toHexString(po.object_id) == prev_closest_stop_obstacle.uuid; + return autoware::universe_utils::toHexString(po.object_id) == + prev_closest_stop_obstacle.uuid; }); // If previous closest obstacle disappear from the perception result, do nothing anymore. if (predicted_object_itr == predicted_objects.objects.end()) { @@ -1449,7 +1456,7 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin( ? std::abs(vehicle_info_.max_longitudinal_offset_m) : std::abs(vehicle_info_.min_longitudinal_offset_m); const double dist_from_ego_to_obstacle = - std::abs(motion_utils::calcSignedArcLength( + std::abs(autoware::motion_utils::calcSignedArcLength( traj_points, ego_pose.position, collision_points.front().point)) - abs_ego_offset; return dist_from_ego_to_obstacle / std::max(1e-6, std::abs(ego_vel)); @@ -1524,10 +1531,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const } } for (size_t i = 0; i < stop_collision_points.size(); ++i) { - auto collision_point_marker = tier4_autoware_utils::createDefaultMarker( + auto collision_point_marker = autoware::universe_utils::createDefaultMarker( "map", now(), "cruise_collision_points", i, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); collision_point_marker.pose.position = stop_collision_points.at(i); debug_marker.markers.push_back(collision_point_marker); } @@ -1540,10 +1547,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const debug_marker.markers.push_back(obstacle_marker); // collision point - auto collision_point_marker = tier4_autoware_utils::createDefaultMarker( + auto collision_point_marker = autoware::universe_utils::createDefaultMarker( "map", now(), "stop_collision_points", 0, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_stop.at(i).collision_point; debug_marker.markers.push_back(collision_point_marker); } @@ -1557,16 +1564,16 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const debug_marker.markers.push_back(obstacle_marker); // collision points - auto front_collision_point_marker = tier4_autoware_utils::createDefaultMarker( + auto front_collision_point_marker = autoware::universe_utils::createDefaultMarker( "map", now(), "slow_down_collision_points", i * 2 + 0, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); front_collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_slow_down.at(i).front_collision_point; - auto back_collision_point_marker = tier4_autoware_utils::createDefaultMarker( + auto back_collision_point_marker = autoware::universe_utils::createDefaultMarker( "map", now(), "slow_down_collision_points", i * 2 + 1, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); back_collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_slow_down.at(i).back_collision_point; @@ -1583,10 +1590,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const } { // footprint polygons - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", now(), "detection_polygons", 0, Marker::LINE_LIST, - tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); for (const auto & detection_polygon : debug_data_ptr_->detection_polygons) { for (size_t dp_idx = 0; dp_idx < detection_polygon.outer().size(); ++dp_idx) { @@ -1595,16 +1602,16 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const detection_polygon.outer().at((dp_idx + 1) % detection_polygon.outer().size()); marker.points.push_back( - tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); marker.points.push_back( - tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); } } debug_marker.markers.push_back(marker); } // slow down debug wall marker - tier4_autoware_utils::appendMarkerArray( + autoware::universe_utils::appendMarkerArray( debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); debug_marker_pub_->publish(debug_marker); diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index c7172435a6f24..d256a64984667 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" - -#include "autoware_obstacle_cruise_planner/utils.hpp" +#include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" + +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" -#include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -174,7 +174,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( // Publish Debug trajectories const double traj_front_to_vehicle_offset = - motion_utils::calcSignedArcLength(stop_traj_points, 0, closest_idx); + autoware::motion_utils::calcSignedArcLength(stop_traj_points, 0, closest_idx); publishDebugTrajectory( planner_data, traj_front_to_vehicle_offset, time_vec, *s_boundaries, optimized_result); @@ -217,7 +217,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( } } const auto traj_stop_dist = - motion_utils::calcDistanceToForwardStopPoint(stop_traj_points, closest_idx); + autoware::motion_utils::calcDistanceToForwardStopPoint(stop_traj_points, closest_idx); if (traj_stop_dist) { closest_stop_dist = std::min(*traj_stop_dist + traj_front_to_vehicle_offset, closest_stop_dist); } @@ -226,8 +226,8 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( size_t break_id = stop_traj_points.size(); std::vector resampled_opt_position; for (size_t i = closest_idx; i < stop_traj_points.size(); ++i) { - const double query_s = - std::max(motion_utils::calcSignedArcLength(stop_traj_points, 0, i), opt_position.front()); + const double query_s = std::max( + autoware::motion_utils::calcSignedArcLength(stop_traj_points, 0, i), opt_position.front()); if (query_s > opt_position.back()) { break_id = i; break; @@ -253,7 +253,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( output.back().longitudinal_velocity_mps = 0.0; // terminal velocity is zero // Insert Closest Stop Point - motion_utils::insertStopPoint(0, closest_stop_dist, output); + autoware::motion_utils::insertStopPoint(0, closest_stop_dist, output); prev_output_ = output; return output; @@ -309,7 +309,7 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( TrajectoryPoint prev_output_closest_point; if (smoothed_trajectory_ptr_) { - prev_output_closest_point = motion_utils::calcInterpolatedPoint( + prev_output_closest_point = autoware::motion_utils::calcInterpolatedPoint( *smoothed_trajectory_ptr_, ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); } else { @@ -336,7 +336,7 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( const double engage_vel_thr = engage_velocity_ * engage_exit_ratio_; if (vehicle_speed < engage_vel_thr) { if (target_vel >= engage_velocity_) { - const auto stop_dist = motion_utils::calcDistanceToForwardStopPoint( + const auto stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint( input_traj_points, ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if ((stop_dist && *stop_dist > stop_dist_to_prohibit_engage_) || !stop_dist) { @@ -372,7 +372,7 @@ bool OptimizationBasedPlanner::checkHasReachedGoal( const PlannerData & planner_data, const std::vector & stop_traj_points) { // If goal is close and current velocity is low, we don't optimize trajectory - const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint( + const auto closest_stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint( stop_traj_points, planner_data.ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if (closest_stop_dist && *closest_stop_dist < 0.5 && planner_data.ego_vel < 0.6) { @@ -428,7 +428,7 @@ std::optional OptimizationBasedPlanner::getSBoundaries( const double rss_dist = calcRSSDistance(planner_data.ego_vel, obj_vel); const auto & safe_distance_margin = longitudinal_info_.safe_distance_margin; - const double ego_obj_length = motion_utils::calcSignedArcLength( + const double ego_obj_length = autoware::motion_utils::calcSignedArcLength( stop_traj_points, planner_data.ego_pose.position, obj.collision_points.front().point); const double slow_down_point_length = ego_obj_length - (rss_dist + safe_distance_margin); @@ -442,15 +442,15 @@ std::optional OptimizationBasedPlanner::getSBoundaries( if (min_slow_down_idx) { const auto & current_time = planner_data.current_time; - const auto marker_pose = motion_utils::calcLongitudinalOffsetPose( + const auto marker_pose = autoware::motion_utils::calcLongitudinalOffsetPose( stop_traj_points, planner_data.ego_pose.position, min_slow_down_point_length); if (marker_pose) { MarkerArray wall_msg; - const auto markers = motion_utils::createSlowDownVirtualWallMarker( + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( marker_pose.value(), "obstacle to follow", current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &wall_msg); + autoware::universe_utils::appendMarkerArray(markers, &wall_msg); // publish rviz marker debug_wall_marker_pub_->publish(wall_msg); @@ -583,7 +583,7 @@ bool OptimizationBasedPlanner::checkOnTrajectory( } const double lateral_offset = - std::fabs(motion_utils::calcLateralOffset(stop_traj_points, point.point)); + std::fabs(autoware::motion_utils::calcLateralOffset(stop_traj_points, point.point)); if (lateral_offset < vehicle_info_.max_lateral_offset_m + 0.1) { return true; @@ -597,10 +597,10 @@ std::optional OptimizationBasedPlanner::calcTrajectoryLengthFromCurrentP { const size_t ego_segment_idx = ego_nearest_param_.findSegmentIndex(traj_points, ego_pose); - const double traj_length = motion_utils::calcSignedArcLength( + const double traj_length = autoware::motion_utils::calcSignedArcLength( traj_points, ego_pose.position, ego_segment_idx, traj_points.size() - 1); - const auto dist_to_closest_stop_point = motion_utils::calcDistanceToForwardStopPoint( + const auto dist_to_closest_stop_point = autoware::motion_utils::calcDistanceToForwardStopPoint( traj_points, ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if (dist_to_closest_stop_point) { return std::min(traj_length, dist_to_closest_stop_point.value()); @@ -627,7 +627,7 @@ geometry_msgs::msg::Pose OptimizationBasedPlanner::transformBaseLink2Center( geometry_msgs::msg::Pose center_pose; center_pose.position = - tier4_autoware_utils::createPoint(map2center.x(), map2center.y(), map2center.z()); + autoware::universe_utils::createPoint(map2center.x(), map2center.y(), map2center.z()); center_pose.orientation = pose_base_link.orientation; return center_pose; diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp index 695db439e566c..8e31411bfb4af 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" +#include "autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" #include diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 5156c61038122..345265b1d20b0 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" -#include "autoware_obstacle_cruise_planner/utils.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "interpolation/spline_interpolation.hpp" -#include "motion_utils/marker/marker_helper.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" @@ -313,12 +313,13 @@ std::vector PIDBasedPlanner::planCruise( const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( stop_traj_points, dist_to_rss_wall, ego_idx); - auto markers = motion_utils::createSlowDownVirtualWallMarker( + auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( stop_traj_points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, 0); // NOTE: use a different color from slow down one to visualize cruise and slow down // separately. - markers.markers.front().color = tier4_autoware_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); + markers.markers.front().color = + autoware::universe_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); // cruise obstacle debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); @@ -344,8 +345,8 @@ std::vector PIDBasedPlanner::planCruise( // delete marker const auto markers = - motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); + autoware::motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); return stop_traj_points; } @@ -479,7 +480,7 @@ std::vector PIDBasedPlanner::doCruiseWithTrajectory( // set target longitudinal motion const auto prev_traj_closest_point = [&]() -> TrajectoryPoint { // if (smoothed_trajectory_ptr_) { - // return motion_utils::calcInterpolatedPoint( + // return autoware::motion_utils::calcInterpolatedPoint( // *smoothed_trajectory_ptr_, planner_data.ego_pose, nearest_dist_deviation_threshold_, // nearest_yaw_deviation_threshold_); // } @@ -491,7 +492,7 @@ std::vector PIDBasedPlanner::doCruiseWithTrajectory( auto cruise_traj_points = getAccelerationLimitedTrajectory( stop_traj_points, planner_data.ego_pose, v0, a0, target_acc, target_jerk_ratio); - const auto zero_vel_idx_opt = motion_utils::searchZeroVelocityIndex(cruise_traj_points); + const auto zero_vel_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(cruise_traj_points); if (!zero_vel_idx_opt) { return cruise_traj_points; } @@ -528,7 +529,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( }; // calculate sv graph - const double s_traj = motion_utils::calcArcLength(traj_points); + const double s_traj = autoware::motion_utils::calcArcLength(traj_points); // const double t_max = 10.0; const double s_max = 50.0; const double max_sampling_num = 100.0; @@ -598,7 +599,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( double sum_dist = 0.0; for (size_t i = ego_seg_idx; i < acc_limited_traj_points.size(); ++i) { if (i != ego_seg_idx) { - sum_dist += tier4_autoware_utils::calcDistance2d( + sum_dist += autoware::universe_utils::calcDistance2d( acc_limited_traj_points.at(i - 1), acc_limited_traj_points.at(i)); } @@ -621,7 +622,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( void PIDBasedPlanner::updateCruiseParam(const std::vector & parameters) { - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_); { // velocity limit based planner @@ -631,26 +632,26 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki = p.pid_vel_controller->getKi(); double kd = p.pid_vel_controller->getKd(); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kp", kp); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.ki", ki); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kd", kd); p.pid_vel_controller->updateParam(kp, ki, kd); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel", p.output_ratio_during_accel); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.vel_to_acc_weight", p.vel_to_acc_weight); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc", p.enable_jerk_limit_to_output_acc); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_limit_based_planner.disable_target_acceleration", p.disable_target_acceleration); @@ -664,11 +665,11 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki_acc = p.pid_acc_controller->getKi(); double kd_acc = p.pid_acc_controller->getKd(); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_acc", kp_acc); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_acc", ki_acc); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_acc", kd_acc); p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc); @@ -677,32 +678,32 @@ void PIDBasedPlanner::updateCruiseParam(const std::vector & p double ki_jerk = p.pid_jerk_controller->getKi(); double kd_jerk = p.pid_jerk_controller->getKd(); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_jerk", kp_jerk); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_jerk", ki_jerk); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_jerk", kd_jerk); p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel", p.output_acc_ratio_during_accel); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel", p.output_jerk_ratio_during_accel); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc", p.enable_jerk_limit_to_output_acc); } // min_cruise_target_vel - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "cruise.pid_based_planner.time_to_evaluate_rss", time_to_evaluate_rss_); } diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index f3b5af842acdb..18e0fc7f5bb03 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_obstacle_cruise_planner/planner_interface.hpp" - -#include "motion_utils/distance/distance.hpp" -#include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/obstacle_cruise_planner/planner_interface.hpp" + +#include "autoware/motion_utils/distance/distance.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "signal_processing/lowpass_filter_1d.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" #include #include @@ -50,7 +50,7 @@ tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( stop_factor.stop_pose = stop_pose; geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_point; stop_factor_point.z = stop_pose.position.z; - stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength( + stop_factor.dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position); stop_factor.stop_factor_points.emplace_back(stop_factor_point); @@ -143,7 +143,7 @@ double findReachTime( return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; }; if (f(min, j, a, v, d) > 0 || f(max, j, a, v, d) < 0) { - std::logic_error("[obstacle_cruise_planner](findReachTime): search range is invalid"); + throw std::logic_error("[obstacle_cruise_planner](findReachTime): search range is invalid"); } const double eps = 1e-5; const int warn_iter = 100; @@ -175,7 +175,7 @@ double calcDecelerationVelocityFromDistanceToTarget( const double current_velocity, const double distance_to_target) { if (max_slowdown_jerk > 0 || max_slowdown_accel > 0) { - std::logic_error("max_slowdown_jerk and max_slowdown_accel should be negative"); + throw std::logic_error("max_slowdown_jerk and max_slowdown_accel should be negative"); } // case0: distance to target is behind ego if (distance_to_target <= 0) return current_velocity; @@ -218,14 +218,14 @@ double calcDecelerationVelocityFromDistanceToTarget( std::vector resampleTrajectoryPoints( const std::vector & traj_points, const double interval) { - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory(traj, interval); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); } -tier4_autoware_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) +autoware::universe_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) { - return tier4_autoware_utils::Point2d{p.x, p.y}; + return autoware::universe_utils::Point2d{p.x, p.y}; } } // namespace @@ -248,8 +248,8 @@ std::vector PlannerInterface::generateStopTrajectory( // delete marker const auto markers = - motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); prev_stop_distance_info_ = std::nullopt; return planner_data.traj_points; @@ -269,14 +269,14 @@ std::vector PlannerInterface::generateStopTrajectory( const auto ego_segment_idx = ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); const double dist_to_collide_on_ref_traj = - motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + + autoware::motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + stop_obstacle.dist_to_collide_on_decimated_traj; const double desired_margin = [&]() { const double margin_from_obstacle = calculateMarginFromObstacleOnCurve(planner_data, stop_obstacle); // Use terminal margin (terminal_safe_distance_margin) for obstacle stop - const auto ref_traj_length = motion_utils::calcSignedArcLength( + const auto ref_traj_length = autoware::motion_utils::calcSignedArcLength( planner_data.traj_points, 0, planner_data.traj_points.size() - 1); if (dist_to_collide_on_ref_traj > ref_traj_length) { return longitudinal_info_.terminal_safe_distance_margin; @@ -284,11 +284,12 @@ std::vector PlannerInterface::generateStopTrajectory( // If behavior stop point is ahead of the closest_obstacle_stop point within a certain // margin we set closest_obstacle_stop_distance to closest_behavior_stop_distance - const auto closest_behavior_stop_idx = - motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1); + const auto closest_behavior_stop_idx = autoware::motion_utils::searchZeroVelocityIndex( + planner_data.traj_points, ego_segment_idx + 1); if (closest_behavior_stop_idx) { - const double closest_behavior_stop_dist_on_ref_traj = motion_utils::calcSignedArcLength( - planner_data.traj_points, 0, *closest_behavior_stop_idx); + const double closest_behavior_stop_dist_on_ref_traj = + autoware::motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, *closest_behavior_stop_idx); const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj - (dist_to_collide_on_ref_traj - margin_from_obstacle); if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { @@ -328,7 +329,7 @@ std::vector PlannerInterface::generateStopTrajectory( } const double acceptable_stop_pos = - motion_utils::calcSignedArcLength( + autoware::motion_utils::calcSignedArcLength( planner_data.traj_points, 0, planner_data.ego_pose.position) + calcMinimumDistanceToStop( planner_data.ego_vel, longitudinal_info_.limit_max_accel, acceptable_stop_acc.value()); @@ -355,8 +356,8 @@ std::vector PlannerInterface::generateStopTrajectory( if (!determined_zero_vel_dist) { // delete marker const auto markers = - motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); prev_stop_distance_info_ = std::nullopt; return planner_data.traj_points; @@ -369,9 +370,9 @@ std::vector PlannerInterface::generateStopTrajectory( // NOTE: We assume that the current trajectory's front point is ahead of the previous // trajectory's front point. const size_t traj_front_point_prev_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prev_stop_distance_info_->first, planner_data.traj_points.front().pose); - const double diff_dist_front_points = motion_utils::calcSignedArcLength( + const double diff_dist_front_points = autoware::motion_utils::calcSignedArcLength( prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, traj_front_point_prev_seg_idx); @@ -386,13 +387,13 @@ std::vector PlannerInterface::generateStopTrajectory( // Insert stop point auto output_traj_points = planner_data.traj_points; const auto zero_vel_idx = - motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points); + autoware::motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points); if (zero_vel_idx) { // virtual wall marker for stop obstacle - const auto markers = motion_utils::createStopVirtualWallMarker( + const auto markers = autoware::motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, abs_ego_offset, "", planner_data.is_driving_forward); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); // Publish Stop Reason @@ -405,7 +406,7 @@ std::vector PlannerInterface::generateStopTrajectory( // Publish if ego vehicle will over run against the stop point with a limit acceleration const bool will_over_run = determined_zero_vel_dist.value() > - motion_utils::calcSignedArcLength( + autoware::motion_utils::calcSignedArcLength( planner_data.traj_points, 0, planner_data.ego_pose.position) + determined_stop_obstacle->dist_to_collide_on_decimated_traj + determined_desired_margin.value() + 0.1; @@ -447,8 +448,8 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( : std::abs(vehicle_info_.min_longitudinal_offset_m); // calculate short trajectory points towards obstacle - const size_t obj_segment_idx = - motion_utils::findNearestSegmentIndex(planner_data.traj_points, stop_obstacle.collision_point); + const size_t obj_segment_idx = autoware::motion_utils::findNearestSegmentIndex( + planner_data.traj_points, stop_obstacle.collision_point); std::vector short_traj_points{planner_data.traj_points.at(obj_segment_idx + 1)}; double sum_short_traj_length{0.0}; for (int i = obj_segment_idx; 0 <= i; --i) { @@ -459,7 +460,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) { break; } - sum_short_traj_length += tier4_autoware_utils::calcDistance2d( + sum_short_traj_length += autoware::universe_utils::calcDistance2d( planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); } std::reverse(short_traj_points.begin(), short_traj_points.end()); @@ -470,15 +471,15 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( // calculate collision index between straight line from ego pose and object const auto calculate_distance_from_straight_ego_path = [&](const auto & ego_pose, const auto & object_polygon) { - const auto forward_ego_pose = tier4_autoware_utils::calcOffsetPose( + const auto forward_ego_pose = autoware::universe_utils::calcOffsetPose( ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); - const auto ego_straight_segment = tier4_autoware_utils::Segment2d{ + const auto ego_straight_segment = autoware::universe_utils::Segment2d{ convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; return boost::geometry::distance(ego_straight_segment, object_polygon); }; const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); const auto object_polygon = - tier4_autoware_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + autoware::universe_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); const auto collision_idx = [&]() -> std::optional { for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { const double dist_to_obj = calculate_distance_from_straight_ego_path( @@ -498,7 +499,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( // calculate margin from obstacle const double partial_segment_length = [&]() { - const double collision_segment_length = tier4_autoware_utils::calcDistance2d( + const double collision_segment_length = autoware::universe_utils::calcDistance2d( resampled_short_traj_points.at(*collision_idx - 1), resampled_short_traj_points.at(*collision_idx)); const double prev_dist = calculate_distance_from_straight_ego_path( @@ -511,7 +512,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( const double short_margin_from_obstacle = partial_segment_length + - motion_utils::calcSignedArcLength( + autoware::motion_utils::calcSignedArcLength( resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) - abs_ego_offset + additional_safe_distance_margin_on_curve_; @@ -531,9 +532,9 @@ double PlannerInterface::calcDistanceToCollisionPoint( ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); const size_t collision_segment_idx = - motion_utils::findNearestSegmentIndex(planner_data.traj_points, collision_point); + autoware::motion_utils::findNearestSegmentIndex(planner_data.traj_points, collision_point); - const auto dist_to_collision_point = motion_utils::calcSignedArcLength( + const auto dist_to_collision_point = autoware::motion_utils::calcSignedArcLength( planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, collision_point, collision_segment_idx); @@ -552,7 +553,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const double dist_to_ego = [&]() { const size_t ego_seg_idx = ego_nearest_param_.findSegmentIndex(slow_down_traj_points, planner_data.ego_pose); - return motion_utils::calcSignedArcLength( + return autoware::motion_utils::calcSignedArcLength( slow_down_traj_points, 0, planner_data.ego_pose.position, ego_seg_idx); }(); const double abs_ego_offset = planner_data.is_driving_forward @@ -561,7 +562,8 @@ std::vector PlannerInterface::generateSlowDownTrajectory( // define function to insert slow down velocity to trajectory const auto insert_point_in_trajectory = [&](const double lon_dist) -> std::optional { - const auto inserted_idx = motion_utils::insertTargetPoint(0, lon_dist, slow_down_traj_points); + const auto inserted_idx = + autoware::motion_utils::insertTargetPoint(0, lon_dist, slow_down_traj_points); if (inserted_idx) { if (inserted_idx.value() + 1 <= slow_down_traj_points.size() - 1) { // zero-order hold for velocity interpolation @@ -596,8 +598,9 @@ std::vector PlannerInterface::generateSlowDownTrajectory( obstacle.uuid.c_str()); continue; } - const auto [dist_to_slow_down_start, dist_to_slow_down_end, feasible_slow_down_vel] = - *dist_vec_to_slow_down; + const auto dist_to_slow_down_start = std::get<0>(*dist_vec_to_slow_down); + const auto dist_to_slow_down_end = std::get<1>(*dist_vec_to_slow_down); + const auto feasible_slow_down_vel = std::get<2>(*dist_vec_to_slow_down); // calculate slow down end distance, and insert slow down velocity // NOTE: slow_down_start_idx will not be wrong since inserted back point is after inserted @@ -646,25 +649,25 @@ std::vector PlannerInterface::generateSlowDownTrajectory( return *slow_down_end_idx; }(); - const auto markers = motion_utils::createSlowDownVirtualWallMarker( + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); } // add debug virtual wall if (slow_down_start_idx) { - const auto markers = motion_utils::createSlowDownVirtualWallMarker( + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward); - tier4_autoware_utils::appendMarkerArray( + autoware::universe_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { - const auto markers = motion_utils::createSlowDownVirtualWallMarker( + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward); - tier4_autoware_utils::appendMarkerArray( + autoware::universe_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } @@ -726,9 +729,9 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( // calculate distance to collision points const double dist_to_front_collision = - motion_utils::calcSignedArcLength(traj_points, 0, obstacle.front_collision_point); + autoware::motion_utils::calcSignedArcLength(traj_points, 0, obstacle.front_collision_point); const double dist_to_back_collision = - motion_utils::calcSignedArcLength(traj_points, 0, obstacle.back_collision_point); + autoware::motion_utils::calcSignedArcLength(traj_points, 0, obstacle.back_collision_point); // calculate offset distance to first collision considering relative velocity const double relative_vel = planner_data.ego_vel - obstacle.velocity; @@ -771,9 +774,9 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( const auto apply_lowpass_filter = [&](const double dist_to_slow_down, const auto prev_point) { if (prev_output && prev_point) { const size_t seg_idx = - motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); + autoware::motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); const double prev_dist_to_slow_down = - motion_utils::calcSignedArcLength(traj_points, 0, prev_point->position, seg_idx); + autoware::motion_utils::calcSignedArcLength(traj_points, 0, prev_point->position, seg_idx); return signal_processing::lowpassFilter( dist_to_slow_down, prev_dist_to_slow_down, slow_down_param_.lpf_gain_dist_to_slow_down); } diff --git a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp index e5f53d1b88c0b..8c3abe58f5d59 100644 --- a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_obstacle_cruise_planner/polygon_utils.hpp" +#include "autoware/obstacle_cruise_planner/polygon_utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" namespace { @@ -35,8 +35,8 @@ PointWithStamp calcNearestCollisionPoint( std::vector dist_vec; for (const auto & collision_point : collision_points) { - const double dist = - motion_utils::calcLongitudinalOffsetToSegment(segment_points, 0, collision_point.point); + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + segment_points, 0, collision_point.point); dist_vec.push_back(dist); } @@ -51,10 +51,10 @@ std::optional>> getCollisionIndex( const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, const Shape & object_shape, const double max_dist = std::numeric_limits::max()) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object_pose, object_shape); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object_pose, object_shape); for (size_t i = 0; i < traj_polygons.size(); ++i) { const double approximated_dist = - tier4_autoware_utils::calcDistance2d(traj_points.at(i).pose, object_pose); + autoware::universe_utils::calcDistance2d(traj_points.at(i).pose, object_pose); if (approximated_dist > max_dist) { continue; } @@ -105,14 +105,14 @@ std::optional> getCollisionPoint( const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m : vehicle_info.min_longitudinal_offset_m; - const auto bumper_pose = tier4_autoware_utils::calcOffsetPose( + const auto bumper_pose = autoware::universe_utils::calcOffsetPose( traj_points.at(collision_info->first).pose, x_diff_to_bumper, 0.0, 0.0); std::optional max_collision_length = std::nullopt; std::optional max_collision_point = std::nullopt; for (const auto & poly_vertex : collision_info->second) { const double dist_from_bumper = - std::abs(tier4_autoware_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { max_collision_length = dist_from_bumper; @@ -120,8 +120,9 @@ std::optional> getCollisionPoint( } } return std::make_pair( - *max_collision_point, motion_utils::calcSignedArcLength(traj_points, 0, collision_info->first) - - *max_collision_length); + *max_collision_point, + autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info->first) - + *max_collision_length); } // NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index cb36f35b8e837..8162d8036d9dc 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_obstacle_cruise_planner/utils.hpp" +#include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" namespace obstacle_cruise_utils { @@ -59,10 +59,10 @@ visualization_msgs::msg::Marker getObjectMarker( { const auto current_time = rclcpp::Clock().now(); - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", current_time, ns, idx, visualization_msgs::msg::Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(2.0, 2.0, 2.0), - tier4_autoware_utils::createMarkerColor(r, g, b, 0.8)); + autoware::universe_utils::createMarkerScale(2.0, 2.0, 2.0), + autoware::universe_utils::createMarkerColor(r, g, b, 0.8)); marker.pose = obj_pose; diff --git a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp index b28afba8b4895..ef5f695756b7e 100644 --- a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_obstacle_cruise_planner/node.hpp" +#include "autoware/obstacle_cruise_planner/node.hpp" #include #include diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp similarity index 89% rename from planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp rename to planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp index d2de8d201d9fa..7f2688ffaad95 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ -#define AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ +#ifndef AUTOWARE__PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ +#define AUTOWARE__PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ -#include "autoware_path_optimizer/type_alias.hpp" +#include "autoware/path_optimizer/type_alias.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -89,7 +89,7 @@ struct TimeKeeper double accumulated_time{0.0}; - tier4_autoware_utils::StopWatch< + autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; @@ -122,7 +122,7 @@ struct TrajectoryParam void onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; // common updateParam( @@ -145,7 +145,7 @@ struct EgoNearestParam void onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); } @@ -155,4 +155,4 @@ struct EgoNearestParam }; } // namespace autoware::path_optimizer -#endif // AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ +#endif // AUTOWARE__PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/debug_marker.hpp similarity index 73% rename from planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp rename to planning/autoware_path_optimizer/include/autoware/path_optimizer/debug_marker.hpp index cea58a11a530a..487320330f953 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/debug_marker.hpp @@ -11,15 +11,15 @@ // 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 AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ -#define AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE__PATH_OPTIMIZER__DEBUG_MARKER_HPP_ +#define AUTOWARE__PATH_OPTIMIZER__DEBUG_MARKER_HPP_ -#include "autoware_path_optimizer/common_structs.hpp" -#include "autoware_path_optimizer/type_alias.hpp" +#include "autoware/path_optimizer/common_structs.hpp" +#include "autoware/path_optimizer/type_alias.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/clock.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" #include #include @@ -32,4 +32,4 @@ MarkerArray getDebugMarker( const std::vector & optimized_points, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const bool publish_extra_marker); } // namespace autoware::path_optimizer -#endif // AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ +#endif // AUTOWARE__PATH_OPTIMIZER__DEBUG_MARKER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp similarity index 94% rename from planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp rename to planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 98d7e131f4bdc..4303e5c7e05b4 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ -#define AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ +#ifndef AUTOWARE__PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ +#define AUTOWARE__PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ -#include "autoware_path_optimizer/common_structs.hpp" -#include "autoware_path_optimizer/state_equation_generator.hpp" -#include "autoware_path_optimizer/type_alias.hpp" +#include "autoware/path_optimizer/common_structs.hpp" +#include "autoware/path_optimizer/state_equation_generator.hpp" +#include "autoware/path_optimizer/type_alias.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -93,9 +93,9 @@ struct ReferencePoint geometry_msgs::msg::Pose offsetDeviation(const double lat_dev, const double yaw_dev) const { - auto pose_with_deviation = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lat_dev, 0.0); + auto pose_with_deviation = autoware::universe_utils::calcOffsetPose(pose, 0.0, lat_dev, 0.0); pose_with_deviation.orientation = - tier4_autoware_utils::createQuaternionFromYaw(getYaw() + yaw_dev); + autoware::universe_utils::createQuaternionFromYaw(getYaw() + yaw_dev); return pose_with_deviation; } }; @@ -310,4 +310,4 @@ class MPTOptimizer std::optional calcNormalizedAvoidanceCost(const ReferencePoint & ref_point) const; }; } // namespace autoware::path_optimizer -#endif // AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ +#endif // AUTOWARE__PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp similarity index 83% rename from planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp rename to planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp index c1f644d31aa9a..359c20f2a4d29 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ -#define AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ - -#include "autoware_path_optimizer/common_structs.hpp" -#include "autoware_path_optimizer/mpt_optimizer.hpp" -#include "autoware_path_optimizer/replan_checker.hpp" -#include "autoware_path_optimizer/type_alias.hpp" +#ifndef AUTOWARE__PATH_OPTIMIZER__NODE_HPP_ +#define AUTOWARE__PATH_OPTIMIZER__NODE_HPP_ + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/path_optimizer/common_structs.hpp" +#include "autoware/path_optimizer/mpt_optimizer.hpp" +#include "autoware/path_optimizer/replan_checker.hpp" +#include "autoware/path_optimizer/type_alias.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include +#include #include #include @@ -52,7 +52,7 @@ class PathOptimizer : public rclcpp::Node public: bool isDrivingForward(const std::vector & path_points) { - const auto is_driving_forward = motion_utils::isDrivingForward(path_points); + const auto is_driving_forward = autoware::motion_utils::isDrivingForward(path_points); is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; return is_driving_forward_; } @@ -90,7 +90,7 @@ class PathOptimizer : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber ego_odom_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber ego_odom_sub_{ this, "~/input/odometry"}; // debug publisher @@ -137,10 +137,10 @@ class PathOptimizer : public rclcpp::Node private: double vehicle_stop_margin_outside_drivable_area_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::path_optimizer -#endif // AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ +#endif // AUTOWARE__PATH_OPTIMIZER__NODE_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/replan_checker.hpp similarity index 87% rename from planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp rename to planning/autoware_path_optimizer/include/autoware/path_optimizer/replan_checker.hpp index 26d9599f07091..57dfb4c5e4dff 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/replan_checker.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ -#define AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ +#ifndef AUTOWARE__PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ +#define AUTOWARE__PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ -#include "autoware_path_optimizer/common_structs.hpp" -#include "autoware_path_optimizer/mpt_optimizer.hpp" -#include "autoware_path_optimizer/type_alias.hpp" +#include "autoware/path_optimizer/common_structs.hpp" +#include "autoware/path_optimizer/mpt_optimizer.hpp" +#include "autoware/path_optimizer/type_alias.hpp" #include @@ -68,4 +68,4 @@ class ReplanChecker }; } // namespace autoware::path_optimizer -#endif // AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ +#endif // AUTOWARE__PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp similarity index 82% rename from planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp rename to planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp index 0c41da0ee5f62..b8162f5ef2d32 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ -#define AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ +#ifndef AUTOWARE__PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ +#define AUTOWARE__PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ -#include "autoware_path_optimizer/common_structs.hpp" -#include "autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/path_optimizer/common_structs.hpp" +#include "autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -59,4 +59,4 @@ class StateEquationGenerator mutable std::shared_ptr time_keeper_ptr_; }; } // namespace autoware::path_optimizer -#endif // AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ +#endif // AUTOWARE__PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/type_alias.hpp similarity index 91% rename from planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp rename to planning/autoware_path_optimizer/include/autoware/path_optimizer/type_alias.hpp index 63a5840789a6b..5b16d4d5ba5c6 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ -#define AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE__PATH_OPTIMIZER__TYPE_ALIAS_HPP_ +#define AUTOWARE__PATH_OPTIMIZER__TYPE_ALIAS_HPP_ #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -47,4 +47,4 @@ using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; } // namespace autoware::path_optimizer -#endif // AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ +#endif // AUTOWARE__PATH_OPTIMIZER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp similarity index 78% rename from planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp rename to planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp index aa44a515a08ec..edc91bd40d4dc 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ -#define AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ +#ifndef AUTOWARE__PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ +#define AUTOWARE__PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ -#include "autoware_path_optimizer/common_structs.hpp" -#include "autoware_path_optimizer/type_alias.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/path_optimizer/common_structs.hpp" +#include "autoware/path_optimizer/type_alias.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include @@ -36,14 +36,14 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { template <> geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); template <> geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p); -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils namespace autoware::path_optimizer { @@ -52,8 +52,8 @@ namespace geometry_utils template bool isSamePoint(const T1 & t1, const T2 & t2) { - const auto p1 = tier4_autoware_utils::getPoint(t1); - const auto p2 = tier4_autoware_utils::getPoint(t2); + const auto p1 = autoware::universe_utils::getPoint(t1); + const auto p2 = autoware::universe_utils::getPoint(t2); constexpr double epsilon = 1e-6; if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) { @@ -69,4 +69,4 @@ bool isOutsideDrivableAreaFromRectangleFootprint( const bool use_footprint_polygon_for_outside_drivable_area_check); } // namespace geometry_utils } // namespace autoware::path_optimizer -#endif // AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ +#endif // AUTOWARE__PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp similarity index 81% rename from planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp rename to planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp index f3f528df2ec42..a54d60835aadf 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ -#define AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE__PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE__PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ -#include "autoware_path_optimizer/common_structs.hpp" -#include "autoware_path_optimizer/type_alias.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/path_optimizer/common_structs.hpp" +#include "autoware/path_optimizer/type_alias.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include @@ -35,7 +35,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { template <> geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); @@ -45,7 +45,7 @@ geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint template <> double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & p); -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils namespace autoware::path_optimizer { @@ -61,14 +61,14 @@ std::optional getPointIndexAfter( } double sum_length = - -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + -autoware::motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); std::optional output_idx{std::nullopt}; // search forward if (sum_length < min_offset) { for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (min_offset < sum_length) { output_idx = i - 1; } @@ -82,7 +82,7 @@ std::optional getPointIndexAfter( // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value - sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < min_offset) { output_idx = i - 1; } @@ -98,7 +98,7 @@ template TrajectoryPoint convertToTrajectoryPoint(const T & point) { TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.pose = autoware::universe_utils::getPose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -109,8 +109,9 @@ template <> inline TrajectoryPoint convertToTrajectoryPoint(const ReferencePoint & ref_point) { TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(ref_point); - traj_point.longitudinal_velocity_mps = tier4_autoware_utils::getLongitudinalVelocity(ref_point); + traj_point.pose = autoware::universe_utils::getPose(ref_point); + traj_point.longitudinal_velocity_mps = + autoware::universe_utils::getLongitudinalVelocity(ref_point); return traj_point; } @@ -143,7 +144,7 @@ size_t findEgoIndex( const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, const EgoNearestParam & ego_nearest_param) { - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } @@ -152,7 +153,7 @@ size_t findEgoSegmentIndex( const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, const EgoNearestParam & ego_nearest_param) { - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } @@ -172,13 +173,13 @@ std::optional updateFrontPointForFix( { // calculate front point to insert in points as a fixed point const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( - points_for_fix, tier4_autoware_utils::getPose(points.front()), ego_nearest_param); + points_for_fix, autoware::universe_utils::getPose(points.front()), ego_nearest_param); const size_t front_point_idx_for_fix = front_seg_idx_for_fix; const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); // check if the points_for_fix is longer in front than points const double lon_offset_to_prev_front = - motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); + autoware::motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { RCLCPP_DEBUG( rclcpp::get_logger("autoware_path_optimizer.trajectory_utils"), @@ -186,7 +187,7 @@ std::optional updateFrontPointForFix( return std::nullopt; } - const double dist = tier4_autoware_utils::calcDistance2d(points.front(), front_fix_point); + const double dist = autoware::universe_utils::calcDistance2d(points.front(), front_fix_point); // check if deviation is not too large constexpr double max_lat_error = 3.0; @@ -215,4 +216,4 @@ void insertStopPoint( const size_t stop_seg_idx); } // namespace trajectory_utils } // namespace autoware::path_optimizer -#endif // AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ +#endif // AUTOWARE__PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp similarity index 75% rename from planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp rename to planning/autoware_path_optimizer/include/autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp index ec46a96e3d12b..e59b332ecf69c 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#define AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#ifndef AUTOWARE__PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#define AUTOWARE__PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#include "autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -32,4 +32,4 @@ class KinematicsBicycleModel : public VehicleModelInterface Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double curvature, const double ds) const override; }; -#endif // AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#endif // AUTOWARE__PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp similarity index 83% rename from planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp rename to planning/autoware_path_optimizer/include/autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp index 85180bd33b34c..1374ccae9203c 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#define AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#ifndef AUTOWARE__PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#define AUTOWARE__PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #include #include @@ -44,4 +44,4 @@ class VehicleModelInterface const double ds) const = 0; }; -#endif // AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#endif // AUTOWARE__PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index c158815999398..0830d534c7cb0 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -14,19 +14,19 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation - motion_utils nav_msgs osqp_interface rclcpp rclcpp_components std_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_path_optimizer/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp index c97d5c5adec46..397c334eca5b7 100644 --- a/planning/autoware_path_optimizer/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_optimizer/debug_marker.hpp" +#include "autoware/path_optimizer/debug_marker.hpp" -#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware/path_optimizer/mpt_optimizer.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace autoware::path_optimizer { -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; namespace { @@ -53,16 +53,16 @@ MarkerArray getFootprintsMarkerArray( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) + autoware::universe_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) + autoware::universe_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) + autoware::universe_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) + autoware::universe_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) .position); marker.points.push_back(marker.points.front()); @@ -110,7 +110,7 @@ MarkerArray getBoundsWidthMarkerArray( (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double lb_y = ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - base_to_right; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + const auto lb = autoware::universe_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(pose.position); lb_marker.points.push_back(lb); @@ -132,7 +132,7 @@ MarkerArray getBoundsWidthMarkerArray( (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + base_to_left; - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + const auto ub = autoware::universe_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(pose.position); ub_marker.points.push_back(ub); @@ -167,11 +167,11 @@ MarkerArray getBoundsLineMarkerArray( const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = ref_points.at(i).bounds.upper_bound + base_to_left; - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + const auto ub = autoware::universe_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(ub); const double lb_y = ref_points.at(i).bounds.lower_bound - base_to_right; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + const auto lb = autoware::universe_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(lb); } marker_array.markers.push_back(ub_marker); @@ -205,22 +205,22 @@ MarkerArray getVehicleCircleLinesMarkerArray( // apply lateral and yaw deviation auto pose_with_deviation = - tier4_autoware_utils::calcOffsetPose(ref_point.pose, 0.0, lat_dev, 0.0); + autoware::universe_utils::calcOffsetPose(ref_point.pose, 0.0, lat_dev, 0.0); pose_with_deviation.orientation = - tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + yaw_dev); + autoware::universe_utils::createQuaternionFromYaw(ref_point.getYaw() + yaw_dev); for (const double d : vehicle_circle_longitudinal_offsets) { // apply longitudinal offset - auto base_pose = tier4_autoware_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); + auto base_pose = autoware::universe_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); base_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); + autoware::universe_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const auto ub = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, base_to_left, 0.0).position; + autoware::universe_utils::calcOffsetPose(base_pose, 0.0, base_to_left, 0.0).position; const auto lb = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -base_to_right, 0.0).position; + autoware::universe_utils::calcOffsetPose(base_pose, 0.0, -base_to_right, 0.0).position; marker.points.push_back(ub); marker.points.push_back(lb); @@ -247,7 +247,7 @@ MarkerArray getCurrentVehicleCirclesMarkerArray( "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = tier4_autoware_utils::calcOffsetPose(ego_pose, offset, 0.0, 0.0); + marker.pose = autoware::universe_utils::calcOffsetPose(ego_pose, offset, 0.0, 0.0); constexpr size_t circle_dividing_num = 16; for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { @@ -289,7 +289,7 @@ MarkerArray getVehicleCirclesMarkerArray( "map", rclcpp::Clock().now(), ns, id, Marker::LINE_STRIP, createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); - marker.pose = tier4_autoware_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); + marker.pose = autoware::universe_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); constexpr size_t circle_dividing_num = 16; for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { @@ -356,13 +356,15 @@ visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, base_to_left, 0.0).position); + autoware::universe_utils::calcOffsetPose(stop_pose, base_to_front, base_to_left, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, base_to_front, -base_to_right, 0.0).position); + autoware::universe_utils::calcOffsetPose(stop_pose, base_to_front, -base_to_right, 0.0) + .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, -base_to_right, 0.0).position); + autoware::universe_utils::calcOffsetPose(stop_pose, -base_to_rear, -base_to_right, 0.0) + .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(stop_pose, -base_to_rear, base_to_left, 0.0).position); + autoware::universe_utils::calcOffsetPose(stop_pose, -base_to_rear, base_to_left, 0.0).position); marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 0652085d85c48..b5fd266006a48 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_optimizer/mpt_optimizer.hpp" - -#include "autoware_path_optimizer/utils/geometry_utils.hpp" -#include "autoware_path_optimizer/utils/trajectory_utils.hpp" +#include "autoware/path_optimizer/mpt_optimizer.hpp" + +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/path_optimizer/utils/geometry_utils.hpp" +#include "autoware/path_optimizer/utils/trajectory_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" #include #include @@ -125,8 +125,8 @@ std::vector toStdVector(const Eigen::VectorXd & eigen_vec) bool isLeft(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_pos) { const double base_theta = tf2::getYaw(pose.orientation); - const double target_theta = tier4_autoware_utils::calcAzimuthAngle(pose.position, target_pos); - const double diff_theta = tier4_autoware_utils::normalizeRadian(target_theta - base_theta); + const double target_theta = autoware::universe_utils::calcAzimuthAngle(pose.position, target_pos); + const double diff_theta = autoware::universe_utils::normalizeRadian(target_theta - base_theta); return diff_theta > 0; } @@ -141,18 +141,18 @@ double calcLateralDistToBounds( const double max_lat_offset = is_left_bound ? max_lat_offset_for_left : -max_lat_offset_for_left; const double min_lat_offset = is_left_bound ? min_lat_offset_for_left : -min_lat_offset_for_left; const auto max_lat_offset_point = - tier4_autoware_utils::calcOffsetPose(pose, 0.0, max_lat_offset, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, 0.0, max_lat_offset, 0.0).position; const auto min_lat_offset_point = - tier4_autoware_utils::calcOffsetPose(pose, 0.0, min_lat_offset, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, 0.0, min_lat_offset, 0.0).position; double closest_dist_to_bound = max_lat_offset; for (size_t i = 0; i < bound.size() - 1; ++i) { - const auto intersect_point = tier4_autoware_utils::intersect( + const auto intersect_point = autoware::universe_utils::intersect( min_lat_offset_point, max_lat_offset_point, bound.at(i), bound.at(i + 1)); if (intersect_point) { const bool is_point_left = isLeft(pose, *intersect_point); const double dist_to_bound = - tier4_autoware_utils::calcDistance2d(pose.position, *intersect_point) * + autoware::universe_utils::calcDistance2d(pose.position, *intersect_point) * (is_point_left ? 1.0 : -1.0); // the bound which is closest to the centerline will be chosen @@ -283,7 +283,7 @@ MPTOptimizer::MPTParam::MPTParam( void MPTOptimizer::MPTParam::onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; { // option updateParam(parameters, "mpt.option.steer_limit_constraint", steer_limit_constraint); @@ -564,7 +564,7 @@ std::vector MPTOptimizer::calcReferencePoints( constexpr double tmp_margin = 10.0; size_t ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); - ref_points = motion_utils::cropPoints( + ref_points = autoware::motion_utils::cropPoints( ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, backward_traj_length + tmp_margin); @@ -579,7 +579,7 @@ std::vector MPTOptimizer::calcReferencePoints( // 4. crop backward // NOTE: Start point may change. Spline calculation is required. - ref_points = motion_utils::cropPoints( + ref_points = autoware::motion_utils::cropPoints( ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, backward_traj_length); ref_points_spline = SplineInterpolationPoints2d(ref_points); @@ -605,7 +605,7 @@ std::vector MPTOptimizer::calcReferencePoints( updateExtraPoints(ref_points); // 9. crop forward - // ref_points = motion_utils::cropForwardPoints( + // ref_points = autoware::motion_utils::cropForwardPoints( // ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length); if (static_cast(mpt_param_.num_points) < ref_points.size()) { ref_points.resize(mpt_param_.num_points); @@ -623,7 +623,7 @@ void MPTOptimizer::updateOrientation( const auto yaw_vec = ref_points_spline.getSplineInterpolatedYaws(); for (size_t i = 0; i < ref_points.size(); ++i) { ref_points.at(i).pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw_vec.at(i)); + autoware::universe_utils::createQuaternionFromYaw(yaw_vec.at(i)); } } @@ -691,7 +691,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points ref_points.at(i).delta_arc_length = (i == ref_points.size() - 1) ? 0.0 - : tier4_autoware_utils::calcDistance2d(ref_points.at(i + 1), ref_points.at(i)); + : autoware::universe_utils::calcDistance2d(ref_points.at(i + 1), ref_points.at(i)); } } @@ -704,14 +704,14 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c const auto front_wheel_pos = trajectory_utils::getNearestPosition(ref_points, i, vehicle_info_.wheel_base_m); - const bool are_too_close_points = - tier4_autoware_utils::calcDistance2d(front_wheel_pos, ref_points.at(i).pose.position) < 1e-03; - const auto front_wheel_yaw = - are_too_close_points - ? ref_points.at(i).getYaw() - : tier4_autoware_utils::calcAzimuthAngle(ref_points.at(i).pose.position, front_wheel_pos); + const bool are_too_close_points = autoware::universe_utils::calcDistance2d( + front_wheel_pos, ref_points.at(i).pose.position) < 1e-03; + const auto front_wheel_yaw = are_too_close_points + ? ref_points.at(i).getYaw() + : autoware::universe_utils::calcAzimuthAngle( + ref_points.at(i).pose.position, front_wheel_pos); ref_points.at(i).alpha = - tier4_autoware_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).getYaw()); + autoware::universe_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).getYaw()); } { // avoidance @@ -771,10 +771,10 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c if (prev_ref_points_ptr_ && !prev_ref_points_ptr_->empty()) { for (int i = 0; i < static_cast(ref_points.size()); ++i) { const size_t prev_idx = trajectory_utils::findEgoIndex( - *prev_ref_points_ptr_, tier4_autoware_utils::getPose(ref_points.at(i)), + *prev_ref_points_ptr_, autoware::universe_utils::getPose(ref_points.at(i)), ego_nearest_param_); - const double dist_to_prev = tier4_autoware_utils::calcDistance2d( + const double dist_to_prev = autoware::universe_utils::calcDistance2d( ref_points.at(i), prev_ref_points_ptr_->at(prev_idx)); if (max_dist_threshold < dist_to_prev) { continue; @@ -954,7 +954,6 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_poin // much. b.lower_bound = std::min(b.lower_bound, original_b.upper_bound - min_dynamic_drivable_width_vec.at(p_idx)); - continue; } // extend longitudinal if it overlaps out_of_upper_bound_sections if (upper_bound_start_idx) { @@ -1004,7 +1003,6 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_poin // much. b.upper_bound = std::max(b.upper_bound, original_b.lower_bound + min_dynamic_drivable_width_vec.at(p_idx)); - continue; } // extend longitudinal if it overlaps out_of_lower_bound_sections if (lower_bound_start_idx) { @@ -1081,7 +1079,8 @@ void MPTOptimizer::avoidSuddenSteering( return; } const size_t prev_ego_idx = trajectory_utils::findEgoIndex( - *prev_ref_points_ptr_, tier4_autoware_utils::getPose(ref_points.front()), ego_nearest_param_); + *prev_ref_points_ptr_, autoware::universe_utils::getPose(ref_points.front()), + ego_nearest_param_); const double max_bound_fixing_length = ego_vel * mpt_param_.max_bound_fixing_time; const int max_bound_fixing_idx = @@ -1128,11 +1127,11 @@ void MPTOptimizer::updateVehicleBounds( collision_check_pose.position.y - ref_point.pose.position.y, collision_check_pose.position.x - ref_point.pose.position.x); const double offset_y = - -tier4_autoware_utils::calcDistance2d(ref_point, collision_check_pose) * + -autoware::universe_utils::calcDistance2d(ref_point, collision_check_pose) * std::sin(tmp_yaw - collision_check_yaw); const auto vehicle_bounds_pose = - tier4_autoware_utils::calcOffsetPose(collision_check_pose, 0.0, offset_y, 0.0); + autoware::universe_utils::calcOffsetPose(collision_check_pose, 0.0, offset_y, 0.0); // interpolate bounds const auto bounds = [&]() { @@ -1586,7 +1585,7 @@ Eigen::VectorXd MPTOptimizer::calcInitialSolutionForManualWarmStart( Eigen::VectorXd u0 = Eigen::VectorXd::Zero(D_un); - const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( prev_ref_points, ref_points.front().pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); @@ -1712,17 +1711,17 @@ void MPTOptimizer::publishDebugTrajectories( time_keeper_ptr_->tic(__func__); // reference points - const auto ref_traj = motion_utils::convertToTrajectory( + const auto ref_traj = autoware::motion_utils::convertToTrajectory( trajectory_utils::convertToTrajectoryPoints(ref_points), header); debug_ref_traj_pub_->publish(ref_traj); // fixed reference points const auto fixed_traj_points = extractFixedPoints(ref_points); - const auto fixed_traj = motion_utils::convertToTrajectory(fixed_traj_points, header); + const auto fixed_traj = autoware::motion_utils::convertToTrajectory(fixed_traj_points, header); debug_fixed_traj_pub_->publish(fixed_traj); // mpt points - const auto mpt_traj = motion_utils::convertToTrajectory(mpt_traj_points, header); + const auto mpt_traj = autoware::motion_utils::convertToTrajectory(mpt_traj_points, header); debug_mpt_traj_pub_->publish(mpt_traj); time_keeper_ptr_->toc(__func__, " "); diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index d9df26938a9f8..3899867a9dcce 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_optimizer/node.hpp" +#include "autoware/path_optimizer/node.hpp" -#include "autoware_path_optimizer/debug_marker.hpp" -#include "autoware_path_optimizer/utils/geometry_utils.hpp" -#include "autoware_path_optimizer/utils/trajectory_utils.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/path_optimizer/debug_marker.hpp" +#include "autoware/path_optimizer/utils/geometry_utils.hpp" +#include "autoware/path_optimizer/utils/trajectory_utils.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include "rclcpp/time.hpp" #include @@ -56,7 +56,7 @@ Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data void setZeroVelocityAfterStopPoint(std::vector & traj_points) { - const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); + const auto opt_zero_vel_idx = autoware::motion_utils::searchZeroVelocityIndex(traj_points); if (opt_zero_vel_idx) { for (size_t i = opt_zero_vel_idx.value(); i < traj_points.size(); ++i) { traj_points.at(i).longitudinal_velocity_mps = 0.0; @@ -75,7 +75,7 @@ std::vector calcSegmentLengthVector(const std::vector & std::vector segment_length_vector; for (size_t i = 0; i < points.size() - 1; ++i) { const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); segment_length_vector.push_back(segment_length); } return segment_length_vector; @@ -150,14 +150,15 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) set_param_res_ = this->add_on_set_parameters_callback( std::bind(&PathOptimizer::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; // parameters for option updateParam( @@ -244,7 +245,8 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) "Backward path is NOT supported. Just converting path to trajectory"); const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); - const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); + const auto output_traj_msg = + autoware::motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); return; @@ -277,7 +279,7 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = - motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); + autoware::motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); } @@ -398,7 +400,7 @@ void PathOptimizer::applyInputVelocity( const size_t ego_seg_idx = trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_); - const auto cropped_points = motion_utils::cropForwardPoints( + const auto cropped_points = autoware::motion_utils::cropForwardPoints( input_traj_points, ego_pose.position, ego_seg_idx, optimized_traj_length + margin_traj_length); @@ -450,14 +452,15 @@ void PathOptimizer::applyInputVelocity( } // insert stop point explicitly - const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); + const auto stop_idx = + autoware::motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); if (stop_idx) { const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.value()).pose; - // NOTE: motion_utils::findNearestSegmentIndex is used instead of + // NOTE: autoware::motion_utils::findNearestSegmentIndex is used instead of // trajectory_utils::findEgoSegmentIndex // for the case where input_traj_points is much longer than output_traj_points, and the // former has a stop point but the latter will not have. - const auto stop_seg_idx = motion_utils::findNearestSegmentIndex( + const auto stop_seg_idx = autoware::motion_utils::findNearestSegmentIndex( output_traj_points, input_stop_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); @@ -468,10 +471,10 @@ void PathOptimizer::applyInputVelocity( } if (*stop_seg_idx == output_traj_points.size() - 2) { const double signed_projected_length_to_segment = - motion_utils::calcLongitudinalOffsetToSegment( + autoware::motion_utils::calcLongitudinalOffsetToSegment( output_traj_points, *stop_seg_idx, input_stop_pose.position); - const double segment_length = - motion_utils::calcSignedArcLength(output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); + const double segment_length = autoware::motion_utils::calcSignedArcLength( + output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); if (segment_length < signed_projected_length_to_segment) { // NOTE: input_stop_pose is outside output_traj_points. return false; @@ -530,10 +533,10 @@ void PathOptimizer::insertZeroVelocityOutsideDrivableArea( debug_data_ptr_->stop_pose_by_drivable_area = optimized_traj_points.at(*first_outside_idx).pose; const auto stop_idx = [&]() { const auto dist = - motion_utils::calcSignedArcLength(optimized_traj_points, 0, *first_outside_idx); + autoware::motion_utils::calcSignedArcLength(optimized_traj_points, 0, *first_outside_idx); const auto dist_with_margin = dist - vehicle_stop_margin_outside_drivable_area_; const auto first_outside_idx_with_margin = - motion_utils::insertTargetPoint(0, dist_with_margin, optimized_traj_points); + autoware::motion_utils::insertTargetPoint(0, dist_with_margin, optimized_traj_points); if (first_outside_idx_with_margin) { return *first_outside_idx_with_margin; } @@ -558,11 +561,11 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos { time_keeper_ptr_->tic(__func__); - auto virtual_wall_marker = motion_utils::createStopVirtualWallMarker( + auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); if (!enable_outside_drivable_area_stop_) { virtual_wall_marker.markers.front().color = - tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.5); + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.5); } virtual_wall_pub_->publish(virtual_wall_marker); @@ -661,7 +664,7 @@ void PathOptimizer::publishDebugData(const Header & header) const // publish trajectories const auto debug_extended_traj = - motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header); + autoware::motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header); debug_extended_traj_pub_->publish(debug_extended_traj); time_keeper_ptr_->toc(__func__, " "); diff --git a/planning/autoware_path_optimizer/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp index 6745872f50b02..11e31bfd2d459 100644 --- a/planning/autoware_path_optimizer/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_optimizer/replan_checker.hpp" +#include "autoware/path_optimizer/replan_checker.hpp" -#include "autoware_path_optimizer/utils/trajectory_utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/path_optimizer/utils/trajectory_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -39,7 +39,7 @@ ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_ne void ReplanChecker::onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam( parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); @@ -78,7 +78,7 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const // 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); + autoware::universe_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); if (max_ego_moving_dist_ < delta_dist) { RCLCPP_DEBUG( logger_, @@ -144,14 +144,14 @@ bool ReplanChecker::isPathAroundEgoChanged( // 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); + const double prev_ego_lat_offset = autoware::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); + autoware::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_) { @@ -174,17 +174,17 @@ bool ReplanChecker::isPathForwardChanged( 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); + const auto prev_forward_point = autoware::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); + autoware::motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); + const double forward_lat_offset = autoware::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; } @@ -205,7 +205,7 @@ bool ReplanChecker::isPathGoalChanged( } const double goal_moving_dist = - tier4_autoware_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + autoware::universe_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); if (goal_moving_dist < max_goal_moving_dist_) { return false; } diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index ec6a9e575be95..5aa2ab4ffbfa8 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_optimizer/state_equation_generator.hpp" +#include "autoware/path_optimizer/state_equation_generator.hpp" -#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware/path_optimizer/mpt_optimizer.hpp" namespace autoware::path_optimizer { diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 99eb85d27e054..bed9bbfa500c6 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_optimizer/utils/geometry_utils.hpp" +#include "autoware/path_optimizer/utils/geometry_utils.hpp" -#include "autoware_path_optimizer/mpt_optimizer.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/path_optimizer/mpt_optimizer.hpp" #include "tf2/utils.h" -#include +#include #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -39,17 +39,17 @@ namespace autoware::path_optimizer { namespace bg = boost::geometry; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; namespace { geometry_msgs::msg::Point getStartPoint( const std::vector & bound, const geometry_msgs::msg::Point & point) { - const size_t segment_idx = motion_utils::findNearestSegmentIndex(bound, point); + const size_t segment_idx = autoware::motion_utils::findNearestSegmentIndex(bound, point); const auto & curr_seg_point = bound.at(segment_idx); const auto & next_seg_point = bound.at(segment_idx); const Eigen::Vector2d first_to_target{point.x - curr_seg_point.x, point.y - curr_seg_point.y}; @@ -61,7 +61,8 @@ geometry_msgs::msg::Point getStartPoint( return bound.front(); } - const auto first_point = motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); + const auto first_point = + autoware::motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); if (first_point) { return *first_point; } @@ -84,7 +85,8 @@ bool isFrontDrivableArea( // ignore point behind of the front line const std::vector front_bound = {left_start_point, right_start_point}; - const double lat_dist_to_front_bound = motion_utils::calcLateralOffset(front_bound, point); + const double lat_dist_to_front_bound = + autoware::motion_utils::calcLateralOffset(front_bound, point); if (lat_dist_to_front_bound < min_dist) { return true; } @@ -134,13 +136,13 @@ bool isOutsideDrivableAreaFromRectangleFootprint( // calculate footprint corner points const auto top_left_pos = - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, base_to_left, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, base_to_front, base_to_left, 0.0).position; const auto top_right_pos = - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -base_to_right, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, base_to_front, -base_to_right, 0.0).position; const auto bottom_right_pos = - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -base_to_right, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, -base_to_right, 0.0).position; const auto bottom_left_pos = - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, base_to_left, 0.0).position; + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, base_to_left, 0.0).position; if (use_footprint_polygon_for_outside_drivable_area_check) { // calculate footprint polygon diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index 75e2b75b232e0..433d1d0995088 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_optimizer/utils/trajectory_utils.hpp" +#include "autoware/path_optimizer/utils/trajectory_utils.hpp" -#include "autoware_path_optimizer/mpt_optimizer.hpp" -#include "autoware_path_optimizer/utils/geometry_utils.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/path_optimizer/mpt_optimizer.hpp" +#include "autoware/path_optimizer/utils/geometry_utils.hpp" #include "tf2/utils.h" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -33,7 +33,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { template <> geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p) @@ -52,7 +52,7 @@ double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & { return p.longitudinal_velocity_mps; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils namespace autoware::path_optimizer { @@ -108,12 +108,12 @@ void compensateLastPose( const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; - const double dist = - tier4_autoware_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); + const double dist = autoware::universe_utils::calcDistance2d( + last_path_point.pose.position, last_traj_pose.position); const double norm_diff_yaw = [&]() { const double diff_yaw = tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); - return tier4_autoware_utils::normalizeRadian(diff_yaw); + return autoware::universe_utils::normalizeRadian(diff_yaw); }(); if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { traj_points.push_back(convertToTrajectoryPoint(last_path_point)); @@ -140,10 +140,10 @@ std::vector resampleTrajectoryPoints( { constexpr bool enable_resampling_stop_point = true; - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory( + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory( traj, interval, false, true, true, enable_resampling_stop_point); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); } // NOTE: stop point will not be resampled @@ -152,10 +152,10 @@ std::vector resampleTrajectoryPointsWithoutStopPoint( { constexpr bool enable_resampling_stop_point = false; - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory( + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory( traj, interval, false, true, true, enable_resampling_stop_point); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); } std::vector resampleReferencePoints( @@ -175,7 +175,7 @@ std::vector resampleReferencePoints( base_keys.push_back(0.0); } else { const double delta_arc_length = - tier4_autoware_utils::calcDistance2d(ref_points.at(i), ref_points.at(i - 1)); + autoware::universe_utils::calcDistance2d(ref_points.at(i), ref_points.at(i - 1)); base_keys.push_back(base_keys.back() + delta_arc_length); } @@ -187,7 +187,7 @@ std::vector resampleReferencePoints( if (i == 0) { query_keys.push_back(0.0); } else { - const double delta_arc_length = tier4_autoware_utils::calcDistance2d( + const double delta_arc_length = autoware::universe_utils::calcDistance2d( resampled_ref_points.at(i), resampled_ref_points.at(i - 1)); const double key = query_keys.back() + delta_arc_length; if (base_keys.back() < key) { @@ -220,7 +220,7 @@ void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx) { - const double offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + const double offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); const auto traj_spline = SplineInterpolationPoints2d(traj_points); diff --git a/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 89e9e0e2c9b74..58ea31545ef0f 100644 --- a/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include #include diff --git a/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp index 57e4671a4e0cc..55e0a61dac430 100644 --- a/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp +++ b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp" VehicleModelInterface::VehicleModelInterface( const int dim_x, const int dim_u, const int dim_y, const double wheelbase, diff --git a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp index 8be7806c7e48a..9d24702cf75ea 100644 --- a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp +++ b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_optimizer/node.hpp" +#include "autoware/path_optimizer/node.hpp" #include #include diff --git a/planning/autoware_path_smoother/include/autoware_path_smoother/common_structs.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp similarity index 88% rename from planning/autoware_path_smoother/include/autoware_path_smoother/common_structs.hpp rename to planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp index 18f5ef82fd6d4..0b7e28ee1cbb5 100644 --- a/planning/autoware_path_smoother/include/autoware_path_smoother/common_structs.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/common_structs.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_SMOOTHER__COMMON_STRUCTS_HPP_ -#define AUTOWARE_PATH_SMOOTHER__COMMON_STRUCTS_HPP_ +#ifndef AUTOWARE__PATH_SMOOTHER__COMMON_STRUCTS_HPP_ +#define AUTOWARE__PATH_SMOOTHER__COMMON_STRUCTS_HPP_ -#include "autoware_path_smoother/type_alias.hpp" +#include "autoware/path_smoother/type_alias.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -83,7 +83,7 @@ struct TimeKeeper double accumulated_time{0.0}; - tier4_autoware_utils::StopWatch< + autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; @@ -100,7 +100,7 @@ struct CommonParam void onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; // common updateParam( @@ -123,7 +123,7 @@ struct EgoNearestParam void onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "ego_nearest_dist_threshold", dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", yaw_threshold); } @@ -133,4 +133,4 @@ struct EgoNearestParam }; } // namespace autoware::path_smoother -#endif // AUTOWARE_PATH_SMOOTHER__COMMON_STRUCTS_HPP_ +#endif // AUTOWARE__PATH_SMOOTHER__COMMON_STRUCTS_HPP_ diff --git a/planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp similarity index 93% rename from planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band.hpp rename to planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp index 65b9db9098561..1d0441c66c481 100644 --- a/planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_HPP_ -#define AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_HPP_ +#ifndef AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_HPP_ +#define AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_HPP_ -#include "autoware_path_smoother/common_structs.hpp" -#include "autoware_path_smoother/type_alias.hpp" +#include "autoware/path_smoother/common_structs.hpp" +#include "autoware/path_smoother/type_alias.hpp" #include "osqp_interface/osqp_interface.hpp" #include @@ -130,4 +130,4 @@ class EBPathSmoother }; } // namespace autoware::path_smoother -#endif // AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_HPP_ +#endif // AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_HPP_ diff --git a/planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band_smoother.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp similarity index 76% rename from planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band_smoother.hpp rename to planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp index 16a4b2bb39db7..2de74a4f14f4f 100644 --- a/planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band_smoother.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ -#define AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ - -#include "autoware_path_smoother/common_structs.hpp" -#include "autoware_path_smoother/elastic_band.hpp" -#include "autoware_path_smoother/replan_checker.hpp" -#include "autoware_path_smoother/type_alias.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#ifndef AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ +#define AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/path_smoother/common_structs.hpp" +#include "autoware/path_smoother/elastic_band.hpp" +#include "autoware/path_smoother/replan_checker.hpp" +#include "autoware/path_smoother/type_alias.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include +#include #include #include @@ -49,7 +49,7 @@ class ElasticBandSmoother : public rclcpp::Node public: bool isDrivingForward(const std::vector & path_points) { - const auto is_driving_forward = motion_utils::isDrivingForward(path_points); + const auto is_driving_forward = autoware::motion_utils::isDrivingForward(path_points); is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; return is_driving_forward_; } @@ -82,7 +82,8 @@ class ElasticBandSmoother : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber odom_sub_{this, "~/input/odometry"}; + autoware::universe_utils::InterProcessPollingSubscriber odom_sub_{ + this, "~/input/odometry"}; // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; @@ -112,10 +113,10 @@ class ElasticBandSmoother : public rclcpp::Node const std::vector & traj_points, const std::vector & optimized_points) const; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::path_smoother -#endif // AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ +#endif // AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ diff --git a/planning/autoware_path_smoother/include/autoware_path_smoother/replan_checker.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/replan_checker.hpp similarity index 89% rename from planning/autoware_path_smoother/include/autoware_path_smoother/replan_checker.hpp rename to planning/autoware_path_smoother/include/autoware/path_smoother/replan_checker.hpp index 35b0e0102068b..8c33121b1b36b 100644 --- a/planning/autoware_path_smoother/include/autoware_path_smoother/replan_checker.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/replan_checker.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_SMOOTHER__REPLAN_CHECKER_HPP_ -#define AUTOWARE_PATH_SMOOTHER__REPLAN_CHECKER_HPP_ +#ifndef AUTOWARE__PATH_SMOOTHER__REPLAN_CHECKER_HPP_ +#define AUTOWARE__PATH_SMOOTHER__REPLAN_CHECKER_HPP_ -#include "autoware_path_smoother/common_structs.hpp" -#include "autoware_path_smoother/type_alias.hpp" +#include "autoware/path_smoother/common_structs.hpp" +#include "autoware/path_smoother/type_alias.hpp" #include @@ -68,4 +68,4 @@ class ReplanChecker }; } // namespace autoware::path_smoother -#endif // AUTOWARE_PATH_SMOOTHER__REPLAN_CHECKER_HPP_ +#endif // AUTOWARE__PATH_SMOOTHER__REPLAN_CHECKER_HPP_ diff --git a/planning/autoware_path_smoother/include/autoware_path_smoother/type_alias.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/type_alias.hpp similarity index 90% rename from planning/autoware_path_smoother/include/autoware_path_smoother/type_alias.hpp rename to planning/autoware_path_smoother/include/autoware/path_smoother/type_alias.hpp index dc565d69d3c4d..5ec4b21955892 100644 --- a/planning/autoware_path_smoother/include/autoware_path_smoother/type_alias.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_SMOOTHER__TYPE_ALIAS_HPP_ -#define AUTOWARE_PATH_SMOOTHER__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE__PATH_SMOOTHER__TYPE_ALIAS_HPP_ +#define AUTOWARE__PATH_SMOOTHER__TYPE_ALIAS_HPP_ #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -41,4 +41,4 @@ using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; } // namespace autoware::path_smoother -#endif // AUTOWARE_PATH_SMOOTHER__TYPE_ALIAS_HPP_ +#endif // AUTOWARE__PATH_SMOOTHER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_path_smoother/include/autoware_path_smoother/utils/geometry_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp similarity index 72% rename from planning/autoware_path_smoother/include/autoware_path_smoother/utils/geometry_utils.hpp rename to planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp index 6cdf34f319442..603ae8a91f944 100644 --- a/planning/autoware_path_smoother/include/autoware_path_smoother/utils/geometry_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/geometry_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ -#define AUTOWARE_PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ +#ifndef AUTOWARE__PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ +#define AUTOWARE__PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ -#include +#include namespace autoware::path_smoother { @@ -24,12 +24,12 @@ namespace geometry_utils template bool isSamePoint(const T1 & t1, const T2 & t2) { - const auto p1 = tier4_autoware_utils::getPoint(t1); - const auto p2 = tier4_autoware_utils::getPoint(t2); + const auto p1 = autoware::universe_utils::getPoint(t1); + const auto p2 = autoware::universe_utils::getPoint(t2); constexpr double epsilon = 1e-6; return (std::abs(p1.x - p2.x) <= epsilon && std::abs(p1.y - p2.y) <= epsilon); } } // namespace geometry_utils } // namespace autoware::path_smoother -#endif // AUTOWARE_PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ +#endif // AUTOWARE__PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/autoware_path_smoother/include/autoware_path_smoother/utils/trajectory_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp similarity index 82% rename from planning/autoware_path_smoother/include/autoware_path_smoother/utils/trajectory_utils.hpp rename to planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp index ea717fae20ef4..77a9dbae81a0c 100644 --- a/planning/autoware_path_smoother/include/autoware_path_smoother/utils/trajectory_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ -#define AUTOWARE_PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE__PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE__PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ -#include "autoware_path_smoother/common_structs.hpp" -#include "autoware_path_smoother/type_alias.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/path_smoother/common_structs.hpp" +#include "autoware/path_smoother/type_alias.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include @@ -49,14 +49,14 @@ std::optional getPointIndexAfter( } double sum_length = - -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + -autoware::motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); std::optional output_idx{std::nullopt}; // search forward if (sum_length < min_offset) { for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (min_offset < sum_length) { output_idx = i - 1; } @@ -70,7 +70,7 @@ std::optional getPointIndexAfter( // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value - sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < min_offset) { output_idx = i - 1; } @@ -86,7 +86,7 @@ template TrajectoryPoint convertToTrajectoryPoint(const T & point) { TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.pose = autoware::universe_utils::getPose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -109,7 +109,7 @@ size_t findEgoSegmentIndex( const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, const EgoNearestParam & ego_nearest_param) { - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } @@ -128,13 +128,13 @@ std::optional updateFrontPointForFix( { // calculate front point to insert in points as a fixed point const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( - points_for_fix, tier4_autoware_utils::getPose(points.front()), ego_nearest_param); + points_for_fix, autoware::universe_utils::getPose(points.front()), ego_nearest_param); const size_t front_point_idx_for_fix = front_seg_idx_for_fix; const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); // check if the points_for_fix is longer in front than points const double lon_offset_to_prev_front = - motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); + autoware::motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { RCLCPP_DEBUG( rclcpp::get_logger("autoware_path_smoother.trajectory_utils"), @@ -142,7 +142,7 @@ std::optional updateFrontPointForFix( return std::nullopt; } - const double dist = tier4_autoware_utils::calcDistance2d(points.front(), front_fix_point); + const double dist = autoware::universe_utils::calcDistance2d(points.front(), front_fix_point); // check if deviation is not too large constexpr double max_lat_error = 3.0; @@ -171,4 +171,4 @@ void insertStopPoint( const size_t stop_seg_idx); } // namespace trajectory_utils } // namespace autoware::path_smoother -#endif // AUTOWARE_PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ +#endif // AUTOWARE__PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index 445f8a0e6e736..6cecef433fc3e 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -14,18 +14,18 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager + autoware_universe_utils geometry_msgs interpolation - motion_utils nav_msgs osqp_interface rclcpp rclcpp_components std_msgs tf2_ros - tier4_autoware_utils tier4_debug_msgs ament_cmake_ros diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index a443cfd37caec..4222e0fe98438 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_smoother/elastic_band.hpp" +#include "autoware/path_smoother/elastic_band.hpp" -#include "autoware_path_smoother/type_alias.hpp" -#include "autoware_path_smoother/utils/geometry_utils.hpp" -#include "autoware_path_smoother/utils/trajectory_utils.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/path_smoother/type_alias.hpp" +#include "autoware/path_smoother/utils/geometry_utils.hpp" +#include "autoware/path_smoother/utils/trajectory_utils.hpp" #include "tf2/utils.h" #include @@ -125,7 +125,7 @@ EBPathSmoother::EBParam::EBParam(rclcpp::Node * node) void EBPathSmoother::EBParam::onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; { // option updateParam(parameters, "elastic_band.option.enable_warm_start", enable_warm_start); @@ -212,7 +212,7 @@ std::vector EBPathSmoother::smoothTrajectory( const size_t ego_seg_idx = trajectory_utils::findEgoSegmentIndex(traj_points, ego_pose, ego_nearest_param_); - const auto cropped_traj_points = motion_utils::cropPoints( + const auto cropped_traj_points = autoware::motion_utils::cropPoints( traj_points, ego_pose.position, ego_seg_idx, forward_traj_length, backward_traj_length); // check if goal is contained in cropped_traj_points @@ -263,7 +263,7 @@ std::vector EBPathSmoother::smoothTrajectory( // 8. publish eb trajectory const auto eb_traj = - motion_utils::convertToTrajectory(*eb_traj_points, createHeader(clock_.now())); + autoware::motion_utils::convertToTrajectory(*eb_traj_points, createHeader(clock_.now())); debug_eb_traj_pub_->publish(eb_traj); time_keeper_ptr_->toc(__func__, " "); @@ -389,8 +389,8 @@ void EBPathSmoother::updateConstraint( } // publish fixed trajectory - const auto eb_fixed_traj = - motion_utils::convertToTrajectory(debug_fixed_traj_points, createHeader(clock_.now())); + const auto eb_fixed_traj = autoware::motion_utils::convertToTrajectory( + debug_fixed_traj_points, createHeader(clock_.now())); debug_eb_fixed_traj_pub_->publish(eb_fixed_traj); time_keeper_ptr_->toc(__func__, " "); @@ -443,12 +443,12 @@ std::optional> EBPathSmoother::convertOptimizedPoin auto eb_traj_point = traj_points.at(i); eb_traj_point.pose = - tier4_autoware_utils::calcOffsetPose(eb_traj_point.pose, 0.0, lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(eb_traj_point.pose, 0.0, lat_offset, 0.0); eb_traj_points.push_back(eb_traj_point); } // update orientation - motion_utils::insertOrientation(eb_traj_points, true); + autoware::motion_utils::insertOrientation(eb_traj_points, true); time_keeper_ptr_->toc(__func__, " "); return eb_traj_points; diff --git a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp index 2f507c8bae311..b92798f92728c 100644 --- a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp +++ b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_smoother/elastic_band_smoother.hpp" +#include "autoware/path_smoother/elastic_band_smoother.hpp" -#include "autoware_path_smoother/utils/geometry_utils.hpp" -#include "autoware_path_smoother/utils/trajectory_utils.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/path_smoother/utils/geometry_utils.hpp" +#include "autoware/path_smoother/utils/trajectory_utils.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include "rclcpp/time.hpp" #include @@ -54,7 +54,7 @@ Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data void setZeroVelocityAfterStopPoint(std::vector & traj_points) { - const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); + const auto opt_zero_vel_idx = autoware::motion_utils::searchZeroVelocityIndex(traj_points); if (opt_zero_vel_idx) { for (size_t i = opt_zero_vel_idx.value(); i < traj_points.size(); ++i) { traj_points.at(i).longitudinal_velocity_mps = 0.0; @@ -105,14 +105,15 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ElasticBandSmoother::onParam, this, std::placeholders::_1)); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; // parameters for ego nearest search ego_nearest_param_.onParam(parameters); @@ -168,7 +169,8 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) "Backward path is NOT supported. Just converting path to trajectory"); const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); - const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); + const auto output_traj_msg = + autoware::motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); path_pub_->publish(*path_ptr); published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp); @@ -222,7 +224,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = - motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); + autoware::motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); const auto output_path_msg = trajectory_utils::create_path(*path_ptr, full_traj_points); path_pub_->publish(output_path_msg); @@ -259,12 +261,12 @@ void ElasticBandSmoother::applyInputVelocity( time_keeper_ptr_->tic(__func__); // crop forward for faster calculation - const double output_traj_length = motion_utils::calcArcLength(output_traj_points); + const double output_traj_length = autoware::motion_utils::calcArcLength(output_traj_points); constexpr double margin_traj_length = 10.0; const auto forward_cropped_input_traj_points = [&]() { const size_t ego_seg_idx = trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_); - return motion_utils::cropForwardPoints( + return autoware::motion_utils::cropForwardPoints( input_traj_points, ego_pose.position, ego_seg_idx, output_traj_length + margin_traj_length); }(); @@ -286,14 +288,15 @@ void ElasticBandSmoother::applyInputVelocity( } // insert stop point explicitly - const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); + const auto stop_idx = + autoware::motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); if (stop_idx) { const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.value()).pose; - // NOTE: motion_utils::findNearestSegmentIndex is used instead of + // NOTE: autoware::motion_utils::findNearestSegmentIndex is used instead of // trajectory_utils::findEgoSegmentIndex // for the case where input_traj_points is much longer than output_traj_points, and the // former has a stop point but the latter will not have. - const auto stop_seg_idx = motion_utils::findNearestSegmentIndex( + const auto stop_seg_idx = autoware::motion_utils::findNearestSegmentIndex( output_traj_points, input_stop_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); @@ -304,10 +307,10 @@ void ElasticBandSmoother::applyInputVelocity( } if (*stop_seg_idx == output_traj_points.size() - 2) { const double signed_projected_length_to_segment = - motion_utils::calcLongitudinalOffsetToSegment( + autoware::motion_utils::calcLongitudinalOffsetToSegment( output_traj_points, *stop_seg_idx, input_stop_pose.position); - const double segment_length = - motion_utils::calcSignedArcLength(output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); + const double segment_length = autoware::motion_utils::calcSignedArcLength( + output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); if (segment_length < signed_projected_length_to_segment) { // NOTE: input_stop_pose is outside output_traj_points. return false; diff --git a/planning/autoware_path_smoother/src/replan_checker.cpp b/planning/autoware_path_smoother/src/replan_checker.cpp index 4e5911ea5b42e..5d2e7a05e4b17 100644 --- a/planning/autoware_path_smoother/src/replan_checker.cpp +++ b/planning/autoware_path_smoother/src/replan_checker.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_smoother/replan_checker.hpp" +#include "autoware/path_smoother/replan_checker.hpp" -#include "autoware_path_smoother/utils/trajectory_utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/path_smoother/utils/trajectory_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -40,7 +40,7 @@ ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_ne void ReplanChecker::onParam(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; updateParam(parameters, "replan.enable", enable_); updateParam( @@ -80,7 +80,7 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const // 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); + autoware::universe_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); if (max_ego_moving_dist_ < delta_dist) { RCLCPP_DEBUG( logger_, @@ -140,14 +140,14 @@ bool ReplanChecker::isPathAroundEgoChanged( // 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); + const double prev_ego_lat_offset = autoware::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); + autoware::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_) { @@ -170,17 +170,17 @@ bool ReplanChecker::isPathForwardChanged( 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); + const auto prev_forward_point = autoware::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); + autoware::motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); + const double forward_lat_offset = autoware::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; } @@ -201,7 +201,7 @@ bool ReplanChecker::isPathGoalChanged( } const double goal_moving_dist = - tier4_autoware_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + autoware::universe_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); if (goal_moving_dist < max_goal_moving_dist_) { return false; } diff --git a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp index 5fea20c9a405f..4a36359ded4f3 100644 --- a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_smoother/utils/trajectory_utils.hpp" +#include "autoware/path_smoother/utils/trajectory_utils.hpp" -#include "autoware_path_smoother/utils/geometry_utils.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/path_smoother/utils/geometry_utils.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -54,10 +54,10 @@ std::vector resampleTrajectoryPoints( { constexpr bool enable_resampling_stop_point = true; - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory( + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory( traj, interval, false, true, true, enable_resampling_stop_point); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); } // NOTE: stop point will not be resampled @@ -66,17 +66,17 @@ std::vector resampleTrajectoryPointsWithoutStopPoint( { constexpr bool enable_resampling_stop_point = false; - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory( + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory( traj, interval, false, true, true, enable_resampling_stop_point); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); } void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx) { - const double offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + const double offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); const auto traj_spline = SplineInterpolationPoints2d(traj_points); diff --git a/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp index ed24c50325330..7c92763f94521 100644 --- a/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp +++ b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_path_smoother/elastic_band_smoother.hpp" +#include "autoware/path_smoother/elastic_band_smoother.hpp" #include #include diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp index 0e86740bd5b9a..9a93b647dac02 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp @@ -14,7 +14,7 @@ #ifndef AUTOWARE_PLANNING_TEST_MANAGER__AUTOWARE_PLANNING_TEST_MANAGER_UTILS_HPP_ #define AUTOWARE_PLANNING_TEST_MANAGER__AUTOWARE_PLANNING_TEST_MANAGER_UTILS_HPP_ -#include +#include #include #include @@ -57,8 +57,8 @@ Pose createPoseFromLaneID(const lanelet::Id & lane_id) // calculate middle pose geometry_msgs::msg::Pose middle_pose; middle_pose.position = middle_pos; - const double yaw = tier4_autoware_utils::calcAzimuthAngle(middle_pos, next_middle_pos); - middle_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + const double yaw = autoware::universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); + middle_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); return middle_pose; } diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index 6ccebfc2f56fe..416528257fe3b 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -15,22 +15,22 @@ autoware_control_msgs autoware_map_msgs + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler autoware_test_utils + autoware_universe_utils autoware_vehicle_msgs component_interface_specs component_interface_utils lanelet2_extension lanelet2_io - motion_utils nav_msgs rclcpp tf2_msgs tf2_ros tier4_api_msgs - tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs unique_identifier_msgs diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 715dc6277f419..1526e061ca4d1 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include #include #include #include -#include namespace autoware::planning_test_manager { @@ -296,7 +296,7 @@ void PlanningInterfaceTestManager::publishNominalPath( { autoware::test_utils::publishToTargetNode( test_node_, target_node, topic_name, normal_path_pub_, - motion_utils::convertToPath( + autoware::motion_utils::convertToPath( autoware::test_utils::loadPathWithLaneIdInYaml()), 5); } diff --git a/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp b/planning/autoware_planning_topic_converter/include/autoware/planning_topic_converter/converter_base.hpp similarity index 88% rename from planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp rename to planning/autoware_planning_topic_converter/include/autoware/planning_topic_converter/converter_base.hpp index e3b0fe5069e92..78de864f6ba41 100644 --- a/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp +++ b/planning/autoware_planning_topic_converter/include/autoware/planning_topic_converter/converter_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ -#define AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#ifndef AUTOWARE__PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#define AUTOWARE__PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -48,4 +48,4 @@ class ConverterBase : public rclcpp::Node } // namespace autoware::planning_topic_converter -#endif // AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#endif // AUTOWARE__PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ diff --git a/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp b/planning/autoware_planning_topic_converter/include/autoware/planning_topic_converter/path_to_trajectory.hpp similarity index 82% rename from planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp rename to planning/autoware_planning_topic_converter/include/autoware/planning_topic_converter/path_to_trajectory.hpp index 7ca50d6c4e50c..5feec380e239d 100644 --- a/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp +++ b/planning/autoware_planning_topic_converter/include/autoware/planning_topic_converter/path_to_trajectory.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ -#define AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#ifndef AUTOWARE__PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#define AUTOWARE__PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ -#include "autoware_planning_topic_converter/converter_base.hpp" +#include "autoware/planning_topic_converter/converter_base.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -42,4 +42,4 @@ class PathToTrajectory : public ConverterBase } // namespace autoware::planning_topic_converter -#endif // AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#endif // AUTOWARE__PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ diff --git a/planning/autoware_planning_topic_converter/package.xml b/planning/autoware_planning_topic_converter/package.xml index be44523fa9852..43009b2921e16 100644 --- a/planning/autoware_planning_topic_converter/package.xml +++ b/planning/autoware_planning_topic_converter/package.xml @@ -14,11 +14,11 @@ ament_cmake_auto + autoware_motion_utils autoware_planning_msgs - motion_utils + autoware_universe_utils rclcpp rclcpp_components - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp index d7aceb1abd106..ff2f3084fc9ba 100644 --- a/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_planning_topic_converter/path_to_trajectory.hpp" +#include "autoware/planning_topic_converter/path_to_trajectory.hpp" -#include -#include +#include +#include namespace autoware::planning_topic_converter { @@ -24,7 +24,7 @@ namespace TrajectoryPoint convertToTrajectoryPoint(const PathPoint & point) { TrajectoryPoint traj_point; - traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.pose = autoware::universe_utils::getPose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -50,7 +50,7 @@ PathToTrajectory::PathToTrajectory(const rclcpp::NodeOptions & options) void PathToTrajectory::process(const Path::ConstSharedPtr msg) { const auto trajectory_points = convertToTrajectoryPoints(msg->points); - const auto output = motion_utils::convertToTrajectory(trajectory_points, msg->header); + const auto output = autoware::motion_utils::convertToTrajectory(trajectory_points, msg->header); pub_->publish(output); } diff --git a/planning/autoware_planning_validator/CMakeLists.txt b/planning/autoware_planning_validator/CMakeLists.txt index da780cfdebdcf..d896fa323a619 100644 --- a/planning/autoware_planning_validator/CMakeLists.txt +++ b/planning/autoware_planning_validator/CMakeLists.txt @@ -11,7 +11,7 @@ ament_auto_add_library(autoware_planning_validator_helpers SHARED # planning validator ament_auto_add_library(autoware_planning_validator_component SHARED - include/autoware_planning_validator/planning_validator.hpp + include/autoware/planning_validator/planning_validator.hpp src/planning_validator.cpp ) target_link_libraries(autoware_planning_validator_component autoware_planning_validator_helpers) diff --git a/planning/autoware_planning_validator/include/autoware_planning_validator/debug_marker.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/debug_marker.hpp similarity index 91% rename from planning/autoware_planning_validator/include/autoware_planning_validator/debug_marker.hpp rename to planning/autoware_planning_validator/include/autoware/planning_validator/debug_marker.hpp index 955ba5bbf64ab..5e8fd31734121 100644 --- a/planning/autoware_planning_validator/include/autoware_planning_validator/debug_marker.hpp +++ b/planning/autoware_planning_validator/include/autoware/planning_validator/debug_marker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ -#define AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE__PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#define AUTOWARE__PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ #include @@ -57,4 +57,4 @@ class PlanningValidatorDebugMarkerPublisher } }; -#endif // AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#endif // AUTOWARE__PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/planning/autoware_planning_validator/include/autoware_planning_validator/planning_validator.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp similarity index 85% rename from planning/autoware_planning_validator/include/autoware_planning_validator/planning_validator.hpp rename to planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp index b2a9c8f2e94a4..e996855b9b4da 100644 --- a/planning/autoware_planning_validator/include/autoware_planning_validator/planning_validator.hpp +++ b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ -#define AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#ifndef AUTOWARE__PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#define AUTOWARE__PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ -#include "autoware_planning_validator/debug_marker.hpp" +#include "autoware/planning_validator/debug_marker.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware_planning_validator/msg/planning_validator_status.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include #include #include -#include #include #include @@ -36,13 +36,13 @@ namespace autoware::planning_validator { +using autoware::universe_utils::StopWatch; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_planning_validator::msg::PlanningValidatorStatus; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; -using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; struct ValidationParams @@ -103,7 +103,7 @@ class PlanningValidator : public rclcpp::Node void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); - tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_kinematics_{ this, "~/input/kinematics"}; rclcpp::Subscription::SharedPtr sub_traj_; rclcpp::Publisher::SharedPtr pub_traj_; @@ -137,12 +137,12 @@ class PlanningValidator : public rclcpp::Node std::shared_ptr debug_pose_publisher_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; StopWatch stop_watch_; }; } // namespace autoware::planning_validator -#endif // AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#endif // AUTOWARE__PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ diff --git a/planning/autoware_planning_validator/include/autoware_planning_validator/utils.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp similarity index 93% rename from planning/autoware_planning_validator/include/autoware_planning_validator/utils.hpp rename to planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp index 1c943f5a60e08..04cfed07a1671 100644 --- a/planning/autoware_planning_validator/include/autoware_planning_validator/utils.hpp +++ b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ -#define AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ +#ifndef AUTOWARE__PLANNING_VALIDATOR__UTILS_HPP_ +#define AUTOWARE__PLANNING_VALIDATOR__UTILS_HPP_ #include @@ -67,4 +67,4 @@ void shiftPose(geometry_msgs::msg::Pose & pose, double longitudinal); } // namespace autoware::planning_validator -#endif // AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ +#endif // AUTOWARE__PLANNING_VALIDATOR__UTILS_HPP_ diff --git a/planning/autoware_planning_validator/package.xml b/planning/autoware_planning_validator/package.xml index 541396b52b02b..f19caabaeb3ff 100644 --- a/planning/autoware_planning_validator/package.xml +++ b/planning/autoware_planning_validator/package.xml @@ -15,16 +15,16 @@ autoware_cmake rosidl_default_generators + autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager + autoware_universe_utils autoware_vehicle_info_utils diagnostic_updater geometry_msgs - motion_utils nav_msgs rclcpp rclcpp_components - tier4_autoware_utils visualization_msgs ament_cmake_ros diff --git a/planning/autoware_planning_validator/src/debug_marker.cpp b/planning/autoware_planning_validator/src/debug_marker.cpp index 413611a72b5ab..7de23de8eda01 100644 --- a/planning/autoware_planning_validator/src/debug_marker.cpp +++ b/planning/autoware_planning_validator/src/debug_marker.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_planning_validator/debug_marker.hpp" +#include "autoware/planning_validator/debug_marker.hpp" -#include -#include +#include +#include #include #include @@ -48,7 +48,7 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) { - using tier4_autoware_utils::createMarkerColor; + using autoware::universe_utils::createMarkerColor; // append arrow marker std_msgs::msg::ColorRGBA color; @@ -64,9 +64,9 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( { color = createMarkerColor(0.0, 0.0, 1.0, 0.999); } - Marker marker = tier4_autoware_utils::createDefaultMarker( + Marker marker = autoware::universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, - tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3), color); + autoware::universe_utils::createMarkerScale(0.2, 0.1, 0.3), color); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; @@ -76,10 +76,10 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( void PlanningValidatorDebugMarkerPublisher::pushWarningMsg( const geometry_msgs::msg::Pose & pose, const std::string & msg) { - visualization_msgs::msg::Marker marker = tier4_autoware_utils::createDefaultMarker( + visualization_msgs::msg::Marker marker = autoware::universe_utils::createDefaultMarker( "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); + autoware::universe_utils::createMarkerScale(0.0, 0.0, 1.0), + autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(0.2); marker.pose = pose; marker.text = msg; @@ -89,9 +89,9 @@ void PlanningValidatorDebugMarkerPublisher::pushWarningMsg( void PlanningValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs::msg::Pose & pose) { const auto now = node_->get_clock()->now(); - const auto stop_wall_marker = - motion_utils::createStopVirtualWallMarker(pose, "autoware_planning_validator", now, 0); - tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); + const auto stop_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( + pose, "autoware_planning_validator", now, 0); + autoware::universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } void PlanningValidatorDebugMarkerPublisher::publish() diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp index 9f54547eb62e1..29e8932c87842 100644 --- a/planning/autoware_planning_validator/src/planning_validator.cpp +++ b/planning/autoware_planning_validator/src/planning_validator.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_planning_validator/planning_validator.hpp" +#include "autoware/planning_validator/planning_validator.hpp" -#include "autoware_planning_validator/utils.hpp" +#include "autoware/planning_validator/utils.hpp" -#include -#include +#include +#include #include #include @@ -44,8 +44,9 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) setupParameters(); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void PlanningValidator::setupParameters() @@ -449,7 +450,7 @@ bool PlanningValidator::checkValidSteeringRate(const Trajectory & trajectory) bool PlanningValidator::checkValidVelocityDeviation(const Trajectory & trajectory) { // TODO(horibe): set appropriate thresholds for index search - const auto idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, current_kinematics_->pose.pose); validation_status_.velocity_deviation = std::abs( @@ -465,11 +466,11 @@ bool PlanningValidator::checkValidVelocityDeviation(const Trajectory & trajector bool PlanningValidator::checkValidDistanceDeviation(const Trajectory & trajectory) { // TODO(horibe): set appropriate thresholds for index search - const auto idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, current_kinematics_->pose.pose); - validation_status_.distance_deviation = - tier4_autoware_utils::calcDistance2d(trajectory.points.at(idx), current_kinematics_->pose.pose); + validation_status_.distance_deviation = autoware::universe_utils::calcDistance2d( + trajectory.points.at(idx), current_kinematics_->pose.pose); if (validation_status_.distance_deviation > validation_params_.distance_deviation_threshold) { return false; @@ -486,7 +487,7 @@ bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory const auto ego_pose = current_kinematics_->pose.pose; const size_t idx = - motion_utils::findFirstNearestIndexWithSoftConstraints(trajectory.points, ego_pose); + autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(trajectory.points, ego_pose); if (0 < idx && idx < trajectory.points.size() - 1) { return true; // ego-nearest point exists between trajectory points. @@ -494,13 +495,13 @@ bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory // Check if the valid longitudinal deviation for given segment index const auto HasValidLongitudinalDeviation = [&](const size_t seg_idx, const bool is_last) { - auto long_offset = - motion_utils::calcLongitudinalOffsetToSegment(trajectory.points, seg_idx, ego_pose.position); + auto long_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( + trajectory.points, seg_idx, ego_pose.position); // for last, need to remove distance for the last segment. if (is_last) { const auto size = trajectory.points.size(); - long_offset -= tier4_autoware_utils::calcDistance2d( + long_offset -= autoware::universe_utils::calcDistance2d( trajectory.points.at(size - 1), trajectory.points.at(size - 2)); } @@ -531,7 +532,7 @@ bool PlanningValidator::checkValidForwardTrajectoryLength(const Trajectory & tra return true; // Ego is almost stopped. } - const auto forward_length = motion_utils::calcSignedArcLength( + const auto forward_length = autoware::motion_utils::calcSignedArcLength( trajectory.points, current_kinematics_->pose.pose.position, trajectory.points.size() - 1); const auto acc = validation_params_.forward_trajectory_length_acceleration; diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp index 4710d3ae84ea3..ae580a7a7c6f1 100644 --- a/planning/autoware_planning_validator/src/utils.cpp +++ b/planning/autoware_planning_validator/src/utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_planning_validator/utils.hpp" +#include "autoware/planning_validator/utils.hpp" -#include -#include +#include +#include #include #include @@ -24,9 +24,9 @@ namespace autoware::planning_validator { -using tier4_autoware_utils::calcCurvature; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::getPoint; +using autoware::universe_utils::calcCurvature; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::getPoint; namespace { @@ -138,7 +138,7 @@ void calcCurvature( const auto p2 = getPoint(trajectory.points.at(i)); const auto p3 = getPoint(trajectory.points.at(next_idx)); try { - curvature_arr.at(i) = tier4_autoware_utils::calcCurvature(p1, p2, p3); + curvature_arr.at(i) = autoware::universe_utils::calcCurvature(p1, p2, p3); } catch (...) { curvature_arr.at(i) = 0.0; // maybe distance is too close } @@ -236,11 +236,12 @@ std::pair calcMaxRelativeAngles(const Trajectory & trajectory) const auto & p2 = trajectory.points.at(i + 1).pose.position; const auto & p3 = trajectory.points.at(i + 2).pose.position; - const auto angle_a = tier4_autoware_utils::calcAzimuthAngle(p1, p2); - const auto angle_b = tier4_autoware_utils::calcAzimuthAngle(p2, p3); + const auto angle_a = autoware::universe_utils::calcAzimuthAngle(p1, p2); + const auto angle_b = autoware::universe_utils::calcAzimuthAngle(p2, p3); // convert relative angle to [-pi ~ pi] - const auto relative_angle = std::abs(tier4_autoware_utils::normalizeRadian(angle_b - angle_a)); + const auto relative_angle = + std::abs(autoware::universe_utils::normalizeRadian(angle_b - angle_a)); takeBigger(max_relative_angles, max_index, std::abs(relative_angle), i); } diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_functions.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_functions.cpp index 41ae41a80bdc5..9282392197665 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_functions.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_functions.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_planning_validator/debug_marker.hpp" -#include "autoware_planning_validator/planning_validator.hpp" +#include "autoware/planning_validator/debug_marker.hpp" +#include "autoware/planning_validator/planning_validator.hpp" #include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp index de7d8293286d7..bdf4ca96d9fdd 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_helper.cpp @@ -14,14 +14,14 @@ #include "test_planning_validator_helper.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "test_parameter.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include +using autoware::universe_utils::createQuaternionFromYaw; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::createQuaternionFromYaw; Trajectory generateTrajectoryWithConstantAcceleration( const double interval_distance, const double speed, const double yaw, const size_t size, diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp index 33ef0a1817af8..6476086c67730 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_planning_validator/planning_validator.hpp" +#include "autoware/planning_validator/planning_validator.hpp" #include #include diff --git a/planning/autoware_planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/autoware_planning_validator/test/src/test_planning_validator_pubsub.cpp index 8cf56525edbc5..a83fa1fd965fe 100644 --- a/planning/autoware_planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/autoware_planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_planning_validator/planning_validator.hpp" +#include "autoware/planning_validator/planning_validator.hpp" #include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" diff --git a/planning/autoware_remaining_distance_time_calculator/package.xml b/planning/autoware_remaining_distance_time_calculator/package.xml index 0e5ae5aa63c3e..4e8fc23ffe391 100644 --- a/planning/autoware_remaining_distance_time_calculator/package.xml +++ b/planning/autoware_remaining_distance_time_calculator/package.xml @@ -12,17 +12,17 @@ autoware_cmake autoware_internal_msgs + autoware_motion_utils autoware_planning_msgs autoware_route_handler autoware_test_utils + autoware_universe_utils geometry_msgs lanelet2_extension - motion_utils nav_msgs rclcpp rclcpp_components std_msgs - tier4_autoware_utils tier4_planning_msgs ament_cmake_ros diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp index 327bd0ff3b582..da1292fa862c6 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp @@ -14,12 +14,12 @@ #include "remaining_distance_time_calculator_node.hpp" +#include #include #include #include #include #include -#include #include @@ -143,8 +143,8 @@ void RemainingDistanceTimeCalculatorNode::calculate_remaining_distance() for (auto & llt : remaining_shortest_path) { if (remaining_shortest_path.size() == 1) { - remaining_distance_ += - tier4_autoware_utils::calcDistance2d(current_vehicle_pose_.position, goal_pose_.position); + remaining_distance_ += autoware::universe_utils::calcDistance2d( + current_vehicle_pose_.position, goal_pose_.position); break; } diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp index 19c5581d26b9d..d6c900af4dfed 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp @@ -15,7 +15,7 @@ #ifndef REMAINING_DISTANCE_TIME_CALCULATOR_NODE_HPP_ #define REMAINING_DISTANCE_TIME_CALCULATOR_NODE_HPP_ -#include +#include #include #include diff --git a/planning/autoware_route_handler/include/autoware_route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp similarity index 94% rename from planning/autoware_route_handler/include/autoware_route_handler/route_handler.hpp rename to planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index 5d7c78e0c1976..3b80314bae5a8 100644 --- a/planning/autoware_route_handler/include/autoware_route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_ROUTE_HANDLER__ROUTE_HANDLER_HPP_ -#define AUTOWARE_ROUTE_HANDLER__ROUTE_HANDLER_HPP_ +#ifndef AUTOWARE__ROUTE_HANDLER__ROUTE_HANDLER_HPP_ +#define AUTOWARE__ROUTE_HANDLER__ROUTE_HANDLER_HPP_ #include @@ -116,6 +116,8 @@ class RouteHandler // for lanelet bool getPreviousLaneletsWithinRoute( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const; + bool getNextLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * next_lanelets) const; bool getNextLaneletWithinRoute( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const; bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const; @@ -260,6 +262,22 @@ class RouteHandler bool getClosestLaneletWithConstrainsWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, const double yaw_threshold) const; + + /** + * Finds the closest route lanelet to the search pose satisfying the distance and yaw constraints + * with respect to a reference lanelet, the search set will include previous route lanelets, + * next route lanelets, and neighbors of reference lanelet. Returns false if failed to find + * lanelet. + * @param search_pose pose to find closest lanelet to + * @param reference_lanelet reference lanelet to decide the search set + * @param dist_threshold distance constraint closest lanelet must be within + * @param yaw_threshold yaw constraint closest lanelet direction must be within + */ + bool getClosestRouteLaneletFromLanelet( + const Pose & search_pose, const lanelet::ConstLanelet & reference_lanelet, + lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, + const double yaw_threshold) const; + lanelet::ConstLanelet getLaneletsFromId(const lanelet::Id id) const; lanelet::ConstLanelets getLaneletsFromIds(const lanelet::Ids & ids) const; lanelet::ConstLanelets getLaneletSequence( @@ -419,4 +437,4 @@ class RoutingCostDrivable : public lanelet::routing::RoutingCostDistance } }; // class RoutingCostDrivable } // namespace autoware::route_handler -#endif // AUTOWARE_ROUTE_HANDLER__ROUTE_HANDLER_HPP_ +#endif // AUTOWARE__ROUTE_HANDLER__ROUTE_HANDLER_HPP_ diff --git a/planning/autoware_route_handler/package.xml b/planning/autoware_route_handler/package.xml index 61a8a43b31461..f7d10a519ed4e 100644 --- a/planning/autoware_route_handler/package.xml +++ b/planning/autoware_route_handler/package.xml @@ -10,6 +10,7 @@ Takayuki Murooka Mamoru Sobue Go Sakayori + Alqudah Mohammad Apache License 2.0 @@ -25,12 +26,12 @@ autoware_map_msgs autoware_planning_msgs autoware_test_utils + autoware_universe_utils geometry_msgs lanelet2_extension rclcpp rclcpp_components tf2_ros - tier4_autoware_utils yaml-cpp diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 213cca3bd5d80..d534ecc4c84a8 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_route_handler/route_handler.hpp" +#include "autoware/route_handler/route_handler.hpp" +#include #include #include #include #include #include #include -#include #include #include @@ -110,7 +110,7 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) constexpr double min_dist = 0.001; if ( - tier4_autoware_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < + autoware::universe_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < min_dist) { filtered_path.points.back().lane_ids.push_back(pt.lane_ids.front()); filtered_path.points.back().point.longitudinal_velocity_mps = std::min( @@ -616,28 +616,41 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( lanelet::ConstLanelet current_lanelet = lanelet; double length = 0; lanelet::ConstLanelets previous_lanelets; + + auto checkForLoop = + [&lanelet](const lanelet::ConstLanelets & lanelets_to_check, const bool is_route_lanelets) { + if (is_route_lanelets) { + return std::none_of( + lanelets_to_check.begin(), lanelets_to_check.end(), + [lanelet](auto & prev_llt) { return lanelet.id() != prev_llt.id(); }); + } + return std::any_of( + lanelets_to_check.begin(), lanelets_to_check.end(), + [lanelet](auto & prev_llt) { return lanelet.id() == prev_llt.id(); }); + }; + + auto isNewLanelet = [&lanelet, + &lanelet_sequence_backward](const lanelet::ConstLanelet & lanelet_to_check) { + if (lanelet.id() == lanelet_to_check.id()) return false; + return std::none_of( + lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), + [&lanelet_to_check](auto & backward) { return (backward.id() == lanelet_to_check.id()); }); + }; + while (rclcpp::ok() && length < min_length) { previous_lanelets.clear(); + bool is_route_lanelets = true; if (!getPreviousLaneletsWithinRoute(current_lanelet, &previous_lanelets)) { if (only_route_lanes) break; - const auto previous_lanelets = getPreviousLanelets(current_lanelet); + previous_lanelets = getPreviousLanelets(current_lanelet); if (previous_lanelets.empty()) break; + is_route_lanelets = false; } - // loop check - if (std::any_of(previous_lanelets.begin(), previous_lanelets.end(), [lanelet](auto & prev_llt) { - return lanelet.id() == prev_llt.id(); - })) { - break; - } + + if (checkForLoop(previous_lanelets, is_route_lanelets)) break; for (const auto & prev_lanelet : previous_lanelets) { - if (std::any_of( - lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), - [prev_lanelet, lanelet](auto & backward) { - return (backward.id() == prev_lanelet.id()); - })) { - continue; - } + if (!isNewLanelet(prev_lanelet) || exists(goal_lanelets_, prev_lanelet)) continue; lanelet_sequence_backward.push_back(prev_lanelet); length += static_cast(boost::geometry::length(prev_lanelet.centerline().basicLineString())); @@ -922,8 +935,32 @@ bool RouteHandler::getClosestLaneletWithConstrainsWithinRoute( route_lanelets_, search_pose, closest_lanelet, dist_threshold, yaw_threshold); } -bool RouteHandler::getNextLaneletWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const +bool RouteHandler::getClosestRouteLaneletFromLanelet( + const Pose & search_pose, const lanelet::ConstLanelet & reference_lanelet, + lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, + const double yaw_threshold) const +{ + lanelet::ConstLanelets previous_lanelets, next_lanelets, lanelet_sequence; + if (getPreviousLaneletsWithinRoute(reference_lanelet, &previous_lanelets)) { + lanelet_sequence = previous_lanelets; + } + + lanelet_sequence.push_back(reference_lanelet); + + if (getNextLaneletsWithinRoute(reference_lanelet, &next_lanelets)) { + lanelet_sequence.insert(lanelet_sequence.end(), next_lanelets.begin(), next_lanelets.end()); + } + + if (lanelet::utils::query::getClosestLaneletWithConstrains( + lanelet_sequence, search_pose, closest_lanelet, dist_threshold, yaw_threshold)) { + return true; + } + + return false; +} + +bool RouteHandler::getNextLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * next_lanelets) const { if (exists(goal_lanelets_, lanelet)) { return false; @@ -932,12 +969,23 @@ bool RouteHandler::getNextLaneletWithinRoute( const auto start_lane_id = route_ptr_->segments.front().preferred_primitive.id; const auto following_lanelets = routing_graph_ptr_->following(lanelet); + next_lanelets->clear(); for (const auto & llt : following_lanelets) { if (start_lane_id != llt.id() && exists(route_lanelets_, llt)) { - *next_lanelet = llt; - return true; + next_lanelets->push_back(llt); } } + return !(next_lanelets->empty()); +} + +bool RouteHandler::getNextLaneletWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const +{ + lanelet::ConstLanelets next_lanelets{}; + if (getNextLaneletsWithinRoute(lanelet, &next_lanelets)) { + *next_lanelet = next_lanelets.front(); + return true; + } return false; } @@ -1677,14 +1725,14 @@ PathWithLaneId RouteHandler::getCenterLinePath( double angle{0.0}; const auto & pts = reference_path.points; if (i + 1 < reference_path.points.size()) { - angle = tier4_autoware_utils::calcAzimuthAngle( + angle = autoware::universe_utils::calcAzimuthAngle( pts.at(i).point.pose.position, pts.at(i + 1).point.pose.position); } else if (i != 0) { - angle = tier4_autoware_utils::calcAzimuthAngle( + angle = autoware::universe_utils::calcAzimuthAngle( pts.at(i - 1).point.pose.position, pts.at(i).point.pose.position); } reference_path.points.at(i).point.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(angle); + autoware::universe_utils::createQuaternionFromYaw(angle); } return reference_path; @@ -2142,6 +2190,9 @@ lanelet::ConstLanelets RouteHandler::getMainLanelets( const lanelet::ConstLanelets & path_lanelets) const { auto lanelet_sequence = getLaneletSequence(path_lanelets.back()); + + RCLCPP_INFO_STREAM(logger_, "getMainLanelets: lanelet_sequence = " << lanelet_sequence); + lanelet::ConstLanelets main_lanelets; while (!lanelet_sequence.empty()) { main_lanelets.insert(main_lanelets.begin(), lanelet_sequence.begin(), lanelet_sequence.end()); diff --git a/planning/autoware_route_handler/test/test_route_handler.cpp b/planning/autoware_route_handler/test/test_route_handler.cpp index 5ee8aa8034abd..35a2fe3ef45da 100644 --- a/planning/autoware_route_handler/test/test_route_handler.cpp +++ b/planning/autoware_route_handler/test/test_route_handler.cpp @@ -14,6 +14,8 @@ #include "test_route_handler.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + #include #include @@ -45,62 +47,122 @@ TEST_F(TestRouteHandler, getGoalLaneId) ASSERT_EQ(goal_lane.id(), 5088); } +TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute) +{ + set_route_handler("/test_map/overlap_map.osm"); + ASSERT_FALSE(route_handler_->isHandlerReady()); + + geometry_msgs::msg::Pose start_pose, goal_pose; + start_pose.position = autoware::universe_utils::createPoint(3728.870361, 73739.281250, 0); + start_pose.orientation = autoware::universe_utils::createQuaternion(0, 0, -0.513117, 0.858319); + goal_pose.position = autoware::universe_utils::createPoint(3729.961182, 73727.328125, 0); + goal_pose.orientation = autoware::universe_utils::createQuaternion(0, 0, 0.234831, 0.972036); + + lanelet::ConstLanelets path_lanelets; + ASSERT_TRUE( + route_handler_->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets)); + ASSERT_EQ(path_lanelets.size(), 12); + ASSERT_EQ(path_lanelets.front().id(), 168); + ASSERT_EQ(path_lanelets.back().id(), 345); + + route_handler_->setRouteLanelets(path_lanelets); + ASSERT_TRUE(route_handler_->isHandlerReady()); + + rclcpp::init(0, nullptr); + ASSERT_TRUE(rclcpp::ok()); + + auto lanelet_sequence = route_handler_->getLaneletSequence(path_lanelets.back()); + ASSERT_EQ(lanelet_sequence.size(), 12); + ASSERT_EQ(lanelet_sequence.front().id(), 168); + ASSERT_EQ(lanelet_sequence.back().id(), 345); +} + +TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) +{ + set_route_handler("/test_map/overlap_map.osm"); + set_test_route("/test_route/overlap_test_route.yaml"); + ASSERT_TRUE(route_handler_->isHandlerReady()); + + geometry_msgs::msg::Pose reference_pose, search_pose; + + lanelet::ConstLanelet reference_lanelet; + reference_pose.position = autoware::universe_utils::createPoint(3730.88, 73735.3, 0); + reference_pose.orientation = + autoware::universe_utils::createQuaternion(0, 0, -0.504626, 0.863338); + const auto found_reference_lanelet = + route_handler_->getClosestLaneletWithinRoute(reference_pose, &reference_lanelet); + ASSERT_TRUE(found_reference_lanelet); + ASSERT_EQ(reference_lanelet.id(), 168); + + lanelet::ConstLanelet closest_lanelet; + search_pose.position = autoware::universe_utils::createPoint(3736.89, 73730.8, 0); + search_pose.orientation = autoware::universe_utils::createQuaternion(0, 0, 0.223244, 0.974763); + bool found_lanelet = route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lanelet); + ASSERT_TRUE(found_lanelet); + ASSERT_EQ(closest_lanelet.id(), 345); + + found_lanelet = route_handler_->getClosestRouteLaneletFromLanelet( + search_pose, reference_lanelet, &closest_lanelet, 3.0, 1.046); + ASSERT_TRUE(found_lanelet); + ASSERT_EQ(closest_lanelet.id(), 277); +} + // TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute) // { // lanelet::ConstLanelet closest_lane; // Pose search_pose; -// search_pose.position = tier4_autoware_utils::createPoint(-1.0, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(-1.0, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained7 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained7); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = tier4_autoware_utils::createPoint(-0.5, 1.75, 0); +// search_pose.position = autoware::universe_utils::createPoint(-0.5, 1.75, 0); // const auto closest_lane_obtained = -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = tier4_autoware_utils::createPoint(-.01, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(-.01, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained3 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained3); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = tier4_autoware_utils::createPoint(0.0, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(0.0, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained1 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained1); // ASSERT_EQ(closest_lane.id(), 4775); -// search_pose.position = tier4_autoware_utils::createPoint(0.01, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(0.01, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained2 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained2); // ASSERT_EQ(closest_lane.id(), 4424); -// search_pose.position = tier4_autoware_utils::createPoint(0.5, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(0.5, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained4 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); // ASSERT_TRUE(closest_lane_obtained4); // ASSERT_EQ(closest_lane.id(), 4424); -// search_pose.position = tier4_autoware_utils::createPoint(1.0, 1.75, 0); -// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// search_pose.position = autoware::universe_utils::createPoint(1.0, 1.75, 0); +// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); // const auto closest_lane_obtained5 = // route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); diff --git a/planning/autoware_route_handler/test/test_route_handler.hpp b/planning/autoware_route_handler/test/test_route_handler.hpp index 292cbaa6ba7d9..86f7461fc7538 100644 --- a/planning/autoware_route_handler/test/test_route_handler.hpp +++ b/planning/autoware_route_handler/test/test_route_handler.hpp @@ -20,7 +20,7 @@ #include "autoware_test_utils/mock_data_parser.hpp" #include "gtest/gtest.h" -#include +#include #include #include #include @@ -44,14 +44,9 @@ class TestRouteHandler : public ::testing::Test public: TestRouteHandler() { - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto lanelet2_path = autoware_test_utils_dir + "/test_map/2km_test.osm"; - constexpr double center_line_resolution = 5.0; - const auto map_bin_msg = - autoware::test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution); - route_handler_ = std::make_shared(map_bin_msg); - set_lane_change_test_route(); + autoware_test_utils_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); + set_route_handler("/test_map/2km_test.osm"); + set_test_route("/test_route/lane_change_test_route.yaml"); } TestRouteHandler(const TestRouteHandler &) = delete; @@ -60,15 +55,26 @@ class TestRouteHandler : public ::testing::Test TestRouteHandler & operator=(TestRouteHandler &&) = delete; ~TestRouteHandler() override = default; - void set_lane_change_test_route() + void set_route_handler(const std::string & relative_path) + { + route_handler_.reset(); + const auto lanelet2_path = autoware_test_utils_dir + relative_path; + const auto map_bin_msg = + autoware::test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution); + route_handler_ = std::make_shared(map_bin_msg); + } + + void set_test_route(const std::string & route_path) { const auto route_handler_dir = ament_index_cpp::get_package_share_directory("autoware_route_handler"); - const auto rh_test_route = route_handler_dir + "/test_route/lane_change_test_route.yaml"; + const auto rh_test_route = route_handler_dir + route_path; route_handler_->setRoute(autoware::test_utils::parse_lanelet_route_file(rh_test_route)); } std::shared_ptr route_handler_; + std::string autoware_test_utils_dir; + static constexpr double center_line_resolution = 5.0; }; } // namespace autoware::route_handler::test diff --git a/planning/autoware_route_handler/test_route/overlap_test_route.yaml b/planning/autoware_route_handler/test_route/overlap_test_route.yaml new file mode 100644 index 0000000000000..595d22f4204eb --- /dev/null +++ b/planning/autoware_route_handler/test_route/overlap_test_route.yaml @@ -0,0 +1,117 @@ +header: + stamp: + sec: 1715936953 + nanosec: 67206425 + frame_id: map +start_pose: + position: + x: 3728.8 + y: 73739.2 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.513117 + w: 0.858319 +goal_pose: + position: + x: 3729.961182 + y: 73727.328125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.234831 + w: 0.972036 +segments: + - preferred_primitive: + id: 168 + primitive_type: "" + primitives: + - id: 168 + primitive_type: lane + - preferred_primitive: + id: 277 + primitive_type: "" + primitives: + - id: 277 + primitive_type: lane + - preferred_primitive: + id: 282 + primitive_type: "" + primitives: + - id: 282 + primitive_type: lane + - preferred_primitive: + id: 287 + primitive_type: "" + primitives: + - id: 287 + primitive_type: lane + - preferred_primitive: + id: 292 + primitive_type: "" + primitives: + - id: 292 + primitive_type: lane + - preferred_primitive: + id: 297 + primitive_type: "" + primitives: + - id: 297 + primitive_type: lane + - preferred_primitive: + id: 302 + primitive_type: "" + primitives: + - id: 302 + primitive_type: lane + - preferred_primitive: + id: 307 + primitive_type: "" + primitives: + - id: 307 + primitive_type: lane + - preferred_primitive: + id: 312 + primitive_type: "" + primitives: + - id: 312 + primitive_type: lane + - preferred_primitive: + id: 317 + primitive_type: "" + primitives: + - id: 317 + primitive_type: lane + - preferred_primitive: + id: 322 + primitive_type: "" + primitives: + - id: 322 + primitive_type: lane + - preferred_primitive: + id: 345 + primitive_type: "" + primitives: + - id: 345 + primitive_type: lane +uuid: + uuid: + - 231 + - 254 + - 143 + - 227 + - 194 + - 8 + - 220 + - 88 + - 30 + - 194 + - 172 + - 147 + - 127 + - 143 + - 176 + - 133 +allow_modification: false diff --git a/planning/autoware_rtc_interface/include/autoware_rtc_interface/rtc_interface.hpp b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp similarity index 96% rename from planning/autoware_rtc_interface/include/autoware_rtc_interface/rtc_interface.hpp rename to planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp index 8b435a6f8ed51..62f9a55c75455 100644 --- a/planning/autoware_rtc_interface/include/autoware_rtc_interface/rtc_interface.hpp +++ b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_RTC_INTERFACE__RTC_INTERFACE_HPP_ -#define AUTOWARE_RTC_INTERFACE__RTC_INTERFACE_HPP_ +#ifndef AUTOWARE__RTC_INTERFACE__RTC_INTERFACE_HPP_ +#define AUTOWARE__RTC_INTERFACE__RTC_INTERFACE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -103,4 +103,4 @@ class RTCInterface } // namespace autoware::rtc_interface -#endif // AUTOWARE_RTC_INTERFACE__RTC_INTERFACE_HPP_ +#endif // AUTOWARE__RTC_INTERFACE__RTC_INTERFACE_HPP_ diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 3389d08b9015e..985e38b64f2bd 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_rtc_interface/rtc_interface.hpp" +#include "autoware/rtc_interface/rtc_interface.hpp" #include diff --git a/planning/autoware_scenario_selector/include/autoware_scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp similarity index 85% rename from planning/autoware_scenario_selector/include/autoware_scenario_selector/node.hpp rename to planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index 88bbc57815d3d..557c0c871583d 100644 --- a/planning/autoware_scenario_selector/include/autoware_scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_SCENARIO_SELECTOR__NODE_HPP_ -#define AUTOWARE_SCENARIO_SELECTOR__NODE_HPP_ +#ifndef AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_ +#define AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_ +#include #include -#include #include #include @@ -34,8 +34,8 @@ #else #include #endif -#include -#include +#include +#include #include #include @@ -73,12 +73,12 @@ class ScenarioSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr sub_route_; - tier4_autoware_utils::InterProcessPollingSubscriber::SharedPtr + autoware::universe_utils::InterProcessPollingSubscriber::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_lane_driving_trajectory_; rclcpp::Subscription::SharedPtr sub_parking_trajectory_; - tier4_autoware_utils::InterProcessPollingSubscriber::SharedPtr + autoware::universe_utils::InterProcessPollingSubscriber::SharedPtr sub_parking_state_; rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; @@ -93,7 +93,7 @@ class ScenarioSelectorNode : public rclcpp::Node std::deque twist_buffer_; std::shared_ptr route_handler_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; // Parameters double update_rate_; @@ -104,4 +104,4 @@ class ScenarioSelectorNode : public rclcpp::Node bool is_parking_completed_; }; } // namespace autoware::scenario_selector -#endif // AUTOWARE_SCENARIO_SELECTOR__NODE_HPP_ +#endif // AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_ diff --git a/planning/autoware_scenario_selector/package.xml b/planning/autoware_scenario_selector/package.xml index 03a0e92b5a33f..4a3c0c5c99eaf 100644 --- a/planning/autoware_scenario_selector/package.xml +++ b/planning/autoware_scenario_selector/package.xml @@ -20,6 +20,7 @@ autoware_planning_msgs autoware_planning_test_manager autoware_route_handler + autoware_universe_utils lanelet2_extension nav_msgs rclcpp @@ -27,7 +28,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs ros2cli diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index dc34cfb1b523a..4dcf1b6adb111 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_scenario_selector/node.hpp" +#include "autoware/scenario_selector/node.hpp" #include #include @@ -381,7 +381,8 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); - published_time_publisher_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } } // namespace autoware::scenario_selector diff --git a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp index 1b90992dae715..27ac4c564c5e7 100644 --- a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp +++ b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_scenario_selector/node.hpp" +#include "autoware/scenario_selector/node.hpp" #include #include diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index e6b812ff9b388..1362f04e720e3 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -18,11 +18,13 @@ autoware_behavior_path_planner_common autoware_map_msgs autoware_mission_planner + autoware_motion_utils autoware_path_optimizer autoware_path_smoother autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geography_utils geometry_msgs @@ -31,11 +33,9 @@ lanelet2_extension map_loader map_projection_loader - motion_utils osqp_interface rclcpp rclcpp_components - tier4_autoware_utils tier4_map_msgs python3-flask-cors diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index 447773d7a6535..cbfcac5789812 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -73,10 +73,10 @@ std::vector generate_centerline_with_bag(rclcpp::Node & node) centerline_traj_points.at(i - 1).pose.orientation; } } else { - const double yaw_angle = tier4_autoware_utils::calcAzimuthAngle( + const double yaw_angle = autoware::universe_utils::calcAzimuthAngle( centerline_traj_points.at(i).pose.position, centerline_traj_points.at(i + 1).pose.position); centerline_traj_points.at(i).pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(yaw_angle); + autoware::universe_utils::createQuaternionFromYaw(yaw_angle); } } diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 7dd3a20848aca..1f72dea1cd35f 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -14,12 +14,12 @@ #include "centerline_source/optimization_trajectory_based_centerline.hpp" -#include "autoware_path_optimizer/node.hpp" -#include "autoware_path_smoother/elastic_band_smoother.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/path_optimizer/node.hpp" +#include "autoware/path_smoother/elastic_band_smoother.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" #include "static_centerline_generator_node.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" #include "utils.hpp" #include @@ -82,20 +82,21 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( // get ego nearest search parameters and resample interval in behavior_path_planner const double ego_nearest_dist_threshold = - tier4_autoware_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); + autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); const double ego_nearest_yaw_threshold = - tier4_autoware_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); + autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); const double behavior_path_interval = - tier4_autoware_utils::getOrDeclareParameter(node, "output_path_interval"); + autoware::universe_utils::getOrDeclareParameter(node, "output_path_interval"); const double behavior_vel_interval = - tier4_autoware_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); + autoware::universe_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); // extract path with lane id from lanelets const auto raw_path_with_lane_id = [&]() { const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); + return autoware::motion_utils::resamplePath( + non_resampled_path_with_lane_id, behavior_path_interval); }(); pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); RCLCPP_INFO(node.get_logger(), "Calculated raw path with lane id and published."); @@ -103,7 +104,7 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( // convert path with lane id to path const auto raw_path = [&]() { const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); - return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); + return autoware::motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); }(); pub_raw_path_->publish(raw_path); RCLCPP_INFO(node.get_logger(), "Converted to path and published."); @@ -123,7 +124,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra // convert to trajectory points const auto raw_traj_points = [&]() { const auto raw_traj = convert_to_trajectory(raw_path); - return motion_utils::convertToTrajectoryPointArray(raw_traj); + return autoware::motion_utils::convertToTrajectoryPointArray(raw_traj); }(); // create an instance of elastic band and model predictive trajectory. @@ -165,7 +166,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra // connect the previously and currently optimized trajectory points for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { - const double dist = tier4_autoware_utils::calcDistance2d( + const double dist = autoware::universe_utils::calcDistance2d( whole_optimized_traj_points.at(j), optimized_traj_points.front()); if (dist < 0.5) { const std::vector extracted_whole_optimized_traj_points{ diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index b907ec2529fcc..c5ecc48ee209d 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -14,6 +14,10 @@ #include "static_centerline_generator_node.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" #include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "centerline_source/bag_ego_trajectory_based_centerline.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" @@ -22,17 +26,13 @@ #include "map_loader/lanelet2_map_loader_node.hpp" #include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include "map_projection_loader/map_projection_loader.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" #include "type_alias.hpp" #include "utils.hpp" +#include +#include #include -#include #include -#include #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/float32.hpp" @@ -88,10 +88,14 @@ LinearRing2d create_vehicle_footprint( const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); std::vector geom_points; - geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_front, y_left, 0.0).position); - geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_front, y_right, 0.0).position); - geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_rear, y_right, 0.0).position); - geom_points.push_back(tier4_autoware_utils::calcOffsetPose(pose, x_rear, y_left, 0.0).position); + geom_points.push_back( + autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0).position); + geom_points.push_back( + autoware::universe_utils::calcOffsetPose(pose, x_front, y_right, 0.0).position); + geom_points.push_back( + autoware::universe_utils::calcOffsetPose(pose, x_rear, y_right, 0.0).position); + geom_points.push_back( + autoware::universe_utils::calcOffsetPose(pose, x_rear, y_left, 0.0).position); LinearRing2d footprint; for (const auto & geom_point : geom_points) { @@ -113,7 +117,7 @@ geometry_msgs::msg::Pose get_text_pose( const double x_front = i.front_overhang_m + i.wheel_base_m; const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 1.0; - return tier4_autoware_utils::calcOffsetPose(pose, x_front, y_left, 0.0); + return autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0); } std::array convert_hex_string_to_decimal(const std::string & hex_str_color) @@ -162,9 +166,10 @@ std::vector resample_trajectory_points( const std::vector & input_traj_points, const double resample_interval) { // resample and calculate trajectory points' orientation - const auto input_traj = motion_utils::convertToTrajectory(input_traj_points); - auto resampled_input_traj = motion_utils::resampleTrajectory(input_traj, resample_interval); - return motion_utils::convertToTrajectoryPointArray(resampled_input_traj); + const auto input_traj = autoware::motion_utils::convertToTrajectory(input_traj_points); + auto resampled_input_traj = + autoware::motion_utils::resampleTrajectory(input_traj, resample_interval); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_input_traj); } } // namespace @@ -198,7 +203,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( sub_save_map_ = create_subscription( "/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) { const auto lanelet2_output_file_path = - tier4_autoware_utils::getOrDeclareParameter( + autoware::universe_utils::getOrDeclareParameter( *this, "lanelet2_output_file_path"); if (!centerline_with_route_ || msg.data) { const auto & c = *centerline_with_route_; @@ -269,7 +274,7 @@ void StaticCenterlineGeneratorNode::update_centerline_range( centerline.begin() + traj_range_indices_.second + 1); pub_centerline_->publish( - motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); + autoware::motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); } void StaticCenterlineGeneratorNode::run() @@ -321,10 +326,10 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout centerline_with_route.centerline = resample_trajectory_points(centerline_with_route.centerline, output_trajectory_interval); - pub_whole_centerline_->publish(motion_utils::convertToTrajectory( + pub_whole_centerline_->publish(autoware::motion_utils::convertToTrajectory( centerline_with_route.centerline, create_header(this->now()))); - pub_centerline_->publish(motion_utils::convertToTrajectory( + pub_centerline_->publish(autoware::motion_utils::convertToTrajectory( centerline_with_route.centerline, create_header(this->now()))); return centerline_with_route; @@ -374,6 +379,7 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_ Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, *map_projector_info_); // overwrite more dense centerline + // NOTE: overwriteLaneletsCenterlineWithWaypoints is used only in real time calculation. lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); // create map bin msg @@ -571,10 +577,11 @@ void StaticCenterlineGeneratorNode::evaluate( const std::vector & optimized_traj_points) { const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - const auto dist_thresh_vec = tier4_autoware_utils::getOrDeclareParameter>( + const auto dist_thresh_vec = autoware::universe_utils::getOrDeclareParameter>( *this, "marker_color_dist_thresh"); const auto marker_color_vec = - tier4_autoware_utils::getOrDeclareParameter>(*this, "marker_color"); + autoware::universe_utils::getOrDeclareParameter>( + *this, "marker_color"); const auto get_marker_color = [&](const double dist) -> boost::optional> { for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { const double dist_thresh = dist_thresh_vec.at(i); @@ -621,13 +628,13 @@ void StaticCenterlineGeneratorNode::evaluate( // add footprint marker const auto footprint_marker = utils::create_footprint_marker(footprint_poly, marker_color, now(), i); - tier4_autoware_utils::appendMarkerArray(footprint_marker, &marker_array); + autoware::universe_utils::appendMarkerArray(footprint_marker, &marker_array); // add text of distance to bounds marker const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); const auto text_marker = utils::create_distance_text_marker(text_pose, min_dist_to_bound, marker_color, now(), i); - tier4_autoware_utils::appendMarkerArray(text_marker, &marker_array); + autoware::universe_utils::appendMarkerArray(text_marker, &marker_array); } } diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp index 1eb5aa516f3b9..c5157acc2b525 100644 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ b/planning/autoware_static_centerline_generator/src/type_alias.hpp @@ -14,8 +14,8 @@ #ifndef TYPE_ALIAS_HPP_ #define TYPE_ALIAS_HPP_ -#include "autoware_route_handler/route_handler.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/route_handler/route_handler.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_map_msgs/msg/lanelet_map_bin.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" @@ -29,6 +29,9 @@ namespace autoware::static_centerline_generator { using autoware::route_handler::RouteHandler; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::LaneletRoute; @@ -36,9 +39,6 @@ using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp index 46044a83f3764..de362e3af5d7f 100644 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ b/planning/autoware_static_centerline_generator/src/utils.cpp @@ -14,10 +14,10 @@ #include "utils.hpp" -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include #include @@ -94,8 +94,8 @@ geometry_msgs::msg::Pose get_center_pose( // calculate middle pose geometry_msgs::msg::Pose middle_pose; middle_pose.position = middle_pos; - const double yaw = tier4_autoware_utils::calcAzimuthAngle(middle_pos, next_middle_pos); - middle_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + const double yaw = autoware::universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); + middle_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); return middle_pose; } @@ -189,11 +189,11 @@ MarkerArray create_footprint_marker( const double g = marker_color.at(1); const double b = marker_color.at(2); - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock().now(), "unsafe_footprints", idx, visualization_msgs::msg::Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(r, g, b, 0.999)); + autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(r, g, b, 0.999)); marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); @@ -221,11 +221,11 @@ MarkerArray create_distance_text_marker( const double g = marker_color.at(1); const double b = marker_color.at(2); - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock().now(), "unsafe_footprints_distance", idx, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5), - tier4_autoware_utils::createMarkerColor(r, g, b, 0.999)); + autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5), + autoware::universe_utils::createMarkerColor(r, g, b, 0.999)); marker.pose = pose; marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); diff --git a/planning/autoware_static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp index 8b7347f84701a..70a7e61e19b18 100644 --- a/planning/autoware_static_centerline_generator/src/utils.hpp +++ b/planning/autoware_static_centerline_generator/src/utils.hpp @@ -15,7 +15,7 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ -#include "autoware_route_handler/route_handler.hpp" +#include "autoware/route_handler/route_handler.hpp" #include "type_alias.hpp" #include diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 530f850a3c34a..fb5ea169b9c88 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -15,13 +15,14 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs + autoware_universe_utils autoware_vehicle_info_utils diagnostic_msgs eigen libpcl-all-dev - motion_utils pcl_conversions rclcpp rclcpp_components @@ -31,7 +32,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index b676673128269..7897a868ee172 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_surround_obstacle_checker/debug_marker.hpp" +#include "debug_marker.hpp" -#include -#include -#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif +#include #include namespace autoware::surround_obstacle_checker @@ -51,12 +52,12 @@ Polygon2d createSelfPolygon( } } // namespace -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double base_link2front, diff --git a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp similarity index 95% rename from planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp rename to planning/autoware_surround_obstacle_checker/src/debug_marker.hpp index c7ec2e5b564e6..058bf964e7dc4 100644 --- a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ -#define AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ +#ifndef DEBUG_MARKER_HPP_ +#define DEBUG_MARKER_HPP_ #include #include @@ -99,4 +99,4 @@ class SurroundObstacleCheckerDebugNode rclcpp::Clock::SharedPtr clock_; }; } // namespace autoware::surround_obstacle_checker -#endif // AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ +#endif // DEBUG_MARKER_HPP_ diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 8d6279eeefb22..9c899c2226497 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_surround_obstacle_checker/node.hpp" +#include "node.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -51,9 +51,9 @@ namespace autoware::surround_obstacle_checker namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::pose2transform; using autoware_perception_msgs::msg::ObjectClassification; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::pose2transform; namespace { @@ -193,7 +193,7 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio p.publish_debug_footprints = this->declare_parameter("publish_debug_footprints"); p.debug_footprint_label = this->declare_parameter("debug_footprint_label"); - logger_configure_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); } vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); @@ -251,12 +251,12 @@ rcl_interfaces::msg::SetParametersResult SurroundObstacleCheckerNode::onParam( use_dynamic_object_ = false; for (const auto & label_pair : label_map_) { bool & check_current_label = node_param_.enable_check_map.at(label_pair.second); - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, label_pair.first + ".enable_check", check_current_label); use_dynamic_object_ = use_dynamic_object_ || check_current_label; } - tier4_autoware_utils::updateParam( + autoware::universe_utils::updateParam( parameters, "debug_footprint_label", node_param_.debug_footprint_label); const auto check_distances = getCheckDistances(node_param_.debug_footprint_label); debug_ptr_->updateFootprintMargin( @@ -411,7 +411,7 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCl tf2::transformToEigen(transform_stamped.value().transform).cast(); pcl::PointCloud transformed_pointcloud; pcl::fromROSMsg(*pointcloud_ptr_, transformed_pointcloud); - tier4_autoware_utils::transformPointCloud( + autoware::universe_utils::transformPointCloud( transformed_pointcloud, transformed_pointcloud, isometry); const double front_margin = node_param_.pointcloud_surround_check_front_distance; diff --git a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp similarity index 84% rename from planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp rename to planning/autoware_surround_obstacle_checker/src/node.hpp index 4eb094e6617a4..e0bcf771948ca 100644 --- a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_SURROUND_OBSTACLE_CHECKER__NODE_HPP_ -#define AUTOWARE_SURROUND_OBSTACLE_CHECKER__NODE_HPP_ +#ifndef NODE_HPP_ +#define NODE_HPP_ -#include "autoware_surround_obstacle_checker/debug_marker.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "debug_marker.hpp" +#include #include -#include #include #include @@ -46,10 +46,10 @@ namespace autoware::surround_obstacle_checker { +using autoware::motion_utils::VehicleStopChecker; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; -using motion_utils::VehicleStopChecker; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; @@ -111,11 +111,11 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // publisher and subscriber - tier4_autoware_utils::InterProcessPollingSubscriber sub_odometry_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber - sub_pointcloud_{this, "~/input/pointcloud"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ + autoware::universe_utils::InterProcessPollingSubscriber + sub_pointcloud_{this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; + autoware::universe_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ this, "~/input/objects"}; rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; @@ -143,7 +143,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node State state_ = State::PASS; std::shared_ptr last_obstacle_found_time_; - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; bool use_dynamic_object_; @@ -151,4 +151,4 @@ class SurroundObstacleCheckerNode : public rclcpp::Node }; } // namespace autoware::surround_obstacle_checker -#endif // AUTOWARE_SURROUND_OBSTACLE_CHECKER__NODE_HPP_ +#endif // NODE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp similarity index 84% rename from planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp rename to planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 160ee183d8a0a..0dd18615be5ef 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -12,30 +12,30 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ -#define AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ - -#include "autoware_velocity_smoother/resample.hpp" -#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" -#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" -#include "autoware_velocity_smoother/smoother/smoother_base.hpp" -#include "autoware_velocity_smoother/trajectory_utils.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#ifndef AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ + +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/ros/self_pose_listener.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware/velocity_smoother/resample.hpp" +#include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/transform_listener.h" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include "tier4_autoware_utils/ros/self_pose_listener.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" -#include +#include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -88,13 +88,13 @@ class VelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; rclcpp::Subscription::SharedPtr sub_current_trajectory_; - tier4_autoware_utils::InterProcessPollingSubscriber sub_current_odometry_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_current_odometry_{ this, "/localization/kinematic_state"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_current_acceleration_{this, "~/input/acceleration"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_external_velocity_limit_{ - this, "~/input/external_velocity_limit_mps"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ + autoware::universe_utils::InterProcessPollingSubscriber + sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ this, "~/input/operation_mode_state"}; Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry @@ -245,7 +245,7 @@ class VelocitySmootherNode : public rclcpp::Node void initCommonParam(); // debug - tier4_autoware_utils::StopWatch stop_watch_; + autoware::universe_utils::StopWatch stop_watch_; std::shared_ptr prev_time_; double prev_acc_; rclcpp::Publisher::SharedPtr pub_dist_to_stopline_; @@ -273,9 +273,9 @@ class VelocitySmootherNode : public rclcpp::Node void flipVelocity(TrajectoryPoints & points) const; void publishStopWatchTime(); - std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::velocity_smoother -#endif // AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ +#endif // AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/resample.hpp similarity index 93% rename from planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp rename to planning/autoware_velocity_smoother/include/autoware/velocity_smoother/resample.hpp index 24a20ca8a588f..40081e1c7974f 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/resample.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ -#define AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#ifndef AUTOWARE__VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__RESAMPLE_HPP_ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include @@ -50,4 +50,4 @@ TrajectoryPoints resampleTrajectory( } // namespace resampling } // namespace autoware::velocity_smoother -#endif // AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#endif // AUTOWARE__VELOCITY_SMOOTHER__RESAMPLE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp similarity index 87% rename from planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index a5baa59f5d034..42d5a520c9e44 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT -#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT -#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" -#include "autoware_velocity_smoother/smoother/smoother_base.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -116,5 +116,5 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase } // namespace autoware::velocity_smoother // clang-format off -#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT // clang-format on diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp similarity index 83% rename from planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp rename to planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 0c18e102f6f66..541ba62e95d74 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT -#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -54,5 +54,5 @@ double integ_a(double a0, double j0, double t); } // namespace autoware::velocity_smoother // clang-format off -#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT // clang-format on diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp similarity index 85% rename from planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp index 4e93fcd339140..14d0d8ab7ff84 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ -#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ -#include "autoware_velocity_smoother/smoother/smoother_base.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -71,4 +71,4 @@ class JerkFilteredSmoother : public SmootherBase }; } // namespace autoware::velocity_smoother -#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp similarity index 80% rename from planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 44fa263e71fb9..1e2918d8e313b 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ -#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ -#include "autoware_velocity_smoother/smoother/smoother_base.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -58,4 +58,4 @@ class L2PseudoJerkSmoother : public SmootherBase }; } // namespace autoware::velocity_smoother -#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp similarity index 80% rename from planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index f638e03658e05..b88cd26eb4467 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ -#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ -#include "autoware_velocity_smoother/smoother/smoother_base.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -58,4 +58,4 @@ class LinfPseudoJerkSmoother : public SmootherBase }; } // namespace autoware::velocity_smoother -#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp similarity index 93% rename from planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp rename to planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp index 0c17ef5bdbb9d..be7baf694d361 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#include "autoware_velocity_smoother/resample.hpp" +#include "autoware/velocity_smoother/resample.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -88,4 +88,4 @@ class SmootherBase }; } // namespace autoware::velocity_smoother -#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#endif // AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp similarity index 94% rename from planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp rename to planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp index f7999d1c8b04c..29033b69d7a66 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/trajectory_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ -#define AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE__VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE__VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" @@ -79,4 +79,4 @@ double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closes } // namespace autoware::velocity_smoother::trajectory_utils -#endif // AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#endif // AUTOWARE__VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 2bfc32ec90a5f..997f7b16e5652 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -20,18 +20,18 @@ autoware_cmake eigen3_cmake_module + autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation - motion_utils nav_msgs osqp_interface rclcpp tf2 tf2_ros - tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index be1f77fc951b1..aefbd9ef20f84 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_velocity_smoother/node.hpp" +#include "autoware/velocity_smoother/node.hpp" -#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" -#include "motion_utils/marker/marker_helper.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include @@ -90,8 +90,9 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti clock_ = get_clock(); - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void VelocitySmootherNode::setupSmoother(const double wheelbase) @@ -299,7 +300,7 @@ void VelocitySmootherNode::initCommonParam() void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const { - Trajectory publishing_trajectory = motion_utils::convertToTrajectory(trajectory); + Trajectory publishing_trajectory = autoware::motion_utils::convertToTrajectory(trajectory); publishing_trajectory.header = base_traj_raw_ptr_->header; pub_trajectory_->publish(publishing_trajectory); published_time_publisher_->publish_if_subscribed( @@ -433,10 +434,10 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr } // calculate trajectory velocity - auto input_points = motion_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_); + auto input_points = autoware::motion_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_); // guard for invalid trajectory - input_points = motion_utils::removeOverlapPoints(input_points); + input_points = autoware::motion_utils::removeOverlapPoints(input_points); if (input_points.size() < 2) { RCLCPP_ERROR(get_logger(), "No enough points in trajectory after overlap points removal"); return; @@ -686,7 +687,7 @@ void VelocitySmootherNode::insertBehindVelocity( // TODO(planning/control team) deal with overlapped lanes with the same direction const size_t seg_idx = [&]() { // with distance and yaw thresholds - const auto opt_nearest_seg_idx = motion_utils::findNearestSegmentIndex( + const auto opt_nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); if (opt_nearest_seg_idx) { @@ -694,13 +695,14 @@ void VelocitySmootherNode::insertBehindVelocity( } // with distance threshold - const auto opt_second_nearest_seg_idx = motion_utils::findNearestSegmentIndex( + const auto opt_second_nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold); if (opt_second_nearest_seg_idx) { return opt_second_nearest_seg_idx.value(); } - return motion_utils::findNearestSegmentIndex(prev_output_, output.at(i).pose.position); + return autoware::motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose.position); }(); const auto prev_output_point = trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose, seg_idx); @@ -720,9 +722,9 @@ void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajecto // stop distance calculation const double stop_dist_lim{50.0}; double stop_dist{stop_dist_lim}; - const auto stop_idx{motion_utils::searchZeroVelocityIndex(trajectory)}; + const auto stop_idx{autoware::motion_utils::searchZeroVelocityIndex(trajectory)}; if (stop_idx) { - stop_dist = motion_utils::calcSignedArcLength(trajectory, closest, *stop_idx); + stop_dist = autoware::motion_utils::calcSignedArcLength(trajectory, closest, *stop_idx); } else { stop_dist = closest > 0 ? stop_dist : -stop_dist; } @@ -818,14 +820,14 @@ std::pair VelocitySmootherNode::ca void VelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { - const auto stop_idx = motion_utils::searchZeroVelocityIndex(input); + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(input); if (!stop_idx) { return; } // Get Closest Point from Output // TODO(planning/control team) deal with overlapped lanes with the same directions - const auto nearest_output_point_idx = motion_utils::findNearestIndex( + const auto nearest_output_point_idx = autoware::motion_utils::findNearestIndex( output, input.at(*stop_idx).pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); @@ -873,11 +875,12 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c // insert the point at the distance of external velocity limit const auto & current_pose = current_odometry_ptr_->pose.pose; - const size_t closest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - traj, current_pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); + const size_t closest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj, current_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); const auto inserted_index = - motion_utils::insertTargetPoint(closest_seg_idx, external_velocity_limit_.dist, traj); + autoware::motion_utils::insertTargetPoint(closest_seg_idx, external_velocity_limit_.dist, traj); if (!inserted_index) { traj.back().longitudinal_velocity_mps = std::min( traj.back().longitudinal_velocity_mps, static_cast(external_velocity_limit_.velocity)); @@ -890,7 +893,7 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c // create virtual wall if (std::abs(external_velocity_limit_.velocity) < 1e-3) { - const auto virtual_wall_marker = motion_utils::createStopVirtualWallMarker( + const auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( traj.at(*inserted_index).pose, external_velocity_limit_.sender, this->now(), 0, base_link2front_); pub_virtual_wall_->publish(virtual_wall_marker); @@ -902,13 +905,13 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { - const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj); + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { return; // no stop point. } double distance_sum = 0.0; for (size_t i = *stop_idx - 1; i < traj.size(); --i) { // search backward - distance_sum += tier4_autoware_utils::calcDistance2d(traj.at(i), traj.at(i + 1)); + distance_sum += autoware::universe_utils::calcDistance2d(traj.at(i), traj.at(i + 1)); if (distance_sum > node_param_.stopping_distance) { break; } @@ -1019,7 +1022,7 @@ double VelocitySmootherNode::calcTravelDistance() const if (prev_closest_point_) { const double travel_dist = - tier4_autoware_utils::calcDistance2d(*prev_closest_point_, closest_point); + autoware::universe_utils::calcDistance2d(*prev_closest_point_, closest_point); return travel_dist; } @@ -1036,14 +1039,14 @@ bool VelocitySmootherNode::isEngageStatus(const double target_vel) const Trajectory VelocitySmootherNode::toTrajectoryMsg( const TrajectoryPoints & points, const std_msgs::msg::Header * header) const { - auto trajectory = motion_utils::convertToTrajectory(points); + auto trajectory = autoware::motion_utils::convertToTrajectory(points); trajectory.header = header ? *header : base_traj_raw_ptr_->header; return trajectory; } size_t VelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const { - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( points, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); } @@ -1073,9 +1076,10 @@ void VelocitySmootherNode::publishStopWatchTime() TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPoint( const TrajectoryPoints & trajectory, const Pose & pose) const { - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory, pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); return trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, pose, current_seg_idx); } diff --git a/planning/autoware_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp index af3b222a71ef9..f69ad5a592272 100644 --- a/planning/autoware_velocity_smoother/src/resample.cpp +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_velocity_smoother/resample.hpp" +#include "autoware/velocity_smoother/resample.hpp" -#include "autoware_velocity_smoother/trajectory_utils.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" #include #include @@ -32,17 +32,18 @@ TrajectoryPoints resampleTrajectory( const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v) { // Arc length from the initial point to the closest point - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double negative_front_arclength_value = motion_utils::calcSignedArcLength( + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = autoware::motion_utils::calcSignedArcLength( input, current_pose.position, current_seg_idx, input.at(0).pose.position, 0); const auto front_arclength_value = std::fabs(negative_front_arclength_value); const auto dist_to_closest_stop_point = - motion_utils::calcDistanceToForwardStopPoint(input, current_pose); + autoware::motion_utils::calcDistanceToForwardStopPoint(input, current_pose); // Get the resample size from the closest point - const double trajectory_length = motion_utils::calcArcLength(input); + const double trajectory_length = autoware::motion_utils::calcArcLength(input); const double Nt = param.resample_time / std::max(param.dense_resample_dt, 0.001); const double ds_nominal = std::max(v_current * param.dense_resample_dt, param.dense_min_interval_distance); @@ -127,14 +128,14 @@ TrajectoryPoints resampleTrajectory( return input; } - const auto output_traj = motion_utils::resampleTrajectory( - motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); - auto output = motion_utils::convertToTrajectoryPointArray(output_traj); + const auto output_traj = autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); + auto output = autoware::motion_utils::convertToTrajectoryPointArray(output_traj); // add end point directly to consider the endpoint velocity. if (is_endpoint_included) { constexpr double ep_dist = 1.0E-3; - if (tier4_autoware_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { + if (autoware::universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { output.back() = input.back(); } else { output.push_back(input.back()); @@ -150,9 +151,9 @@ TrajectoryPoints resampleTrajectory( const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v) { // input arclength - const double trajectory_length = motion_utils::calcArcLength(input); + const double trajectory_length = autoware::motion_utils::calcArcLength(input); const auto dist_to_closest_stop_point = - motion_utils::calcDistanceToForwardStopPoint(input, current_pose); + autoware::motion_utils::calcDistanceToForwardStopPoint(input, current_pose); // distance to stop point double stop_arclength_value = param.max_trajectory_length; @@ -170,9 +171,10 @@ TrajectoryPoints resampleTrajectory( // Step1. Resample front trajectory // Arc length from the initial point to the closest point - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double negative_front_arclength_value = motion_utils::calcSignedArcLength( + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = autoware::motion_utils::calcSignedArcLength( input, current_pose.position, current_seg_idx, input.at(0).pose.position, static_cast(0)); const auto front_arclength_value = std::fabs(negative_front_arclength_value); @@ -246,14 +248,14 @@ TrajectoryPoints resampleTrajectory( return input; } - const auto output_traj = motion_utils::resampleTrajectory( - motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); - auto output = motion_utils::convertToTrajectoryPointArray(output_traj); + const auto output_traj = autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); + auto output = autoware::motion_utils::convertToTrajectoryPointArray(output_traj); // add end point directly to consider the endpoint velocity. if (is_endpoint_included) { constexpr double ep_dist = 1.0E-3; - if (tier4_autoware_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { + if (autoware::universe_utils::calcDistance2d(output.back(), input.back()) < ep_dist) { output.back() = input.back(); } else { output.push_back(input.back()); diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index dc5f4f19ce771..7f263fdea5e36 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" -#include "autoware_velocity_smoother/trajectory_utils.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" #include #include @@ -252,7 +252,7 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::resampleTrajectory( const auto tp1 = input.at(i + 1); const double dist_thr = 0.001; // 1mm - const double dist_tp0_tp1 = tier4_autoware_utils::calcDistance2d(tp0, tp1); + const double dist_tp0_tp1 = autoware::universe_utils::calcDistance2d(tp0, tp1); if (std::fabs(dist_tp0_tp1) < dist_thr) { output.push_back(input.at(i)); continue; @@ -298,9 +298,9 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilt for (double s = 0; s < in_arclength.back(); s += points_interval) { out_arclength.push_back(s); } - const auto output_traj = - motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); - output = motion_utils::convertToTrajectoryPointArray(output_traj); + const auto output_traj = autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input), out_arclength); + output = autoware::motion_utils::convertToTrajectoryPointArray(output_traj); output.back() = input.back(); // keep the final speed. } else { output = input; @@ -355,7 +355,7 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilt } if ( - tier4_autoware_utils::calcDistance2d(output.at(end_index), output.at(index)) < + autoware::universe_utils::calcDistance2d(output.at(end_index), output.at(index)) < dist_threshold) { end_index = index; min_latacc_velocity = std::min( @@ -441,7 +441,7 @@ bool AnalyticalJerkConstrainedSmoother::applyForwardJerkFilter( for (size_t i = start_index + 1; i < base_trajectory.size(); ++i) { const double prev_vel = output_trajectory.at(i - 1).longitudinal_velocity_mps; const double ds = - tier4_autoware_utils::calcDistance2d(base_trajectory.at(i - 1), base_trajectory.at(i)); + autoware::universe_utils::calcDistance2d(base_trajectory.at(i - 1), base_trajectory.at(i)); const double dt = ds / std::max(prev_vel, 1.0); const double prev_acc = output_trajectory.at(i - 1).acceleration_mps2; @@ -487,8 +487,8 @@ bool AnalyticalJerkConstrainedSmoother::applyBackwardDecelFilter( } } for (size_t i = decel_target_index; i > start_index; --i) { - dist += - tier4_autoware_utils::calcDistance2d(output_trajectory.at(i - 1), output_trajectory.at(i)); + dist += autoware::universe_utils::calcDistance2d( + output_trajectory.at(i - 1), output_trajectory.at(i)); dist_to_target.at(i - 1) = dist; } diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index e3edcfbbec681..739ba6e7cef38 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +#include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" #include "interpolation/linear_interpolation.hpp" @@ -225,8 +225,8 @@ bool calcStopVelocityWithConstantJerkAccLimit( std::vector distances; distances.push_back(distance); for (size_t i = start_index; i < output_trajectory.size() - 1; ++i) { - distance += - tier4_autoware_utils::calcDistance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); + distance += autoware::universe_utils::calcDistance2d( + output_trajectory.at(i), output_trajectory.at(i + 1)); if (distance > xs.back()) { break; } diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index d759930eb4320..cab8e49a3b45d 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "autoware_velocity_smoother/trajectory_utils.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" #include @@ -131,7 +131,7 @@ bool JerkFilteredSmoother::apply( // to avoid getting 0 as a stop point, search zero velocity index from 1. // the size of the resampled trajectory must not be less than 2. - const auto zero_vel_id = motion_utils::searchZeroVelocityIndex( + const auto zero_vel_id = autoware::motion_utils::searchZeroVelocityIndex( opt_resampled_trajectory, 1, opt_resampled_trajectory.size()); if (!zero_vel_id) { @@ -385,7 +385,7 @@ TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter( output.front().longitudinal_velocity_mps = current_vel; output.front().acceleration_mps2 = current_acc; for (size_t i = 1; i < input.size(); ++i) { - const double ds = tier4_autoware_utils::calcDistance2d(input.at(i), input.at(i - 1)); + const double ds = autoware::universe_utils::calcDistance2d(input.at(i), input.at(i - 1)); const double max_dt = std::pow(6.0 * ds / j_max, 1.0 / 3.0); // assuming v0 = a0 = 0. const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); @@ -439,8 +439,8 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( merged.at(i).longitudinal_velocity_mps = current_vel; merged.at(i).acceleration_mps2 = current_acc; - const double ds = - tier4_autoware_utils::calcDistance2d(forward_filtered.at(i + 1), forward_filtered.at(i)); + const double ds = autoware::universe_utils::calcDistance2d( + forward_filtered.at(i + 1), forward_filtered.at(i)); const double max_dt = std::pow(6.0 * ds / std::fabs(j_min), 1.0 / 3.0); // assuming v0 = a0 = 0. const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt); diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index fff1166567aee..b489c994b0495 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "autoware_velocity_smoother/trajectory_utils.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" #include diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index b00936f3092ad..cb8451dba8302 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" -#include "autoware_velocity_smoother/trajectory_utils.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" #include diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index e1d72889203cc..700cf45b7eb9d 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_velocity_smoother/smoother/smoother_base.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" -#include "autoware_velocity_smoother/resample.hpp" -#include "autoware_velocity_smoother/trajectory_utils.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/velocity_smoother/resample.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" #include #include @@ -34,10 +34,10 @@ namespace TrajectoryPoints applyPreProcess( const TrajectoryPoints & input, const double interval, const bool use_resampling) { - using motion_utils::calcArcLength; - using motion_utils::convertToTrajectory; - using motion_utils::convertToTrajectoryPointArray; - using motion_utils::resampleTrajectory; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::convertToTrajectory; + using autoware::motion_utils::convertToTrajectoryPointArray; + using autoware::motion_utils::resampleTrajectory; if (!use_resampling) { return input; @@ -141,13 +141,13 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( // since the resampling takes a long time, omit the resampling when it is not requested if (use_resampling) { std::vector out_arclength; - const auto traj_length = motion_utils::calcArcLength(input); + const auto traj_length = autoware::motion_utils::calcArcLength(input); for (double s = 0; s < traj_length; s += points_interval) { out_arclength.push_back(s); } - const auto output_traj = - motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); - output = motion_utils::convertToTrajectoryPointArray(output_traj); + const auto output_traj = autoware::motion_utils::resampleTrajectory( + autoware::motion_utils::convertToTrajectory(input), out_arclength); + output = autoware::motion_utils::convertToTrajectoryPointArray(output_traj); output.back() = input.back(); // keep the final speed. } else { output = input; @@ -249,14 +249,15 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( } const auto steer_rate = steer_rate_arr.at(i); - if (steer_rate < tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate)) { + if (steer_rate < autoware::universe_utils::deg2rad(base_param_.max_steering_angle_rate)) { continue; } const auto mean_vel = (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0; const auto target_mean_vel = - mean_vel * (tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate) / steer_rate); + mean_vel * + (autoware::universe_utils::deg2rad(base_param_.max_steering_angle_rate) / steer_rate); for (size_t k = 0; k < 2; k++) { auto & velocity = output.at(i + k).longitudinal_velocity_mps; diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index 6ebf246748666..ea0453370e03a 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_velocity_smoother/trajectory_utils.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/linear_interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -34,7 +34,7 @@ inline void convertEulerAngleToMonotonic(std::vector & a) { for (unsigned int i = 1; i < a.size(); ++i) { const double da = a[i] - a[i - 1]; - a[i] = a[i - 1] + tier4_autoware_utils::normalizeRadian(da); + a[i] = a[i - 1] + autoware::universe_utils::normalizeRadian(da); } } @@ -87,7 +87,7 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( { const auto & seg_pt = trajectory.at(seg_idx); const auto & next_pt = trajectory.at(seg_idx + 1); - traj_p.pose = tier4_autoware_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); + traj_p.pose = autoware::universe_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); traj_p.longitudinal_velocity_mps = interpolation::lerp( seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); traj_p.acceleration_mps2 = @@ -110,7 +110,7 @@ TrajectoryPoints extractPathAroundIndex( { double dist_sum = 0.0; for (size_t i = index; i < trajectory.size() - 1; ++i) { - dist_sum += tier4_autoware_utils::calcDistance2d(trajectory.at(i), trajectory.at(i + 1)); + dist_sum += autoware::universe_utils::calcDistance2d(trajectory.at(i), trajectory.at(i + 1)); if (dist_sum > ahead_length) { ahead_index = i + 1; break; @@ -123,7 +123,7 @@ TrajectoryPoints extractPathAroundIndex( { double dist_sum{0.0}; for (size_t i = index; i != 0; --i) { - dist_sum += tier4_autoware_utils::calcDistance2d(trajectory.at(i), trajectory[i - 1]); + dist_sum += autoware::universe_utils::calcDistance2d(trajectory.at(i), trajectory[i - 1]); if (dist_sum > behind_length) { behind_index = i - 1; break; @@ -152,7 +152,7 @@ std::vector calcArclengthArray(const TrajectoryPoints & trajectory) for (unsigned int i = 1; i < trajectory.size(); ++i) { const TrajectoryPoint tp = trajectory.at(i); const TrajectoryPoint tp_prev = trajectory.at(i - 1); - dist += tier4_autoware_utils::calcDistance2d(tp.pose, tp_prev.pose); + dist += autoware::universe_utils::calcDistance2d(tp.pose, tp_prev.pose); arclength.at(i) = dist; } return arclength; @@ -164,7 +164,7 @@ std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & traj for (unsigned int i = 1; i < trajectory.size(); ++i) { const TrajectoryPoint tp = trajectory.at(i); const TrajectoryPoint tp_prev = trajectory.at(i - 1); - const double dist = tier4_autoware_utils::calcDistance2d(tp.pose, tp_prev.pose); + const double dist = autoware::universe_utils::calcDistance2d(tp.pose, tp_prev.pose); intervals.push_back(dist); } return intervals; @@ -173,8 +173,8 @@ std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & traj std::vector calcTrajectoryCurvatureFrom3Points( const TrajectoryPoints & trajectory, size_t idx_dist) { - using tier4_autoware_utils::calcCurvature; - using tier4_autoware_utils::getPoint; + using autoware::universe_utils::calcCurvature; + using autoware::universe_utils::getPoint; if (trajectory.size() < 3) { const std::vector k_arr(trajectory.size(), 0.0); @@ -300,7 +300,6 @@ bool calcStopDistWithJerkConstraints( } double x, v, a, j; - std::tuple state; switch (type) { case AccelerationType::TRAPEZOID: { @@ -596,7 +595,7 @@ std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest) { - const auto idx = motion_utils::searchZeroVelocityIndex(trajectory); + const auto idx = autoware::motion_utils::searchZeroVelocityIndex(trajectory); if (!idx) { return std::numeric_limits::max(); // stop point is located far away @@ -604,7 +603,7 @@ double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closes // TODO(Horibe): use arc length distance const double stop_dist = - tier4_autoware_utils::calcDistance2d(trajectory.at(*idx), trajectory.at(closest)); + autoware::universe_utils::calcDistance2d(trajectory.at(*idx), trajectory.at(closest)); return stop_dist; } diff --git a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp index f1b7dbb197ea2..5daf332fb0cf0 100644 --- a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp +++ b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_velocity_smoother/trajectory_utils.hpp" +#include "autoware/velocity_smoother/trajectory_utils.hpp" #include diff --git a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index d4374e21686e7..d384c319ee361 100644 --- a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_velocity_smoother/node.hpp" +#include "autoware/velocity_smoother/node.hpp" #include #include diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/CMakeLists.txt rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/CMakeLists.txt diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/README.md similarity index 100% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/README.md rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/README.md diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml similarity index 100% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg similarity index 100% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg similarity index 100% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg similarity index 100% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml similarity index 95% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/package.xml rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml index 8bb9e96c881c1..e9a4ba5094310 100644 --- a/planning/autoware_behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml @@ -22,12 +22,12 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common autoware_behavior_path_static_obstacle_avoidance_module + autoware_motion_utils autoware_rtc_interface + autoware_universe_utils lanelet2_extension - motion_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/plugins.xml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/plugins.xml rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/plugins.xml diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp similarity index 95% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp index cd515fbfce5da..2784a6b98de1a 100644 --- a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp @@ -14,7 +14,7 @@ #ifndef DATA_STRUCTS_HPP_ #define DATA_STRUCTS_HPP_ -#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp" namespace autoware::behavior_path_planner { diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp similarity index 95% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index c2e31dd6136f0..4c9f139365a06 100644 --- a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -14,8 +14,8 @@ #include "interface.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" #include #include diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp similarity index 96% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp index 77bcf3ec6e1aa..c7fbba34b1adb 100644 --- a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp @@ -15,7 +15,7 @@ #ifndef INTERFACE_HPP_ #define INTERFACE_HPP_ -#include "autoware_behavior_path_lane_change_module/interface.hpp" +#include "autoware/behavior_path_lane_change_module/interface.hpp" #include "data_structs.hpp" #include "scene.hpp" diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp similarity index 97% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 867187ce5055a..02b90186d9b2f 100644 --- a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -14,10 +14,10 @@ #include "manager.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include "data_structs.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -32,8 +32,8 @@ using autoware::behavior_path_planner::ObjectParameter; void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { + using autoware::universe_utils::getOrDeclareParameter; using autoware_perception_msgs::msg::ObjectClassification; - using tier4_autoware_utils::getOrDeclareParameter; // init manager interface initInterface(node, {"left", "right"}); diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp similarity index 96% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp index 2a282c42d1760..95609a430ac80 100644 --- a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp @@ -15,7 +15,7 @@ #ifndef MANAGER_HPP_ #define MANAGER_HPP_ -#include "autoware_behavior_path_lane_change_module/manager.hpp" +#include "autoware/behavior_path_lane_change_module/manager.hpp" #include "data_structs.hpp" #include "interface.hpp" diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp similarity index 94% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index ac3904c060f3a..a19cbda6ccad7 100644 --- a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -14,14 +14,14 @@ #include "scene.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" - -#include -#include +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" + +#include +#include #include #include @@ -195,10 +195,10 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( std::optional AvoidanceByLaneChange::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { + using autoware::motion_utils::findNearestIndex; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::calcLateralDeviation; using boost::geometry::return_centroid; - using motion_utils::findNearestIndex; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::calcLateralDeviation; const auto p = std::dynamic_pointer_cast(avoidance_parameters_); diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp similarity index 95% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp index bce49f8299083..d6bf6bc29df97 100644 --- a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp @@ -15,8 +15,8 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include "autoware_behavior_path_lane_change_module/scene.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" +#include "autoware/behavior_path_lane_change_module/scene.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp" #include "data_structs.hpp" #include diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp similarity index 98% rename from planning/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index a34c2ed22004c..cdefc7f82125a 100644 --- a/planning/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" #include #include diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-13-01.png b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-13-01.png similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-13-01.png rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-13-01.png diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-32-03.png b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-32-03.png similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-32-03.png rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-32-03.png diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/drivable_area_extraction_width.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/drivable_area_extraction_width.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/drivable_area_extraction_width.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/drivable_area_extraction_width.drawio.svg diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-151945.png b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-151945.png similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-151945.png rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-151945.png diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-152835.png b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-152835.png similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-152835.png rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-152835.png diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-095936.png b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-095936.png similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-095936.png rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-095936.png diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-152853.png b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-152853.png similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-152853.png rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-152853.png diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/opposite_directional_object.svg b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/opposite_directional_object.svg similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/opposite_directional_object.svg rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/opposite_directional_object.svg diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/same_directional_object.svg b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/same_directional_object.svg similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/same_directional_object.svg rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/same_directional_object.svg diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp similarity index 80% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index f42956c60bbce..9c85a463067de 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -55,4 +55,4 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp similarity index 94% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 07cea08d491f9..6ea15a9309c3c 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include #include -#include #include #include @@ -57,8 +57,8 @@ std::vector getAllKeys(const std::unordered_map & map) namespace autoware::behavior_path_planner { +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedPath; -using tier4_autoware_utils::Polygon2d; using tier4_planning_msgs::msg::PathWithLaneId; struct MinMaxValue @@ -176,7 +176,7 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel, const bool arg_is_object_on_ego_path, const std::optional & arg_latest_time_inside_ego_path) - : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)), + : uuid(autoware::universe_utils::toHexString(predicted_object.object_id)), label(predicted_object.classification.front().label), pose(predicted_object.kinematics.initial_pose_with_covariance.pose), shape(predicted_object.shape), @@ -374,8 +374,8 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface }; struct EgoPathReservePoly { - const tier4_autoware_utils::Polygon2d left_avoid; - const tier4_autoware_utils::Polygon2d right_avoid; + const autoware::universe_utils::Polygon2d left_avoid; + const autoware::universe_utils::Polygon2d right_avoid; }; bool canTransitSuccessState() override; @@ -431,11 +431,11 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const DynamicAvoidanceObject & object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; - std::optional calcEgoPathBasedDynamicObstaclePolygon( + std::optional calcEgoPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; - std::optional calcObjectPathBasedDynamicObstaclePolygon( + std::optional calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; - std::optional calcPredictedPathBasedDynamicObstaclePolygon( + std::optional calcPredictedPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const; EgoPathReservePoly calcEgoPathReservePoly(const PathWithLaneId & ego_path) const; @@ -455,10 +455,10 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface TargetObjectsManager target_objects_manager_; - mutable tier4_autoware_utils::StopWatch< + mutable autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml similarity index 94% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index a22e34f20a922..6dd430ce35f44 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -18,18 +18,18 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs + autoware_universe_utils autoware_vehicle_msgs geometry_msgs lanelet2_core lanelet2_extension - motion_utils object_recognition_utils pluginlib rclcpp signal_processing - tier4_autoware_utils visualization_msgs ament_cmake_ros diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp similarity index 98% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp index cc87e9f1c3c81..eccf2b2a1bfde 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp" +#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -152,7 +152,7 @@ void DynamicObstacleAvoidanceModuleManager::init(rclcpp::Node * node) void DynamicObstacleAvoidanceModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; auto & p = parameters_; { // common diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp similarity index 89% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index 0cf9b0fc7bd8b..8b411340e4ddc 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" +#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include "signal_processing/lowpass_filter_1d.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -41,7 +41,7 @@ namespace autoware::behavior_path_planner { namespace { -geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & point) +geometry_msgs::msg::Point toGeometryPoint(const autoware::universe_utils::Point2d & point) { geometry_msgs::msg::Point geom_obj_point; geom_obj_point.x = point.x(); @@ -65,24 +65,25 @@ MinMaxValue combineMinMaxValues(const MinMaxValue & r1, const MinMaxValue & r2) void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Pose & obj_pose) { - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "dynamic_objects_to_avoid", marker_array.markers.size(), visualization_msgs::msg::Marker::CUBE, - tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + autoware::universe_utils::createMarkerScale(3.0, 1.0, 1.0), + autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); marker.pose = obj_pose; marker_array.markers.push_back(marker); } void appendExtractedPolygonMarker( - MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly, const double obj_z) + MarkerArray & marker_array, const autoware::universe_utils::Polygon2d & obj_poly, + const double obj_z) { - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), visualization_msgs::msg::Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); + autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); // NOTE: obj_poly.outer() has already duplicated points to close the polygon. for (size_t i = 0; i < obj_poly.outer().size(); ++i) { @@ -116,7 +117,7 @@ std::pair projectObstacleVelocityToTrajectory( { const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; const double obj_yaw = tf2::getYaw(obj_pose.orientation); - const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); + const size_t obj_idx = autoware::motion_utils::findNearestIndex(path_points, obj_pose.position); const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); const Eigen::Rotation2Dd R_ego_to_obstacle(obj_yaw - path_yaw); @@ -171,27 +172,28 @@ double calcDiffAngleAgainstPath( const std::vector & path_points, const geometry_msgs::msg::Pose & target_pose) { - const size_t nearest_idx = motion_utils::findNearestIndex(path_points, target_pose.position); + const size_t nearest_idx = + autoware::motion_utils::findNearestIndex(path_points, target_pose.position); const double traj_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); const double target_yaw = tf2::getYaw(target_pose.orientation); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(target_yaw - traj_yaw); + const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); return diff_yaw; } [[maybe_unused]] double calcDiffAngleBetweenPaths( const std::vector & path_points, const PredictedPath & predicted_path) { - const size_t nearest_idx = - motion_utils::findNearestSegmentIndex(path_points, predicted_path.path.front().position); + const size_t nearest_idx = autoware::motion_utils::findNearestSegmentIndex( + path_points, predicted_path.path.front().position); const double ego_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); constexpr size_t max_predicted_path_size = 5; double signed_max_angle{0.0}; for (size_t i = 0; i < std::min(max_predicted_path_size, predicted_path.path.size()); ++i) { const double obj_yaw = tf2::getYaw(predicted_path.path.at(i).orientation); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_yaw - ego_yaw); + const double diff_yaw = autoware::universe_utils::normalizeRadian(obj_yaw - ego_yaw); if (std::abs(signed_max_angle) < std::abs(diff_yaw)) { signed_max_angle = diff_yaw; } @@ -203,32 +205,34 @@ double calcDistanceToPath( const std::vector & path_points, const geometry_msgs::msg::Point & target_pos) { - const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); + const size_t target_idx = autoware::motion_utils::findNearestIndex(path_points, target_pos); if (target_idx == 0 || target_idx == path_points.size() - 1) { const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); - const double angle_to_target_pos = tier4_autoware_utils::calcAzimuthAngle( + const double angle_to_target_pos = autoware::universe_utils::calcAzimuthAngle( path_points.at(target_idx).point.pose.position, target_pos); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(angle_to_target_pos - target_yaw); + const double diff_yaw = + autoware::universe_utils::normalizeRadian(angle_to_target_pos - target_yaw); if ( (target_idx == 0 && (diff_yaw < -M_PI_2 || M_PI_2 < diff_yaw)) || (target_idx == path_points.size() - 1 && (-M_PI_2 < diff_yaw && diff_yaw < M_PI_2))) { - return tier4_autoware_utils::calcDistance2d(path_points.at(target_idx), target_pos); + return autoware::universe_utils::calcDistance2d(path_points.at(target_idx), target_pos); } } - return std::abs(motion_utils::calcLateralOffset(path_points, target_pos)); + return std::abs(autoware::motion_utils::calcLateralOffset(path_points, target_pos)); } bool isLeft( const std::vector & path_points, const geometry_msgs::msg::Point & target_pos) { - const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); + const size_t target_idx = autoware::motion_utils::findNearestIndex(path_points, target_pos); const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); - const double angle_to_target_pos = tier4_autoware_utils::calcAzimuthAngle( + const double angle_to_target_pos = autoware::universe_utils::calcAzimuthAngle( path_points.at(target_idx).point.pose.position, target_pos); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(angle_to_target_pos - target_yaw); + const double diff_yaw = + autoware::universe_utils::normalizeRadian(angle_to_target_pos - target_yaw); if (0 < diff_yaw) { return true; @@ -279,7 +283,7 @@ std::optional> intersectLines( ++source_seg_idx) { for (int target_seg_idx = 0; target_seg_idx < static_cast(target_line.size()) - 1; ++target_seg_idx) { - const auto intersect_point = tier4_autoware_utils::intersect( + const auto intersect_point = autoware::universe_utils::intersect( source_line.at(source_seg_idx).position, source_line.at(source_seg_idx + 1).position, target_line.at(target_seg_idx), target_line.at(target_seg_idx + 1)); if (intersect_point) { @@ -325,7 +329,7 @@ bool DynamicObstacleAvoidanceModule::isExecutionRequested() const } // check if the ego is driving forward - const auto is_driving_forward = motion_utils::isDrivingForward(input_path.points); + const auto is_driving_forward = autoware::motion_utils::isDrivingForward(input_path.points); if (!is_driving_forward || !(*is_driving_forward)) { return false; } @@ -494,7 +498,7 @@ void DynamicObstacleAvoidanceModule::registerRegulatedObjects( const auto & predicted_objects = planner_data_->dynamic_object->objects; for (const auto & predicted_object : predicted_objects) { - const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id); + const auto obj_uuid = autoware::universe_utils::toHexString(predicted_object.object_id); const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; const double obj_vel_norm = std::hypot( predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, @@ -582,7 +586,7 @@ void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( const auto & predicted_objects = planner_data_->dynamic_object->objects; for (const auto & predicted_object : predicted_objects) { - const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id); + const auto obj_uuid = autoware::universe_utils::toHexString(predicted_object.object_id); const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; const double obj_vel_norm = std::hypot( predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, @@ -622,7 +626,7 @@ void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( // 1.f. calculate the object is on ego's path or not const double dist_obj_center_to_path = - std::abs(motion_utils::calcLateralOffset(input_path.points, obj_pose.position)); + std::abs(autoware::motion_utils::calcLateralOffset(input_path.points, obj_pose.position)); const bool is_object_on_ego_path = dist_obj_center_to_path < planner_data_->parameters.vehicle_width / 2.0 + parameters_->min_obj_lat_offset_to_ego_path; @@ -762,7 +766,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje // 2.g. check if the ego is not ahead of the object. const double signed_dist_ego_to_obj = [&]() { const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); - const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( + const double lon_offset_ego_to_obj = autoware::motion_utils::calcSignedArcLength( input_path.points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( @@ -784,7 +788,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje // 2.h. calculate longitudinal and lateral offset to avoid to generate object polygon by // "ego_path_base" - const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, time_while_collision); @@ -827,7 +831,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedOb getLateralLongitudinalOffset(input_path.points, object.pose, object.shape); const double signed_dist_ego_to_obj = [&]() { const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); - const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( + const double lon_offset_ego_to_obj = autoware::motion_utils::calcSignedArcLength( input_path.points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( @@ -887,19 +891,19 @@ LatFeasiblePaths DynamicObstacleAvoidanceModule::generateLateralFeasiblePaths( const double y = feasible_lat_offset; const auto feasible_left_bound_point = - tier4_autoware_utils::calcOffsetPose(ego_pose, x, -y, 0.0).position; + autoware::universe_utils::calcOffsetPose(ego_pose, x, -y, 0.0).position; ego_lat_feasible_paths.left_path.push_back(feasible_left_bound_point); const auto feasible_right_bound_point = - tier4_autoware_utils::calcOffsetPose(ego_pose, x, y, 0.0).position; + autoware::universe_utils::calcOffsetPose(ego_pose, x, y, 0.0).position; ego_lat_feasible_paths.right_path.push_back(feasible_right_bound_point); } - tier4_autoware_utils::appendMarkerArray( + autoware::universe_utils::appendMarkerArray( marker_utils::createPointsMarkerArray( ego_lat_feasible_paths.left_path, "ego_lat_feasible_left_path", 0, 0.6, 0.9, 0.9), &debug_marker_); - tier4_autoware_utils::appendMarkerArray( + autoware::universe_utils::appendMarkerArray( marker_utils::createPointsMarkerArray( ego_lat_feasible_paths.right_path, "ego_lat_feasible_right_path", 0, 0.6, 0.9, 0.9), &debug_marker_); @@ -914,7 +918,7 @@ LatFeasiblePaths DynamicObstacleAvoidanceModule::generateLateralFeasiblePaths( // check if the ego is close enough to the current ref path, meaning that lane change ends. const auto ego_pos = getEgoPose().position; const double dist_to_ref_path = - std::abs(motion_utils::calcLateralOffset(ego_ref_path_points, ego_pos)); + std::abs(autoware::motion_utils::calcLateralOffset(ego_ref_path_points, ego_pos)); constexpr double epsilon_dist_to_ref_path = 0.5; if (dist_to_ref_path < epsilon_dist_to_ref_path) { @@ -923,7 +927,7 @@ LatFeasiblePaths DynamicObstacleAvoidanceModule::generateLateralFeasiblePaths( } else { // check if the ego is during lane change. if (prev_input_ref_path_points_ && !prev_input_ref_path_points_->empty()) { - const double dist_ref_paths = std::abs(motion_utils::calcLateralOffset( + const double dist_ref_paths = std::abs(autoware::motion_utils::calcLateralOffset( ego_ref_path_points, prev_input_ref_path_points_->front().point.pose.position)); constexpr double epsilon_ref_paths_diff = 1.0; if (epsilon_ref_paths_diff < dist_ref_paths) { @@ -943,7 +947,7 @@ DynamicObstacleAvoidanceModule::calcCollisionSection( std::optional collision_start_idx{std::nullopt}; double lon_dist = 0.0; for (size_t i = ego_idx; i < ego_path.size() - 1; ++i) { - lon_dist += tier4_autoware_utils::calcDistance2d(ego_path.at(i), ego_path.at(i + 1)); + lon_dist += autoware::universe_utils::calcDistance2d(ego_path.at(i), ego_path.at(i + 1)); const double elapsed_time = lon_dist / ego_vel; const auto future_ego_pose = ego_path.at(i); @@ -952,7 +956,7 @@ DynamicObstacleAvoidanceModule::calcCollisionSection( if (future_obj_pose) { const double dist_ego_to_obj = - tier4_autoware_utils::calcDistance2d(future_ego_pose, *future_obj_pose); + autoware::universe_utils::calcDistance2d(future_ego_pose, *future_obj_pose); if (dist_ego_to_obj < 1.0) { if (!collision_start_idx) { collision_start_idx = i; @@ -978,7 +982,7 @@ TimeWhileCollision DynamicObstacleAvoidanceModule::calcTimeWhileCollision( // Set maximum time-to-collision 0 if the object longitudinally overlaps ego. // NOTE: This is to avoid objects running right beside ego even if time-to-collision is negative. const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); - const double lon_offset_ego_to_obj_idx = motion_utils::calcSignedArcLength( + const double lon_offset_ego_to_obj_idx = autoware::motion_utils::calcSignedArcLength( ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); const double relative_velocity = getEgoSpeed() - obj_tangent_vel; @@ -1050,7 +1054,7 @@ bool DynamicObstacleAvoidanceModule::willObjectCutIn( const bool will_object_cut_in = [&]() { for (const auto & predicted_path_point : predicted_path.path) { const double paths_lat_diff = - motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); + autoware::motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); if (std::abs(paths_lat_diff) < planner_data_->parameters.vehicle_width / 2.0) { return true; } @@ -1066,7 +1070,7 @@ bool DynamicObstacleAvoidanceModule::willObjectCutIn( 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( + autoware::motion_utils::calcSignedArcLength( ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) + lat_lon_offset.min_lon_offset; if ( @@ -1129,8 +1133,8 @@ DynamicObstacleAvoidanceModule::DecisionWithReason DynamicObstacleAvoidanceModul } // Check if object is in the lane before ego's lane change. - const double dist_to_ref_path_before_lane_change = - std::abs(motion_utils::calcLateralOffset(*ref_path_before_lane_change_, obj_pose.position)); + const double dist_to_ref_path_before_lane_change = std::abs( + autoware::motion_utils::calcLateralOffset(*ref_path_before_lane_change_, obj_pose.position)); const double epsilon_dist_checking_in_lane = calcObstacleWidth(obj_shape); if (epsilon_dist_checking_in_lane < dist_to_ref_path_before_lane_change) { return false; @@ -1185,8 +1189,9 @@ DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_perception_msgs::msg::Shape & obj_shape) const { - const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); - const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape); + const size_t obj_seg_idx = + autoware::motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); + const auto obj_points = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape); // TODO(murooka) calculation is not so accurate. std::vector obj_lat_offset_vec; @@ -1194,16 +1199,16 @@ DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( 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); + autoware::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); + autoware::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); + const double lon_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( + ego_path, obj_seg_idx, geom_obj_point); obj_lon_offset_vec.push_back(lon_offset); } @@ -1221,15 +1226,15 @@ MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( const PredictedPath & obj_path, const autoware_perception_msgs::msg::Shape & obj_shape, const TimeWhileCollision & time_while_collision) const { - const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, obj_pose.position); + const size_t obj_seg_idx = autoware::motion_utils::findNearestSegmentIndex( + ref_path_points_for_obj_poly, obj_pose.position); // calculate min/max longitudinal offset from object to path const auto obj_lon_offset = [&]() { 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 double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( + const double lon_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( ref_path_points_for_obj_poly, obj_seg_idx, geom_obj_point); obj_lon_offset_vec.push_back(lon_offset); } @@ -1294,7 +1299,7 @@ double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( { const auto & input_path_points = getPreviousModuleOutput().path.points; const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); + autoware::motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); constexpr double dist_threshold_additional_margin = 0.5; const double dist_threshold_paths = @@ -1312,7 +1317,7 @@ double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( std::reverse(cropped_ego_path_points.begin(), cropped_ego_path_points.end()); } if (cropped_ego_path_points.size() < 2) { - return motion_utils::calcArcLength(obj_path.path); + return autoware::motion_utils::calcArcLength(obj_path.path); } // calculate where the object's path will be forked from (= far from) the ego's path. @@ -1366,17 +1371,18 @@ double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( const auto prev_min_dist = calc_min_dist(prev_valid_obj_path_end_idx); const auto next_min_dist = calc_min_dist(next_valid_obj_path_end_idx); if (prev_min_dist && next_min_dist) { - const double segment_length = tier4_autoware_utils::calcDistance2d( + const double segment_length = autoware::universe_utils::calcDistance2d( obj_path.path.at(prev_valid_obj_path_end_idx), obj_path.path.at(next_valid_obj_path_end_idx)); const double partial_segment_length = segment_length * (dist_threshold_paths - *prev_min_dist) / (*next_min_dist - *prev_min_dist); - return motion_utils::calcSignedArcLength(obj_path.path, 0, prev_valid_obj_path_end_idx) + + return autoware::motion_utils::calcSignedArcLength( + obj_path.path, 0, prev_valid_obj_path_end_idx) + partial_segment_length; } } - return motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); + return autoware::motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); } // min value denotes near side, max value denotes far side @@ -1394,8 +1400,8 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( return true; } const size_t obj_point_idx = - motion_utils::findNearestIndex(ref_path_points_for_obj_poly, obj_pos); - const double paths_lat_diff = std::abs(motion_utils::calcLateralOffset( + autoware::motion_utils::findNearestIndex(ref_path_points_for_obj_poly, obj_pos); + const double paths_lat_diff = std::abs(autoware::motion_utils::calcLateralOffset( prev_object->ref_path_points_for_obj_poly, ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position)); @@ -1413,9 +1419,9 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( std::vector obj_lat_abs_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(ref_path_points_for_obj_poly, geom_obj_point); - const double obj_point_lat_offset = motion_utils::calcLateralOffset( + const size_t obj_point_seg_idx = autoware::motion_utils::findNearestSegmentIndex( + ref_path_points_for_obj_poly, geom_obj_point); + const double obj_point_lat_offset = autoware::motion_utils::calcLateralOffset( ref_path_points_for_obj_poly, geom_obj_point, obj_point_seg_idx); obj_lat_abs_offset_vec.push_back(obj_point_lat_offset); } @@ -1488,8 +1494,8 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( return true; } const size_t obj_point_idx = - motion_utils::findNearestIndex(ref_path_points_for_obj_poly, object.pose.position); - const double paths_lat_diff = std::abs(motion_utils::calcLateralOffset( + autoware::motion_utils::findNearestIndex(ref_path_points_for_obj_poly, object.pose.position); + const double paths_lat_diff = std::abs(autoware::motion_utils::calcLateralOffset( prev_object->ref_path_points_for_obj_poly, ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position)); @@ -1503,13 +1509,14 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( }(); const auto obj_occupancy_region = [&]() { - const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); std::vector lat_pos_vec; for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); - const double obj_point_lat_offset = motion_utils::calcLateralOffset( + const double obj_point_lat_offset = autoware::motion_utils::calcLateralOffset( ref_path_points_for_obj_poly, geom_obj_point, - motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, geom_obj_point)); + autoware::motion_utils::findNearestSegmentIndex( + ref_path_points_for_obj_poly, geom_obj_point)); lat_pos_vec.push_back(obj_point_lat_offset); } const auto current_pos_area = getMinMaxValues(lat_pos_vec); @@ -1559,7 +1566,7 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( } // NOTE: object does not have const only to update min_bound_lat_offset. -std::optional +std::optional DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { @@ -1569,16 +1576,16 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( auto ref_path_points_for_obj_poly = object.ref_path_points_for_obj_poly; - const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, object.pose.position); - // const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + const size_t obj_seg_idx = autoware::motion_utils::findNearestSegmentIndex( + ref_path_points_for_obj_poly, object.pose.position); + // const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); - const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( + const auto lon_bound_start_idx_opt = autoware::motion_utils::insertTargetPoint( obj_seg_idx, object.lon_offset_to_avoid->min_value, ref_path_points_for_obj_poly); const size_t updated_obj_seg_idx = (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 : obj_seg_idx; - const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( + const auto lon_bound_end_idx_opt = autoware::motion_utils::insertTargetPoint( updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, ref_path_points_for_obj_poly); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { @@ -1595,7 +1602,7 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( std::vector obj_inner_bound_poses; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { // NOTE: object.lat_offset_to_avoid->min_value is not the minimum value but the inner value. - obj_inner_bound_poses.push_back(tier4_autoware_utils::calcOffsetPose( + obj_inner_bound_poses.push_back(autoware::universe_utils::calcOffsetPose( ref_path_points_for_obj_poly.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, 0.0)); } @@ -1611,18 +1618,18 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( // Check if the object polygon intersects with the ego_lat_feasible_path. if (intersect_result) { const auto & [bound_seg_idx, intersect_point] = *intersect_result; - const double lon_offset = tier4_autoware_utils::calcDistance2d( + const double lon_offset = autoware::universe_utils::calcDistance2d( obj_inner_bound_poses.at(bound_seg_idx), intersect_point); const auto obj_inner_bound_start_idx_opt = - motion_utils::insertTargetPoint(bound_seg_idx, lon_offset, obj_inner_bound_poses); + autoware::motion_utils::insertTargetPoint(bound_seg_idx, lon_offset, obj_inner_bound_poses); if (obj_inner_bound_start_idx_opt) { return *obj_inner_bound_start_idx_opt; } } // Check if the object polygon is fully outside the ego_lat_feasible_path. - const double obj_poly_lat_offset = motion_utils::calcLateralOffset( + const double obj_poly_lat_offset = autoware::motion_utils::calcLateralOffset( ego_lat_feasible_path, obj_inner_bound_poses.front().position); if ( (!object.is_collision_left && 0 < obj_poly_lat_offset) || @@ -1643,17 +1650,17 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( std::vector feasible_obj_outer_bound_points; for (const auto & feasible_obj_inner_bound_pose : feasible_obj_inner_bound_poses) { feasible_obj_outer_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( + autoware::universe_utils::calcOffsetPose( feasible_obj_inner_bound_pose, 0.0, object.lat_offset_to_avoid->max_value - object.lat_offset_to_avoid->min_value, 0.0) .position); } // create obj_polygon from inner/outer bound points - tier4_autoware_utils::Polygon2d obj_poly; + autoware::universe_utils::Polygon2d obj_poly; const auto add_points_to_obj_poly = [&](const auto & bound_points) { for (const auto & bound_point : bound_points) { - obj_poly.outer().push_back(tier4_autoware_utils::Point2d(bound_point.x, bound_point.y)); + obj_poly.outer().push_back(autoware::universe_utils::Point2d(bound_point.x, bound_point.y)); } }; add_points_to_obj_poly(feasible_obj_inner_bound_points); @@ -1665,7 +1672,7 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( } // should be replace by the function calcPredictedPathBasedDynamicObstaclePolygon() (takagi) -std::optional +std::optional DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { @@ -1676,7 +1683,7 @@ DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( // calculate left and right bound std::vector obj_left_bound_points; std::vector obj_right_bound_points; - const double obj_path_length = motion_utils::calcArcLength(obj_path.path); + const double obj_path_length = autoware::motion_utils::calcArcLength(obj_path.path); for (size_t i = 0; i < obj_path.path.size(); ++i) { const double lon_offset = [&]() { if (i == 0) @@ -1694,26 +1701,26 @@ DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( const auto & obj_pose = obj_path.path.at(i); obj_left_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( + autoware::universe_utils::calcOffsetPose( obj_pose, lon_offset, object.shape.dimensions.y / 2.0 + parameters_->lat_offset_from_obstacle, 0.0) .position); obj_right_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( + autoware::universe_utils::calcOffsetPose( obj_pose, lon_offset, -object.shape.dimensions.y / 2.0 - parameters_->lat_offset_from_obstacle, 0.0) .position); } // create obj_polygon from inner/outer bound points - tier4_autoware_utils::Polygon2d obj_poly; + autoware::universe_utils::Polygon2d obj_poly; for (const auto & bound_point : obj_right_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + const auto obj_poly_point = autoware::universe_utils::Point2d(bound_point.x, bound_point.y); obj_poly.outer().push_back(obj_poly_point); } std::reverse(obj_left_bound_points.begin(), obj_left_bound_points.end()); for (const auto & bound_point : obj_left_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); + const auto obj_poly_point = autoware::universe_utils::Point2d(bound_point.x, bound_point.y); obj_poly.outer().push_back(obj_poly_point); } @@ -1724,7 +1731,7 @@ DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( // Calculate polygons according to predicted_path with certain confidence, // except for the area required for ego safety. // input: an object, the minimum area required for ego safety, and some global params -std::optional +std::optional DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const { @@ -1740,10 +1747,10 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( } } - tier4_autoware_utils::Polygon2d obj_points_as_poly; + autoware::universe_utils::Polygon2d obj_points_as_poly; for (const auto pose : obj_poses) { boost::geometry::append( - obj_points_as_poly, tier4_autoware_utils::toFootprint( + obj_points_as_poly, autoware::universe_utils::toFootprint( pose, object.shape.dimensions.x * 0.5, object.shape.dimensions.x * 0.5, object.shape.dimensions.y * 0.5) .outer()); @@ -1752,7 +1759,7 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( Polygon2d obj_poly; boost::geometry::convex_hull(obj_points_as_poly, obj_poly); - tier4_autoware_utils::MultiPolygon2d expanded_poly; + autoware::universe_utils::MultiPolygon2d expanded_poly; namespace strategy = boost::geometry::strategy::buffer; boost::geometry::buffer( obj_poly, expanded_poly, @@ -1761,7 +1768,7 @@ DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( strategy::point_circle()); if (expanded_poly.empty()) return {}; - tier4_autoware_utils::MultiPolygon2d output_poly; + autoware::universe_utils::MultiPolygon2d output_poly; boost::geometry::difference( expanded_poly[0], object.is_collision_left ? ego_path_poly.right_avoid : ego_path_poly.left_avoid, output_poly); @@ -1797,26 +1804,27 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg assert(!ego_path.points.empty()); - tier4_autoware_utils::LineString2d ego_path_lines; + autoware::universe_utils::LineString2d ego_path_lines; for (const auto & path_point : ego_path.points) { - ego_path_lines.push_back(tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d()); + ego_path_lines.push_back( + autoware::universe_utils::fromMsg(path_point.point.pose.position).to_2d()); } auto calcReservePoly = [&ego_path_lines]( const strategy::distance_asymmetric path_expand_strategy, const strategy::distance_asymmetric steer_expand_strategy, const std::vector & outer_body_path) - -> tier4_autoware_utils::Polygon2d { + -> autoware::universe_utils::Polygon2d { // reserve area based on the reference path - tier4_autoware_utils::MultiPolygon2d path_poly; + autoware::universe_utils::MultiPolygon2d path_poly; boost::geometry::buffer( ego_path_lines, path_poly, path_expand_strategy, strategy::side_straight(), strategy::join_round(), strategy::end_flat(), strategy::point_circle()); // reserve area steer to the avoidance path - tier4_autoware_utils::LineString2d steer_lines; + autoware::universe_utils::LineString2d steer_lines; for (const auto & point : outer_body_path) { - const auto bg_point = tier4_autoware_utils::fromMsg(point).to_2d(); + const auto bg_point = autoware::universe_utils::fromMsg(point).to_2d(); if (boost::geometry::within(bg_point, path_poly)) { if (steer_lines.size() != 0) { ; @@ -1826,12 +1834,12 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg } // boost::geometry::append(steer_lines, bg_point); } - tier4_autoware_utils::MultiPolygon2d steer_poly; + autoware::universe_utils::MultiPolygon2d steer_poly; boost::geometry::buffer( steer_lines, steer_poly, steer_expand_strategy, strategy::side_straight(), strategy::join_round(), strategy::end_flat(), strategy::point_circle()); - tier4_autoware_utils::MultiPolygon2d output_poly; + autoware::universe_utils::MultiPolygon2d output_poly; boost::geometry::union_(path_poly, steer_poly, output_poly); if (output_poly.size() != 1) { assert(false); @@ -1845,11 +1853,11 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg const double vehicle_half_width = planner_data_->parameters.vehicle_width * 0.5; const double reserve_width_obj_side = vehicle_half_width - parameters_->max_lat_offset_to_avoid; - const tier4_autoware_utils::Polygon2d left_avoid_poly = calcReservePoly( + const autoware::universe_utils::Polygon2d left_avoid_poly = calcReservePoly( strategy::distance_asymmetric(vehicle_half_width, reserve_width_obj_side), strategy::distance_asymmetric(vehicle_half_width, 0.0), motion_saturated_outer_paths.right_path); - const tier4_autoware_utils::Polygon2d right_avoid_poly = calcReservePoly( + const autoware::universe_utils::Polygon2d right_avoid_poly = calcReservePoly( strategy::distance_asymmetric(reserve_width_obj_side, vehicle_half_width), strategy::distance_asymmetric(0.0, vehicle_half_width), motion_saturated_outer_paths.left_path); diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp similarity index 98% rename from planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 611a796507f44..fc0fac5ff7b9d 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" #include #include diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt rename to planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml similarity index 94% rename from planning/autoware_behavior_path_external_request_lane_change_module/package.xml rename to planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml index b794789bd19e0..41db5ade58a74 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -21,11 +21,11 @@ autoware_behavior_path_lane_change_module autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_rtc_interface - motion_utils + autoware_universe_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml rename to planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/plugins.xml diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp similarity index 96% rename from planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp rename to planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index 904fbc1f0c4ae..c04914ed1c72b 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include "autoware_behavior_path_lane_change_module/interface.hpp" +#include "autoware/behavior_path_lane_change_module/interface.hpp" #include "scene.hpp" namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp similarity index 93% rename from planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp rename to planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp index 7cfce807cd663..5ec8b632adb92 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp @@ -15,8 +15,8 @@ #ifndef MANAGER_HPP_ #define MANAGER_HPP_ -#include "autoware_behavior_path_lane_change_module/manager.hpp" -#include "autoware_route_handler/route_handler.hpp" +#include "autoware/behavior_path_lane_change_module/manager.hpp" +#include "autoware/route_handler/route_handler.hpp" #include diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp similarity index 100% rename from planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp rename to planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp similarity index 95% rename from planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp rename to planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp index bc26688865798..4725635048788 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp @@ -15,7 +15,7 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include "autoware_behavior_path_lane_change_module/scene.hpp" +#include "autoware/behavior_path_lane_change_module/scene.hpp" #include diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp similarity index 98% rename from planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 12db2d1115efc..4a5fb562eb140 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "ament_index_cpp/get_package_share_directory.hpp" -#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" #include "autoware_planning_test_manager/autoware_planning_test_manager.hpp" #include "autoware_test_utils/autoware_test_utils.hpp" diff --git a/planning/autoware_behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/CMakeLists.txt rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt diff --git a/planning/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/README.md rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md diff --git a/planning/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml diff --git a/planning/autoware_behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_priority_rviz.png b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_priority_rviz.png similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_priority_rviz.png rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_priority_rviz.png diff --git a/planning/autoware_behavior_path_goal_planner_module/images/goal_priority_with_goal.png b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_priority_with_goal.png similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/goal_priority_with_goal.png rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_priority_with_goal.png diff --git a/planning/autoware_behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/images/shift_parking.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/shift_parking.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/images/shift_parking.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/shift_parking.drawio.svg diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/default_fixed_goal_planner.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp similarity index 81% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/default_fixed_goal_planner.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp index 44604084ef480..3b57ab67b265d 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ -#include "autoware_behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" +#include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" #include @@ -42,4 +42,4 @@ class DefaultFixedGoalPlanner : public FixedGoalPlannerBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp similarity index 79% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/fixed_goal_planner_base.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index 2a14bfaf72118..f670e3b05fa77 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ -#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" #include #include #include +using autoware::universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::LinearRing2d; using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner @@ -50,4 +50,4 @@ class FixedGoalPlannerBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/freespace_pull_over.hpp similarity index 75% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/freespace_pull_over.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/freespace_pull_over.hpp index f5e5df05474cd..bb968fe10cba2 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/freespace_pull_over.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ -#include "autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include -#include -#include +#include +#include +#include #include @@ -51,4 +51,4 @@ class FreespacePullOver : public PullOverPlannerBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/geometric_pull_over.hpp similarity index 84% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/geometric_pull_over.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/geometric_pull_over.hpp index 3d72c3089bd77..589379167b80c 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/geometric_pull_over.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ -#include "autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include +#include #include @@ -65,4 +65,4 @@ class GeometricPullOver : public PullOverPlannerBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp similarity index 94% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_module.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 4ecb11ec25776..e34cd202011d0 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -12,27 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ - -#include "autoware_behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" -#include "autoware_behavior_path_goal_planner_module/freespace_pull_over.hpp" -#include "autoware_behavior_path_goal_planner_module/geometric_pull_over.hpp" -#include "autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "autoware_behavior_path_goal_planner_module/goal_searcher.hpp" -#include "autoware_behavior_path_goal_planner_module/shift_pull_over.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" - -#include -#include -#include -#include -#include +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ + +#include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware/behavior_path_goal_planner_module/freespace_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/geometric_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" +#include "autoware/behavior_path_goal_planner_module/shift_pull_over.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include +#include +#include #include #include @@ -69,7 +69,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilter using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; -using tier4_autoware_utils::Polygon2d; +using autoware::universe_utils::Polygon2d; #define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ public: \ @@ -387,7 +387,7 @@ class GoalPlannerModule : public SceneModuleInterface std::shared_ptr ego_predicted_path_params; std::shared_ptr objects_filtering_params; std::shared_ptr safety_check_params; - tier4_autoware_utils::LinearRing2d vehicle_footprint; + autoware::universe_utils::LinearRing2d vehicle_footprint; PlannerData planner_data; ModuleStatus current_status; @@ -409,7 +409,7 @@ class GoalPlannerModule : public SceneModuleInterface const PlannerData & planner_data, const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output, const std::shared_ptr goal_searcher_, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint); + const autoware::universe_utils::LinearRing2d & vehicle_footprint); private: void initializeOccupancyGridMap( @@ -484,7 +484,7 @@ class GoalPlannerModule : public SceneModuleInterface std::deque odometry_buffer_stopped_; std::deque odometry_buffer_stuck_; - tier4_autoware_utils::LinearRing2d vehicle_footprint_; + autoware::universe_utils::LinearRing2d vehicle_footprint_; std::recursive_mutex mutex_; // TODO(Mamoru Sobue): isSafePath() modifies ThreadSafeData::check_collision, avoid this mutable @@ -668,4 +668,4 @@ class GoalPlannerModule : public SceneModuleInterface }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp similarity index 88% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 7ecd7063d623e..bdb267ce223d3 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -13,15 +13,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ -#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -128,4 +128,4 @@ struct GoalPlannerParameters }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp similarity index 88% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index 5897207a3b357..5d056add4665e 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ -#include "autoware_behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include namespace autoware::behavior_path_planner { -using tier4_autoware_utils::LinearRing2d; +using autoware::universe_utils::LinearRing2d; using BasicPolygons2d = std::vector; class GoalSearcher : public GoalSearcherBase @@ -72,4 +72,4 @@ class GoalSearcher : public GoalSearcherBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp similarity index 84% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher_base.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp index 984374ae166c5..baead4c229efd 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ -#include "autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include @@ -26,8 +26,8 @@ namespace autoware::behavior_path_planner { +using autoware::universe_utils::MultiPolygon2d; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::MultiPolygon2d; struct GoalCandidate { @@ -78,4 +78,4 @@ class GoalSearcherBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp similarity index 83% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/manager.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp index 291fbab7cafc3..32d816d9a37e8 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ -#include "autoware_behavior_path_goal_planner_module/goal_planner_module.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -55,4 +55,4 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp similarity index 85% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp index 7a27b2f43b4b3..d2ba416c7fa90 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ -#include "autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" #include #include @@ -25,8 +25,8 @@ #include #include +using autoware::universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::LinearRing2d; using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner @@ -67,7 +67,7 @@ struct PullOverPath partial_paths.at(i).points.end()); } } - path.points = motion_utils::removeOverlapPoints(path.points); + path.points = autoware::motion_utils::removeOverlapPoints(path.points); return path; } @@ -75,7 +75,8 @@ struct PullOverPath PathWithLaneId getParkingPath() const { const PathWithLaneId full_path = getFullPath(); - const size_t start_idx = motion_utils::findNearestIndex(full_path.points, start_pose.position); + const size_t start_idx = + autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position); PathWithLaneId parking_path{}; std::copy( @@ -141,4 +142,4 @@ class PullOverPlannerBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/shift_pull_over.hpp similarity index 86% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/shift_pull_over.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/shift_pull_over.hpp index 2296875e1d8d6..15b8d7ac81fae 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ -#include "autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include +#include #include @@ -59,4 +59,4 @@ class ShiftPullOver : public PullOverPlannerBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp similarity index 88% rename from planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/util.hpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 78e23b979094c..98a513d519b43 100644 --- a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ -#include "autoware_behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include +#include #include "visualization_msgs/msg/detail/marker_array__struct.hpp" #include @@ -42,7 +42,7 @@ using geometry_msgs::msg::Twist; using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -using Polygon2d = tier4_autoware_utils::Polygon2d; +using Polygon2d = autoware::universe_utils::Polygon2d; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -94,8 +94,8 @@ std::vector createPathFootPrints( // debug MarkerArray createPullOverAreaMarkerArray( - const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, - const std_msgs::msg::ColorRGBA & color, const double z); + const autoware::universe_utils::MultiPolygon2d area_polygons, + const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z); MarkerArray createPosesMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createTextsMarkerArray( @@ -110,4 +110,4 @@ MarkerArray createNumObjectsToAvoidTextsMarkerArray( const std_msgs::msg::ColorRGBA & color); } // namespace autoware::behavior_path_planner::goal_planner_utils -#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml similarity index 95% rename from planning/autoware_behavior_path_goal_planner_module/package.xml rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml index 952173564888a..2f107c5644f2e 100644 --- a/planning/autoware_behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml @@ -22,11 +22,11 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_rtc_interface - motion_utils + autoware_universe_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_path_goal_planner_module/plugins.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_path_goal_planner_module/plugins.xml rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/plugins.xml diff --git a/planning/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp similarity index 92% rename from planning/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index 48fc8d519161c..10e0137978bd6 100644 --- a/planning/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" -#include "autoware_behavior_path_goal_planner_module/util.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" #include #include @@ -25,7 +25,7 @@ namespace autoware::behavior_path_planner { -using Point2d = tier4_autoware_utils::Point2d; +using Point2d = autoware::universe_utils::Point2d; using tier4_planning_msgs::msg::PathWithLaneId; BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const diff --git a/planning/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp similarity index 88% rename from planning/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp index 15f17668e5e2b..ee35790ee2ab1 100644 --- a/planning/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_goal_planner_module/freespace_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/freespace_pull_over.hpp" -#include "autoware_behavior_path_goal_planner_module/util.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" #include #include @@ -57,7 +57,7 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) constexpr double straight_distance = 1.0; const Pose end_pose = use_back_ ? goal_pose - : tier4_autoware_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); + : autoware::universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); if (!planner_->makePlan(current_pose, end_pose)) { return {}; } @@ -85,7 +85,7 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) size_t index = std::distance(last_path.points.begin(), it); if (index == 0) continue; const double distance = - tier4_autoware_utils::calcDistance2d(end_pose.position, it->point.pose.position); + autoware::universe_utils::calcDistance2d(end_pose.position, it->point.pose.position); if (distance < th_goal_distance) { last_path.points.erase(it, last_path.points.end()); break; @@ -123,7 +123,7 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) // Check if driving forward for each path, return empty if not for (auto & path : partial_paths) { - if (!motion_utils::isDrivingForward(path.points)) { + if (!autoware::motion_utils::isDrivingForward(path.points)) { return {}; } } diff --git a/planning/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp similarity index 94% rename from planning/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp index 1ff47142dadb1..50cb090ca04d8 100644 --- a/planning/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_goal_planner_module/geometric_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/geometric_pull_over.hpp" -#include "autoware_behavior_path_goal_planner_module/util.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include #include diff --git a/planning/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp similarity index 95% rename from planning/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 9532def19029d..2ac0b9493e67a 100644 --- a/planning/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_goal_planner_module/goal_planner_module.hpp" - -#include "autoware_behavior_path_goal_planner_module/util.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" + +#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include #include @@ -40,15 +40,15 @@ #include using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; -using motion_utils::calcLongitudinalOffsetPose; -using motion_utils::calcSignedArcLength; -using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; -using motion_utils::insertDecelPoint; +using autoware::motion_utils::calcLongitudinalOffsetPose; +using autoware::motion_utils::calcSignedArcLength; +using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using autoware::motion_utils::insertDecelPoint; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::inverseTransformPose; using nav_msgs::msg::OccupancyGrid; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::inverseTransformPose; namespace autoware::behavior_path_planner { @@ -133,12 +133,35 @@ bool GoalPlannerModule::hasPreviousModulePathShapeChanged( return true; } - const auto current_path = previous_module_output.path; - - // the terminal distance is far - return calcDistance2d( - last_previous_module_output->path.points.back().point, - current_path.points.back().point) > 0.3; + // Calculate the lateral distance between each point of the current path and the nearest point of + // the last path + constexpr double LATERAL_DEVIATION_THRESH = 0.3; + for (const auto & p : previous_module_output.path.points) { + const size_t nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( + last_previous_module_output->path.points, p.point.pose.position); + const auto seg_front = last_previous_module_output->path.points.at(nearest_seg_idx); + const auto seg_back = last_previous_module_output->path.points.at(nearest_seg_idx + 1); + // Check if the target point is within the segment + const Eigen::Vector3d segment_vec{ + seg_back.point.pose.position.x - seg_front.point.pose.position.x, + seg_back.point.pose.position.y - seg_front.point.pose.position.y, 0.0}; + const Eigen::Vector3d target_vec{ + p.point.pose.position.x - seg_front.point.pose.position.x, + p.point.pose.position.y - seg_front.point.pose.position.y, 0.0}; + const double dot_product = segment_vec.x() * target_vec.x() + segment_vec.y() * target_vec.y(); + const double segment_length_squared = + segment_vec.x() * segment_vec.x() + segment_vec.y() * segment_vec.y(); + if (dot_product < 0 || dot_product > segment_length_squared) { + // p.point.pose.position is not within the segment, skip lateral distance check + continue; + } + const double lateral_distance = std::abs(autoware::motion_utils::calcLateralOffset( + last_previous_module_output->path.points, p.point.pose.position, nearest_seg_idx)); + if (lateral_distance > LATERAL_DEVIATION_THRESH) { + return true; + } + } + return false; } bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath( @@ -148,7 +171,7 @@ bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath( if (!last_previous_module_output) { return true; } - return std::abs(motion_utils::calcLateralOffset( + return std::abs(autoware::motion_utils::calcLateralOffset( last_previous_module_output->path.points, planner_data->self_odometry->pose.pose.position)) > 0.3; } @@ -157,9 +180,10 @@ bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath( const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output) const { - return std::abs(motion_utils::calcLateralOffset( + constexpr double LATERAL_DEVIATION_THRESH = 0.3; + return std::abs(autoware::motion_utils::calcLateralOffset( previous_module_output.path.points, planner_data->self_odometry->pose.pose.position)) > - 0.3; + LATERAL_DEVIATION_THRESH; } // generate pull over candidate paths @@ -746,7 +770,7 @@ bool GoalPlannerModule::canReturnToLaneParking() const Point & current_point = planner_data_->self_odometry->pose.pose.position; constexpr double th_distance = 0.5; const bool is_close_to_path = - std::abs(motion_utils::calcLateralOffset(path.points, current_point)) < th_distance; + std::abs(autoware::motion_utils::calcLateralOffset(path.points, current_point)) < th_distance; if (!is_close_to_path) { return false; } @@ -1242,10 +1266,10 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( // if ego is sufficiently close to the start of the nearest candidate path, the path is decided const auto & current_pose = planner_data->self_odometry->pose.pose; const size_t ego_segment_idx = - motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); + autoware::motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); - const size_t start_pose_segment_idx = - motion_utils::findNearestSegmentIndex(current_path.points, pull_over_path->start_pose.position); + const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( + current_path.points, pull_over_path->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( current_path.points, current_pose.position, ego_segment_idx, pull_over_path->start_pose.position, start_pose_segment_idx); @@ -1476,19 +1500,19 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); - const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( + const auto ego_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), M_PI_2); if (!ego_segment_idx) { return {std::numeric_limits::max(), std::numeric_limits::max()}; } - const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( + const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, thread_safe_data_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); - const size_t goal_pose_segment_idx = motion_utils::findNearestSegmentIndex( + const size_t goal_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, @@ -1622,7 +1646,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId auto stop_path = path; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto stop_idx = - motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); + autoware::motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); if (stop_idx) { debug_stop_pose_with_info_.set(stop_path.points.at(*stop_idx).point.pose, "feasible stop"); } @@ -1740,7 +1764,7 @@ bool GoalPlannerModule::hasFinishedCurrentPath() const auto current_path_end = thread_safe_data_.get_pull_over_path()->getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; - return tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < + return autoware::universe_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; } @@ -1764,8 +1788,10 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose; const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose; - const auto shift_start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - const auto shift_end_idx = motion_utils::findNearestIndex(path.points, end_pose.position); + const auto shift_start_idx = + autoware::motion_utils::findNearestIndex(path.points, start_pose.position); + const auto shift_end_idx = + autoware::motion_utils::findNearestIndex(path.points, end_pose.position); const auto is_ignore_signal = [this](const lanelet::Id & id) { if (!ignore_signal_.has_value()) { @@ -1808,8 +1834,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() constexpr double distance_threshold = 1.0; const auto stop_point = thread_safe_data_.get_pull_over_path()->partial_paths.front().points.back(); - const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength( - path.points, stop_point.point.pose.position, current_pose.position)); + const double distance_from_ego_to_stop_point = + std::abs(autoware::motion_utils::calcSignedArcLength( + path.points, stop_point.point.pose.position, current_pose.position)); return distance_from_ego_to_stop_point < distance_threshold; }); @@ -1853,7 +1880,7 @@ bool GoalPlannerModule::checkObjectsCollision( std::vector obj_polygons; for (const auto & object : target_objects.objects) { - obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); + obj_polygons.push_back(autoware::universe_utils::toPolygon2d(object)); } /* Expand ego collision check polygon @@ -1862,7 +1889,7 @@ bool GoalPlannerModule::checkObjectsCollision( * - `extra_lateral_margin` adds the lateral margin on curves. */ std::vector ego_polygons_expanded{}; - const auto curvatures = motion_utils::calcCurvature(path.points); + const auto curvatures = autoware::motion_utils::calcCurvature(path.points); for (size_t i = 0; i < path.points.size(); ++i) { const auto p = path.points.at(i); @@ -1877,7 +1904,7 @@ bool GoalPlannerModule::checkObjectsCollision( extra_stopping_margin, std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - const auto ego_polygon = tier4_autoware_utils::toFootprint( + const auto ego_polygon = autoware::universe_utils::toFootprint( p.point.pose, planner_data->parameters.base_link2front + collision_check_margin + extra_stopping_margin, planner_data->parameters.base_link2rear + collision_check_margin, @@ -2175,7 +2202,7 @@ static std::vector filterOb for (const auto & target_lane : target_lanes) { const auto lane_poly = target_lane.polygon2d().basicPolygon(); for (const auto & filtered_object : filtered_objects.objects) { - const auto object_bbox = tier4_autoware_utils::toPolygon2d(filtered_object); + const auto object_bbox = autoware::universe_utils::toPolygon2d(filtered_object); if (boost::geometry::within(object_bbox, lane_poly)) { within_filtered_objects.push_back(filtered_object); } @@ -2258,7 +2285,7 @@ std::pair GoalPlannerModule::isSafePath( lanelet::utils::conversion::toGeomMsgPt(fist_road_lane.centerline().front()); const double lane_yaw = lanelet::utils::getLaneletAngle(fist_road_lane, first_road_point); first_road_pose.position = first_road_point; - first_road_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); + first_road_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); // if current ego pose is before pull over lanes segment, use first road lanelet center pose if ( calcSignedArcLength(pull_over_path.points, first_road_pose.position, current_pose.position) < @@ -2335,6 +2362,10 @@ void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); + using autoware::motion_utils::createStopVirtualWallMarker; + using autoware::universe_utils::createDefaultMarker; + using autoware::universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerScale; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; @@ -2342,10 +2373,6 @@ void GoalPlannerModule::setDebugData() using marker_utils::showPolygon; using marker_utils::showPredictedPath; using marker_utils::showSafetyCheckInfo; - using motion_utils::createStopVirtualWallMarker; - using tier4_autoware_utils::createDefaultMarker; - using tier4_autoware_utils::createMarkerColor; - using tier4_autoware_utils::createMarkerScale; const auto header = planner_data_->route_handler->getRouteHeader(); @@ -2353,7 +2380,7 @@ void GoalPlannerModule::setDebugData() for (auto & marker : added.markers) { marker.lifetime = rclcpp::Duration::from_seconds(1.5); } - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + autoware::universe_utils::appendMarkerArray(added, &debug_marker_); }; if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas @@ -2401,10 +2428,10 @@ void GoalPlannerModule::setDebugData() createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST, - tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); + autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); const double ego_z = planner_data_->self_odometry->pose.pose.position.z; for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) { for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) { @@ -2412,19 +2439,19 @@ void GoalPlannerModule::setDebugData() const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size()); marker.points.push_back( - tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), ego_z)); + autoware::universe_utils::createPoint(current_point.x(), current_point.y(), ego_z)); marker.points.push_back( - tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), ego_z)); + autoware::universe_utils::createPoint(next_point.x(), next_point.y(), ego_z)); } } debug_marker_.markers.push_back(marker); if (parameters_->safety_check_params.enable_safety_check) { - tier4_autoware_utils::appendMarkerArray( + autoware::universe_utils::appendMarkerArray( goal_planner_utils::createLaneletPolygonMarkerArray( debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header, "expanded_pull_over_lane_between_ego", - tier4_autoware_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), + autoware::universe_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), &debug_marker_); } @@ -2551,7 +2578,7 @@ void GoalPlannerModule::printParkingPositionError() const real_shoulder_to_map_shoulder + parameters_->margin_from_boundary - dy; RCLCPP_INFO( getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy, - tier4_autoware_utils::rad2deg( + autoware::universe_utils::rad2deg( tf2::getYaw(current_pose.orientation) - tf2::getYaw(thread_safe_data_.get_modified_goal_pose()->goal_pose.orientation)), distance_from_real_shoulder); @@ -2607,7 +2634,7 @@ void GoalPlannerModule::GoalPlannerData::update( const PlannerData & planner_data_, const ModuleStatus & current_status_, const BehaviorModuleOutput & previous_module_output_, const std::shared_ptr goal_searcher_, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint_) + const autoware::universe_utils::LinearRing2d & vehicle_footprint_) { parameters = parameters_; ego_predicted_path_params = ego_predicted_path_params_; diff --git a/planning/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp similarity index 95% rename from planning/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index c268431b63836..04ee5278912ac 100644 --- a/planning/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_goal_planner_module/goal_searcher.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" -#include "autoware_behavior_path_goal_planner_module/util.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -33,9 +33,9 @@ namespace autoware::behavior_path_planner { using autoware::lane_departure_checker::LaneDepartureChecker; +using autoware::universe_utils::calcOffsetPose; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; -using tier4_autoware_utils::calcOffsetPose; // Sort with smaller longitudinal distances taking precedence over smaller lateral distances. struct SortByLongitudinalDistance @@ -136,7 +136,7 @@ GoalCandidates GoalSearcher::search( // todo(kosuke55): fix orientation for inverseTransformPoint temporarily Pose center_pose = p.point.pose; center_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); + autoware::universe_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); // ignore goal_pose near lane start const double distance_from_lane_start = @@ -156,7 +156,7 @@ GoalCandidates GoalSearcher::search( // original means non lateral offset poses const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = - std::abs(motion_utils::calcSignedArcLength( + std::abs(autoware::motion_utils::calcSignedArcLength( center_line_path.points, reference_goal_pose_.position, original_search_pose.position)); original_search_poses.push_back(original_search_pose); // for createAreaPolygon Pose search_pose{}; @@ -167,7 +167,7 @@ GoalCandidates GoalSearcher::search( search_pose = calcOffsetPose(original_search_pose, 0, sign * dy, 0); const auto transformed_vehicle_footprint = - transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); + transformVector(vehicle_footprint_, autoware::universe_utils::pose2transform(search_pose)); if (isInAreas(transformed_vehicle_footprint, getNoParkingAreaPolygons(pull_over_lanes))) { // break here to exclude goals located laterally in no_parking_areas @@ -245,8 +245,8 @@ void GoalSearcher::countObjectsToAvoid( for (const auto & object : objects.objects) { for (const auto & p : current_center_line_path.points) { const auto transformed_vehicle_footprint = - transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(p.point.pose)); - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + transformVector(vehicle_footprint_, autoware::universe_utils::pose2transform(p.point.pose)); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object); const double distance = boost::geometry::distance(obj_polygon, transformed_vehicle_footprint); if (distance > parameters_.object_recognition_collision_check_hard_margins.back()) { continue; @@ -431,9 +431,9 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( void GoalSearcher::createAreaPolygons( std::vector original_search_poses, const std::shared_ptr & planner_data) { - using tier4_autoware_utils::MultiPolygon2d; - using tier4_autoware_utils::Point2d; - using tier4_autoware_utils::Polygon2d; + using autoware::universe_utils::MultiPolygon2d; + using autoware::universe_utils::Point2d; + using autoware::universe_utils::Polygon2d; const double vehicle_width = planner_data->parameters.vehicle_width; const double base_link2front = planner_data->parameters.base_link2front; diff --git a/planning/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp similarity index 99% rename from planning/autoware_behavior_path_goal_planner_module/src/manager.cpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index be5845d2dd37d..901fe351fc68b 100644 --- a/planning/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_goal_planner_module/manager.hpp" +#include "autoware/behavior_path_goal_planner_module/manager.hpp" -#include "autoware_behavior_path_goal_planner_module/util.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -428,7 +428,7 @@ void GoalPlannerModuleManager::updateModuleParams( // object_recognition_collision_check_hard_margins, maximum_deceleration, shift_sampling_num or // parking_policy, there seems to be a problem when we use a temp value to check these values. - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; auto & p = parameters_; diff --git a/planning/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp similarity index 93% rename from planning/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index 5c1e874b949ab..beb27c67833ea 100644 --- a/planning/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_goal_planner_module/shift_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/shift_pull_over.hpp" -#include "autoware_behavior_path_goal_planner_module/util.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include #include @@ -101,8 +101,8 @@ std::optional ShiftPullOver::cropPrevModulePath( const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const { // clip previous module path to shift end pose nearest segment index - const size_t shift_end_idx = - motion_utils::findNearestSegmentIndex(prev_module_path.points, shift_end_pose.position); + const size_t shift_end_idx = autoware::motion_utils::findNearestSegmentIndex( + prev_module_path.points, shift_end_pose.position); std::vector clipped_points{ prev_module_path.points.begin(), prev_module_path.points.begin() + shift_end_idx}; if (clipped_points.empty()) { @@ -111,10 +111,10 @@ std::optional ShiftPullOver::cropPrevModulePath( // add projected shift end pose to clipped points PathPointWithLaneId projected_point = clipped_points.back(); - const double offset = motion_utils::calcSignedArcLength( + const double offset = autoware::motion_utils::calcSignedArcLength( prev_module_path.points, shift_end_idx, shift_end_pose.position); projected_point.point.pose = - tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + autoware::universe_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); clipped_points.push_back(projected_point); auto clipped_prev_module_path = prev_module_path; clipped_prev_module_path.points = clipped_points; @@ -131,7 +131,7 @@ std::optional ShiftPullOver::generatePullOverPath( // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = - tier4_autoware_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); + autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); // calculate lateral shift of previous module path terminal pose from road lane reference path const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( @@ -162,7 +162,7 @@ std::optional ShiftPullOver::generatePullOverPath( const Pose & shift_end_pose_prev_module_path = processed_prev_module_path->points.back().point.pose; const double shift_end_road_to_target_distance = - tier4_autoware_utils::inverseTransformPoint( + autoware::universe_utils::inverseTransformPoint( shift_end_pose.position, shift_end_pose_prev_module_path) .y; @@ -171,7 +171,7 @@ std::optional ShiftPullOver::generatePullOverPath( shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); - const auto shift_start_pose = motion_utils::calcLongitudinalOffsetPose( + const auto shift_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( processed_prev_module_path->points, shift_end_pose_prev_module_path.position, -before_shifted_pull_over_distance); @@ -188,8 +188,8 @@ std::optional ShiftPullOver::generatePullOverPath( if (!path_shifter.generate(&shifted_path, offset_back)) { return {}; } - shifted_path.path.points = motion_utils::removeOverlapPoints(shifted_path.path.points); - motion_utils::insertOrientation(shifted_path.path.points, true); + shifted_path.path.points = autoware::motion_utils::removeOverlapPoints(shifted_path.path.points); + autoware::motion_utils::insertOrientation(shifted_path.path.points, true); // set same orientation, because the reference center line orientation is not same to the shifted_path.path.points.back().point.pose.orientation = shift_end_pose.orientation; @@ -309,7 +309,7 @@ double ShiftPullOver::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; for (const auto & [k, segment_length] : - motion_utils::calcCurvatureAndArcLength(reversed_path.points)) { + autoware::motion_utils::calcCurvatureAndArcLength(reversed_path.points)) { // after shifted segment length const double after_segment_length = k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr); diff --git a/planning/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp similarity index 86% rename from planning/autoware_behavior_path_goal_planner_module/src/util.cpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index aa3b61899978c..99bef9c243508 100644 --- a/planning/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include #include #include #include -#include #include @@ -38,11 +38,11 @@ namespace autoware::behavior_path_planner::goal_planner_utils { -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -116,9 +116,9 @@ lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( const double ego_length_to_front = wheel_base + front_overhang; const double ego_width_to_front = !left_side ? (-wheel_tread / 2.0 - side_overhang) : (wheel_tread / 2.0 + side_overhang); - tier4_autoware_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front}; - const auto front_edge_glob = tier4_autoware_utils::transformPoint( - front_edge_local, tier4_autoware_utils::pose2transform(ego_pose)); + autoware::universe_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front}; + const auto front_edge_glob = autoware::universe_utils::transformPoint( + front_edge_local, autoware::universe_utils::pose2transform(ego_pose)); geometry_msgs::msg::Pose ego_front_pose; ego_front_pose.position = createPoint(front_edge_glob.x(), front_edge_glob.y(), ego_pose.position.z); @@ -180,8 +180,8 @@ PredictedObjects filterObjectsByLateralDistance( } MarkerArray createPullOverAreaMarkerArray( - const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, - const std_msgs::msg::ColorRGBA & color, const double z) + const autoware::universe_utils::MultiPolygon2d area_polygons, + const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z) { MarkerArray marker_array{}; for (size_t i = 0; i < area_polygons.size(); ++i) { @@ -205,7 +205,7 @@ MarkerArray createPosesMarkerArray( MarkerArray msg{}; int32_t i = 0; for (const auto & pose : poses) { - Marker marker = tier4_autoware_utils::createDefaultMarker( + Marker marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::ARROW, createMarkerScale(0.5, 0.25, 0.25), color); marker.pose = pose; @@ -305,11 +305,11 @@ double calcLateralDeviationBetweenPaths( { double lateral_deviation = 0.0; for (const auto & target_point : target_path.points) { - const size_t nearest_index = - motion_utils::findNearestIndex(reference_path.points, target_point.point.pose.position); + const size_t nearest_index = autoware::motion_utils::findNearestIndex( + reference_path.points, target_point.point.pose.position); lateral_deviation = std::max( lateral_deviation, - std::abs(tier4_autoware_utils::calcLateralDeviation( + std::abs(autoware::universe_utils::calcLateralDeviation( reference_path.points[nearest_index].point.pose, target_point.point.pose.position))); } return lateral_deviation; @@ -325,7 +325,8 @@ bool isReferencePath( std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose) { - const size_t end_idx = motion_utils::findNearestSegmentIndex(path.points, end_pose.position); + const size_t end_idx = + autoware::motion_utils::findNearestSegmentIndex(path.points, end_pose.position); std::vector clipped_points{ path.points.begin(), path.points.begin() + end_idx}; if (clipped_points.empty()) { @@ -334,9 +335,10 @@ std::optional cropPath(const PathWithLaneId & path, const Pose & // add projected end pose to clipped points PathPointWithLaneId projected_point = clipped_points.back(); - const double offset = motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); + const double offset = + autoware::motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); projected_point.point.pose = - tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + autoware::universe_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); clipped_points.push_back(projected_point); auto clipped_path = path; clipped_path.points = clipped_points; @@ -351,7 +353,8 @@ PathWithLaneId cropForwardPoints( double sum_length = 0; for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - const double seg_length = tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + const double seg_length = + autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (forward_length < sum_length + seg_length) { const auto cropped_points = std::vector{points.begin() + target_seg_idx, points.begin() + i}; @@ -390,33 +393,33 @@ PathWithLaneId extendPath( const auto & target_terminal_pose = target_path.points.back().point.pose; // generate clipped road lane reference path from previous module path terminal pose to shift end - const size_t target_path_terminal_idx = - motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); + const size_t target_path_terminal_idx = autoware::motion_utils::findNearestSegmentIndex( + reference_path.points, target_terminal_pose.position); PathWithLaneId clipped_path = cropForwardPoints(reference_path, target_path_terminal_idx, extend_length); // shift clipped path to previous module path terminal pose const double lateral_shift_from_reference_path = - motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); + autoware::motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); for (auto & p : clipped_path.points) { - p.point.pose = - tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, lateral_shift_from_reference_path, 0); + p.point.pose = autoware::universe_utils::calcOffsetPose( + p.point.pose, 0, lateral_shift_from_reference_path, 0); } auto extended_path = target_path; const auto start_point = std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) { const bool is_forward = - tier4_autoware_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose).x > - 0.0; - const bool is_close = tier4_autoware_utils::calcDistance2d( + autoware::universe_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose) + .x > 0.0; + const bool is_close = autoware::universe_utils::calcDistance2d( p.point.pose.position, target_terminal_pose.position) < 0.1; return is_forward && !is_close; }); std::copy(start_point, clipped_path.points.end(), std::back_inserter(extended_path.points)); - extended_path.points = motion_utils::removeOverlapPoints(extended_path.points); + extended_path.points = autoware::motion_utils::removeOverlapPoints(extended_path.points); return extended_path; } @@ -426,9 +429,9 @@ PathWithLaneId extendPath( const Pose & extend_pose) { const auto & target_terminal_pose = target_path.points.back().point.pose; - const size_t target_path_terminal_idx = - motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); - const double extend_distance = motion_utils::calcSignedArcLength( + const size_t target_path_terminal_idx = autoware::motion_utils::findNearestSegmentIndex( + reference_path.points, target_terminal_pose.position); + const double extend_distance = autoware::motion_utils::calcSignedArcLength( reference_path.points, target_path_terminal_idx, extend_pose.position); return extendPath(target_path, reference_path, extend_distance); @@ -442,7 +445,7 @@ std::vector createPathFootPrints( for (const auto & point : path.points) { const auto & pose = point.point.pose; footprints.push_back( - tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width)); + autoware::universe_utils::toFootprint(pose, base_to_front, base_to_rear, width)); } return footprints; } diff --git a/planning/autoware_behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/CMakeLists.txt rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt diff --git a/planning/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/README.md rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md diff --git a/planning/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-abort.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-abort.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-1.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-debug-1.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-1.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-debug-1.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-2.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-debug-2.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-2.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-debug-2.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-3.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-debug-3.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-3.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-debug-3.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-intersection_case.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-intersection_case.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-intersection_case.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-intersection_case.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_change_phases.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-lane_change_phases.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_change_phases.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-lane_change_phases.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_change.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_change.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change.drawio.svg diff --git a/planning/autoware_behavior_path_lane_change_module/images/lane_objects.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_objects.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/images/lane_objects.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_objects.drawio.svg diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp similarity index 77% rename from planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/interface.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 5ad4660dcf39c..9c014b77a7e82 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ -#include "autoware_behavior_path_lane_change_module/scene.hpp" -#include "autoware_behavior_path_lane_change_module/utils/base_class.hpp" -#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware_behavior_path_lane_change_module/utils/path.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_lane_change_module/scene.hpp" +#include "autoware/behavior_path_lane_change_module/utils/base_class.hpp" +#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include @@ -59,8 +59,6 @@ class LaneChangeInterface : public SceneModuleInterface LaneChangeInterface & operator=(LaneChangeInterface &&) = delete; ~LaneChangeInterface() override = default; - void processOnEntry() override; - void processOnExit() override; bool isExecutionRequested() const override; @@ -93,25 +91,6 @@ class LaneChangeInterface : public SceneModuleInterface MarkerArray getModuleVirtualWall() override; - // TODO(someone): remove this, and use base class function - [[deprecated]] BehaviorModuleOutput run() override - { - updateData(); - - if (!isWaitingApproval()) { - return plan(); - } - - // module is waiting approval. Check it. - if (isActivated()) { - RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); - return plan(); - } else { - RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate()."); - return planWaitingApproval(); - } - } - protected: std::shared_ptr parameters_; @@ -156,4 +135,4 @@ class LaneChangeInterface : public SceneModuleInterface }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp similarity index 83% rename from planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/manager.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp index e614a9a084a14..6e6d267dc445f 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ -#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "autoware_route_handler/route_handler.hpp" +#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware/route_handler/route_handler.hpp" #include @@ -73,4 +73,4 @@ class LaneChangeLeftModuleManager : public LaneChangeModuleManager }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp similarity index 96% rename from planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/scene.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 5391f75adfd46..69e9e90a236b9 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -11,11 +11,11 @@ // 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 AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "autoware_behavior_path_lane_change_module/utils/base_class.hpp" -#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/utils/base_class.hpp" +#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" #include #include @@ -206,4 +206,4 @@ class NormalLaneChange : public LaneChangeBase double stop_time_{0.0}; }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp similarity index 91% rename from planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/base_class.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index f1b7e6308313d..229f46f89377f 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -11,17 +11,17 @@ // 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 AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ - -#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware_behavior_path_lane_change_module/utils/debug_structs.hpp" -#include "autoware_behavior_path_lane_change_module/utils/path.hpp" -#include "autoware_behavior_path_lane_change_module/utils/utils.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ + +#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/utils/debug_structs.hpp" +#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -37,11 +37,11 @@ namespace autoware::behavior_path_planner { using autoware::route_handler::Direction; +using autoware::universe_utils::StopWatch; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_autoware_utils::StopWatch; using tier4_planning_msgs::msg::PathWithLaneId; class LaneChangeBase @@ -239,4 +239,4 @@ class LaneChangeBase mutable rclcpp::Clock clock_{RCL_ROS_TIME}; }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp similarity index 94% rename from planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/data_structs.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 0307799ff7bdc..0facd8c077505 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -11,11 +11,11 @@ // 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 AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include @@ -226,4 +226,4 @@ struct LanesPolygon }; } // namespace autoware::behavior_path_planner::data::lane_change -#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp similarity index 85% rename from planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/debug_structs.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp index 14b900b2fa826..3b8159dc3ad47 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -11,13 +11,13 @@ // 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 AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ -#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware_behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/utils/path.hpp" -#include +#include #include @@ -73,4 +73,4 @@ struct Debug }; } // namespace autoware::behavior_path_planner::data::lane_change -#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp similarity index 83% rename from planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/markers.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index 87724bb25a7b5..c40db47adf280 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ -#include "autoware_behavior_path_lane_change_module/utils/debug_structs.hpp" -#include "autoware_behavior_path_lane_change_module/utils/path.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_lane_change_module/utils/debug_structs.hpp" +#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -48,4 +48,4 @@ MarkerArray createDebugMarkerArray( const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); } // namespace marker_utils::lane_change_markers -#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp similarity index 78% rename from planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/path.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index 28f4925eebdd1..77c603e3bd975 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ -#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include @@ -50,4 +50,4 @@ struct LaneChangeStatus }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp similarity index 95% rename from planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 1fd716dc74c7c..4071c39568987 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ - -#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware_behavior_path_lane_change_module/utils/path.hpp" -#include "autoware_behavior_path_planner_common/parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ + +#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_planner_common/parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "rclcpp/logger.hpp" -#include -#include +#include +#include #include #include @@ -44,6 +44,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker:: using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using autoware::route_handler::Direction; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; @@ -53,7 +54,6 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; -using tier4_autoware_utils::Polygon2d; using tier4_planning_msgs::msg::PathWithLaneId; double calcLaneChangeResampleInterval( @@ -309,4 +309,4 @@ geometry_msgs::msg::Polygon createExecutionArea( double additional_lon_offset, double additional_lat_offset); } // namespace autoware::behavior_path_planner::utils::lane_change::debug -#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml similarity index 90% rename from planning/autoware_behavior_path_lane_change_module/package.xml rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index dca36f99e3faa..74d11dbcb1e13 100644 --- a/planning/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -12,6 +12,7 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Alqudah Mohammad Apache License 2.0 @@ -21,11 +22,11 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_rtc_interface - motion_utils + autoware_universe_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_path_lane_change_module/plugins.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_path_lane_change_module/plugins.xml rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/plugins.xml diff --git a/planning/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp similarity index 93% rename from planning/autoware_behavior_path_lane_change_module/src/interface.cpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 888594e0c41e5..0a5b31f9e32d2 100644 --- a/planning/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_lane_change_module/interface.hpp" +#include "autoware/behavior_path_lane_change_module/interface.hpp" -#include "autoware_behavior_path_lane_change_module/utils/markers.hpp" -#include "autoware_behavior_path_lane_change_module/utils/utils.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware/behavior_path_lane_change_module/utils/markers.hpp" +#include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" -#include +#include #include #include @@ -29,7 +29,7 @@ namespace autoware::behavior_path_planner { -using tier4_autoware_utils::appendMarkerArray; +using autoware::universe_utils::appendMarkerArray; using utils::lane_change::assignToCandidate; LaneChangeInterface::LaneChangeInterface( @@ -47,11 +47,6 @@ LaneChangeInterface::LaneChangeInterface( logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); } -void LaneChangeInterface::processOnEntry() -{ - waitApproval(); -} - void LaneChangeInterface::processOnExit() { module_type_->resetParameters(); @@ -79,7 +74,7 @@ void LaneChangeInterface::updateData() module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->updateSpecialData(); - if (isWaitingApproval()) { + if (isWaitingApproval() || module_type_->isAbortState()) { module_type_->updateLaneChangeStatus(); } @@ -116,14 +111,12 @@ BehaviorModuleOutput LaneChangeInterface::plan() updateSteeringFactorPtr(output); if (module_type_->isAbortState()) { - waitApproval(); const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus( std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, State::ABORTING); } else { - clearWaitingApproval(); const auto path = assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition()); updateRTCStatus( @@ -153,7 +146,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() if (!module_type_->isValidPath()) { updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + std::numeric_limits::lowest(), std::numeric_limits::lowest(), false, State::FAILED); path_candidate_ = std::make_shared(); return out; @@ -209,11 +202,6 @@ bool LaneChangeInterface::canTransitSuccessState() } } - if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { - log_debug_throttled("Abort process has completed."); - return true; - } - if (module_type_->hasFinishedLaneChange()) { module_type_->resetParameters(); log_debug_throttled("Lane change process has completed."); @@ -243,6 +231,16 @@ bool LaneChangeInterface::canTransitFailureState() return false; } + if (!module_type_->isValidPath()) { + log_debug_throttled("Transit to failure state due not to find valid path"); + return true; + } + + if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { + log_debug_throttled("Abort process has completed."); + return true; + } + if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) { if (module_type_->isStoppedAtRedTrafficLight()) { log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); @@ -354,9 +352,9 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o const auto current_position = module_type_->getEgoPosition(); const auto status = module_type_->getLaneChangeStatus(); - const auto start_distance = motion_utils::calcSignedArcLength( + const auto start_distance = autoware::motion_utils::calcSignedArcLength( output.path.points, current_position, status.lane_change_path.info.shift_line.start.position); - const auto finish_distance = motion_utils::calcSignedArcLength( + const auto finish_distance = autoware::motion_utils::calcSignedArcLength( output.path.points, current_position, status.lane_change_path.info.shift_line.end.position); steering_factor_interface_ptr_->updateSteeringFactor( diff --git a/planning/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp similarity index 98% rename from planning/autoware_behavior_path_lane_change_module/src/manager.cpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 8a359df98f04f..61ff54e8f99f3 100644 --- a/planning/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_lane_change_module/manager.hpp" +#include "autoware/behavior_path_lane_change_module/manager.hpp" -#include "autoware_behavior_path_lane_change_module/interface.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/behavior_path_lane_change_module/interface.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -36,7 +36,7 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) void LaneChangeModuleManager::initParams(rclcpp::Node * node) { - using tier4_autoware_utils::getOrDeclareParameter; + using autoware::universe_utils::getOrDeclareParameter; LaneChangeParameters p{}; @@ -281,7 +281,7 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod void LaneChangeModuleManager::updateModuleParams(const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; auto p = parameters_; diff --git a/planning/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp similarity index 97% rename from planning/autoware_behavior_path_lane_change_module/src/scene.cpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 44132f490c018..505bb3ef93340 100644 --- a/planning/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_lane_change_module/scene.hpp" +#include "autoware/behavior_path_lane_change_module/scene.hpp" -#include "autoware_behavior_path_lane_change_module/utils/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include #include -#include #include #include @@ -37,7 +37,7 @@ namespace autoware::behavior_path_planner { -using motion_utils::calcSignedArcLength; +using autoware::motion_utils::calcSignedArcLength; using utils::lane_change::calcMinimumLaneChangeLength; using utils::lane_change::createLanesPolygon; using utils::path_safety_checker::isPolygonOverlapLanelet; @@ -182,13 +182,13 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const double buffer = next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front; - const double path_length = motion_utils::calcArcLength(path.points); + const double path_length = autoware::motion_utils::calcArcLength(path.points); const size_t current_nearest_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes); - const auto start_pose = - motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0)); + const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( + path.points, 0, std::max(path_length - buffer, 0.0)); if (dist_to_terminal - base_to_front < buffer && start_pose) { // modify turn signal current_turn_signal_info.desired_start_point = *start_pose; @@ -389,10 +389,10 @@ void NormalLaneChange::insertStopPoint( // calculate distance from path front to the stationary object polygon on the ego lane. const auto polygon = - tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); for (const auto & polygon_p : polygon) { - const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); + const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); + const auto lateral_fp = autoware::motion_utils::calcLateralOffset(path.points, p_fp); // ignore if the point is around the ego path if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { @@ -442,7 +442,7 @@ void NormalLaneChange::insertStopPoint( // target_objects includes objects out of target lanes, so filter them out if (!boost::geometry::intersects( - tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + autoware::universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), lanelet::utils::combineLaneletsShape(status_.target_lanes) .polygon2d() .basicPolygon())) { @@ -690,7 +690,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } - const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const auto nearest_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( status_.lane_change_path.path.points, getEgoPose(), planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -729,7 +729,7 @@ bool NormalLaneChange::isAbleToStopSafely() const return false; } - const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const auto nearest_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( status_.lane_change_path.path.points, getEgoPose(), planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -798,7 +798,7 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons const auto vehicle_min_acc = std::max(p.min_acc, lane_change_parameters_->min_longitudinal_acc); const auto vehicle_max_acc = std::min(p.max_acc, lane_change_parameters_->max_longitudinal_acc); - const auto ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const auto ego_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prev_module_output_.path.points, getEgoPose(), p.ego_nearest_dist_threshold, p.ego_nearest_yaw_threshold); const auto max_path_velocity = @@ -914,8 +914,9 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( } auto prepare_segment = prev_module_output_.path; - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prepare_segment.points, getEgoPose(), 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prepare_segment.points, getEgoPose(), 3.0, 1.0); utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); return prepare_segment; @@ -991,11 +992,11 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( } const auto is_ahead_of_ego = [&path, ¤t_pose](const auto & object) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object).outer(); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); double max_dist_ego_to_obj = std::numeric_limits::lowest(); for (const auto & polygon_p : obj_polygon) { - const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); } @@ -1083,11 +1084,11 @@ void NormalLaneChange::filterAheadTerminalObjects( // ignore object that are ahead of terminal lane change start utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object).outer(); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); // ignore object that are ahead of terminal lane change start auto distance_to_terminal_from_object = std::numeric_limits::max(); for (const auto & polygon_p : obj_polygon) { - const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); Pose polygon_pose; polygon_pose.position = obj_p; polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; @@ -1667,7 +1668,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); } - const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( + const auto lane_changing_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( prev_module_output_.path.points, current_lane_terminal_point, -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); @@ -1752,7 +1753,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( target_lane_reference_path, shift_length); auto reference_segment = prev_module_output_.path; - const double length_to_lane_changing_start = motion_utils::calcSignedArcLength( + const double length_to_lane_changing_start = autoware::motion_utils::calcSignedArcLength( reference_segment.points, reference_segment.points.front().point.pose.position, lane_changing_start_pose->position); utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); @@ -1894,12 +1895,12 @@ bool NormalLaneChange::calcAbortPath() lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, 0.0); const auto ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( lane_changing_path.points, ref.points.back().point.pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); }); - const auto ego_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto ego_pose_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( lane_changing_path.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); const auto get_abort_idx_and_distance = [&](const double param_time) { @@ -1981,10 +1982,10 @@ bool NormalLaneChange::calcAbortPath() // reference_lane_segment = terminal_path->path; // } const auto return_pose = shifted_path.path.points.at(abort_return_idx).point.pose; - const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const auto seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( reference_lane_segment.points, return_pose, common_param.ego_nearest_dist_threshold, common_param.ego_nearest_yaw_threshold); - reference_lane_segment.points = motion_utils::cropPoints( + reference_lane_segment.points = autoware::motion_utils::cropPoints( reference_lane_segment.points, return_pose.position, seg_idx, common_param.forward_path_length, 0.0); } @@ -2079,7 +2080,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( utils::path_safety_checker::updateCollisionCheckDebugMap( debug_data, current_debug_data, is_safe); const auto & obj_pose = obj.initial_pose.pose; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(obj_pose, obj.shape); path_safety_status.is_object_coming_from_rear |= !utils::path_safety_checker::isTargetObjectFront( path, current_pose, common_parameters.vehicle_info, obj_polygon); diff --git a/planning/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp similarity index 94% rename from planning/autoware_behavior_path_lane_change_module/src/utils/markers.cpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 2306eef90e61d..9a553cf97af5c 100644 --- a/planning/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/marker_utils/colors.hpp" -#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/colors.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include +#include #include -#include #include #include @@ -34,9 +34,9 @@ namespace marker_utils::lane_change_markers { +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerScale; using geometry_msgs::msg::Point; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerScale; MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) { @@ -172,7 +172,7 @@ MarkerArray createDebugMarkerArray( MarkerArray debug_marker; const auto add = [&debug_marker](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker); + autoware::universe_utils::appendMarkerArray(added, &debug_marker); }; if (!debug_data.execution_area.points.empty()) { diff --git a/planning/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp similarity index 91% rename from planning/autoware_behavior_path_lane_change_module/src/utils/utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index b07d2100f210a..d989a1e6d301d 100644 --- a/planning/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -12,28 +12,28 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_lane_change_module/utils/utils.hpp" - -#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware_behavior_path_lane_change_module/utils/path.hpp" -#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware_behavior_path_planner_common/parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_lane_change_module/utils/utils.hpp" + +#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware/behavior_path_planner_common/parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include #include -#include -#include #include @@ -58,12 +58,12 @@ namespace autoware::behavior_path_planner::utils::lane_change { using autoware::route_handler::RouteHandler; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; using tier4_planning_msgs::msg::PathWithLaneId; using lanelet::ArcCoordinates; @@ -368,8 +368,8 @@ std::optional constructCandidatePath( LaneChangePath candidate_path; candidate_path.info = lane_change_info; - const auto lane_change_end_idx = - motion_utils::findNearestIndex(shifted_path.path.points, candidate_path.info.lane_changing_end); + const auto lane_change_end_idx = autoware::motion_utils::findNearestIndex( + shifted_path.path.points, candidate_path.info.lane_changing_end); if (!lane_change_end_idx) { RCLCPP_DEBUG(get_logger(), "Lane change end idx not found on target path."); @@ -385,7 +385,7 @@ std::optional constructCandidatePath( continue; } const auto nearest_idx = - motion_utils::findNearestIndex(target_segment.points, point.point.pose); + autoware::motion_utils::findNearestIndex(target_segment.points, point.point.pose); point.lane_ids = target_segment.points.at(*nearest_idx).lane_ids; } @@ -404,13 +404,13 @@ std::optional constructCandidatePath( std::prev(prepare_segment.points.end() - 1)->point.pose; const auto & lane_change_start_from_shifted = std::next(shifted_path.path.points.begin())->point.pose; - const auto yaw_diff2 = std::abs(tier4_autoware_utils::normalizeRadian( + const auto yaw_diff2 = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(prepare_segment_second_last_point.orientation) - tf2::getYaw(lane_change_start_from_shifted.orientation))); - if (yaw_diff2 > tier4_autoware_utils::deg2rad(5.0)) { + if (yaw_diff2 > autoware::universe_utils::deg2rad(5.0)) { RCLCPP_DEBUG( get_logger(), "Excessive yaw difference %.3f which exceeds the 5 degrees threshold.", - tier4_autoware_utils::rad2deg(yaw_diff2)); + autoware::universe_utils::rad2deg(yaw_diff2)); return std::nullopt; } } @@ -473,10 +473,10 @@ ShiftLine getLaneChangingShiftLine( shift_line.end_shift_length = shift_length; shift_line.start = lane_changing_start_pose; shift_line.end = lane_changing_end_pose; - shift_line.start_idx = - motion_utils::findNearestIndex(reference_path.points, lane_changing_start_pose.position); - shift_line.end_idx = - motion_utils::findNearestIndex(reference_path.points, lane_changing_end_pose.position); + shift_line.start_idx = autoware::motion_utils::findNearestIndex( + reference_path.points, lane_changing_start_pose.position); + shift_line.end_idx = autoware::motion_utils::findNearestIndex( + reference_path.points, lane_changing_end_pose.position); return shift_line; } @@ -768,9 +768,9 @@ CandidateOutput assignToCandidate( CandidateOutput candidate_output; candidate_output.path_candidate = lane_change_path.path; candidate_output.lateral_shift = utils::lane_change::getLateralShift(lane_change_path); - candidate_output.start_distance_to_path_change = motion_utils::calcSignedArcLength( + candidate_output.start_distance_to_path_change = autoware::motion_utils::calcSignedArcLength( lane_change_path.path.points, ego_position, lane_change_path.info.shift_line.start.position); - candidate_output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( + candidate_output.finish_distance_to_path_change = autoware::motion_utils::calcSignedArcLength( lane_change_path.path.points, ego_position, lane_change_path.info.shift_line.end.position); return candidate_output; @@ -806,9 +806,10 @@ std::vector convertToPredictedPath( const auto & minimum_lane_changing_velocity = lane_change_parameters.minimum_lane_changing_velocity; - const auto nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, - common_parameters.ego_nearest_yaw_threshold); + const auto nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); std::vector predicted_path; const auto vehicle_pose_frenet = @@ -820,8 +821,8 @@ std::vector convertToPredictedPath( const double velocity = std::max(initial_velocity + prepare_acc * t, minimum_lane_changing_velocity); const double length = initial_velocity * t + 0.5 * prepare_acc * t * t; - const auto pose = - motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + const auto pose = autoware::motion_utils::calcInterpolatedPose( + path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } @@ -835,8 +836,8 @@ std::vector convertToPredictedPath( const double velocity = lane_changing_velocity + lane_changing_acc * delta_t; const double length = lane_changing_velocity * delta_t + 0.5 * lane_changing_acc * delta_t * delta_t + offset; - const auto pose = - motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + const auto pose = autoware::motion_utils::calcInterpolatedPose( + path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } @@ -869,7 +870,7 @@ bool isParkedObject( const auto & object_pose = object.initial_pose.pose; const auto object_closest_index = - motion_utils::findNearestIndex(path.points, object_pose.position); + autoware::motion_utils::findNearestIndex(path.points, object_pose.position); const auto object_closest_pose = path.points.at(object_closest_index).point.pose; lanelet::ConstLanelet closest_lanelet; @@ -877,7 +878,8 @@ bool isParkedObject( return false; } - const double lat_dist = motion_utils::calcLateralOffset(path.points, object_pose.position); + const double lat_dist = + autoware::motion_utils::calcLateralOffset(path.points, object_pose.position); lanelet::BasicLineString2d bound; double center_to_bound_buffer = 0.0; if (lat_dist > 0.0) { @@ -935,7 +937,7 @@ bool isParkedObject( const auto & obj_pose = object.initial_pose.pose; const auto & obj_shape = object.shape; - const auto obj_poly = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape); + const auto obj_poly = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape); const auto obj_point = obj_pose.position; double max_dist_to_bound = std::numeric_limits::lowest(); @@ -994,7 +996,7 @@ bool passParkedObject( const auto & leading_obj = objects.at(*leading_obj_idx); auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); const auto leading_obj_poly = - tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); + autoware::universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { return false; } @@ -1002,14 +1004,14 @@ bool passParkedObject( const auto & current_path_end = current_lane_path.points.back().point.pose.position; double min_dist_to_end_of_current_lane = std::numeric_limits::max(); for (const auto & point : leading_obj_poly.outer()) { - const auto obj_p = tier4_autoware_utils::createPoint(point.x(), point.y(), 0.0); - const double dist = - motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, current_path_end); + const auto obj_p = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); + const double dist = autoware::motion_utils::calcSignedArcLength( + current_lane_path.points, obj_p, current_path_end); min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); if (is_goal_in_route) { const auto goal_pose = route_handler.getGoalPose(); - const double dist_to_goal = - motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, goal_pose.position); + const double dist_to_goal = autoware::motion_utils::calcSignedArcLength( + current_lane_path.points, obj_p, goal_pose.position); min_dist_to_end_of_current_lane = std::min(min_dist_to_end_of_current_lane, dist_to_goal); } } @@ -1059,15 +1061,15 @@ std::optional getLeadingStaticObjectIdx( continue; } - const double dist_back_to_obj = motion_utils::calcSignedArcLength( + const double dist_back_to_obj = autoware::motion_utils::calcSignedArcLength( path.points, path_end.point.pose.position, obj_pose.position); if (dist_back_to_obj > 0.0) { // object is not on the lane change path continue; } - const double dist_lc_start_to_obj = - motion_utils::calcSignedArcLength(path.points, lane_change_start.position, obj_pose.position); + const double dist_lc_start_to_obj = autoware::motion_utils::calcSignedArcLength( + path.points, lane_change_start.position, obj_pose.position); if (dist_lc_start_to_obj < 0.0) { // object is on the lane changing path or behind it. It will be detected in safety check continue; @@ -1122,7 +1124,7 @@ ExtendedPredictedObject transform( } 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); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths.at(i).path.emplace_back( t, *obj_pose, obj_vel_norm, obj_polygon); } @@ -1165,7 +1167,7 @@ Polygon2d getEgoCurrentFootprint( const auto base_to_rear = ego_info.rear_overhang_m; const auto width = ego_info.vehicle_width_m; - return tier4_autoware_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); + return autoware::universe_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); } bool isWithinIntersection( @@ -1252,10 +1254,14 @@ geometry_msgs::msg::Polygon createExecutionArea( const double backward_lon_offset = -base_to_rear; const double lat_offset = width / 2.0 + additional_lat_offset; - const auto p1 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); - const auto p2 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); - const auto p3 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); - const auto p4 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); + const auto p1 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); + const auto p2 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); + const auto p3 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); + const auto p4 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); geometry_msgs::msg::Polygon polygon; polygon.points.push_back(create_point32(p1)); diff --git a/planning/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp similarity index 98% rename from planning/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 3d6987e66e07f..504de657eabda 100644 --- a/planning/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "ament_index_cpp/get_package_share_directory.hpp" -#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" #include "autoware_planning_test_manager/autoware_planning_test_manager.hpp" #include "autoware_test_utils/autoware_test_utils.hpp" diff --git a/planning/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp similarity index 79% rename from planning/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp index 37040e0a8c275..68547a491324d 100644 --- a/planning/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp @@ -11,10 +11,10 @@ // 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 "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include -#include +#include +#include #include #include @@ -24,16 +24,16 @@ constexpr double epsilon = 1e-6; TEST(BehaviorPathPlanningLaneChangeUtilsTest, projectCurrentPoseToTarget) { geometry_msgs::msg::Pose ego_pose; - const auto ego_yaw = tier4_autoware_utils::deg2rad(0.0); - ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ego_yaw); - ego_pose.position = tier4_autoware_utils::createPoint(0, 0, 0); + const auto ego_yaw = autoware::universe_utils::deg2rad(0.0); + ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(ego_yaw); + ego_pose.position = autoware::universe_utils::createPoint(0, 0, 0); geometry_msgs::msg::Pose obj_pose; - const auto obj_yaw = tier4_autoware_utils::deg2rad(0.0); - obj_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(obj_yaw); - obj_pose.position = tier4_autoware_utils::createPoint(-4, 3, 0); + const auto obj_yaw = autoware::universe_utils::deg2rad(0.0); + obj_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(obj_yaw); + obj_pose.position = autoware::universe_utils::createPoint(-4, 3, 0); - const auto result = tier4_autoware_utils::inverseTransformPose(obj_pose, ego_pose); + const auto result = autoware::universe_utils::inverseTransformPose(obj_pose, ego_pose); EXPECT_NEAR(result.position.x, -4, epsilon); EXPECT_NEAR(result.position.y, 3, epsilon); diff --git a/planning/autoware_behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_path_planner/CMakeLists.txt rename to planning/behavior_path_planner/autoware_behavior_path_planner/CMakeLists.txt diff --git a/planning/autoware_behavior_path_planner/README.md b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md similarity index 100% rename from planning/autoware_behavior_path_planner/README.md rename to planning/behavior_path_planner/autoware_behavior_path_planner/README.md diff --git a/planning/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml similarity index 100% rename from planning/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml rename to planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml diff --git a/planning/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml similarity index 100% rename from planning/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml rename to planning/behavior_path_planner/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml diff --git a/planning/autoware_behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner/config/scene_module_manager.param.yaml similarity index 100% rename from planning/autoware_behavior_path_planner/config/scene_module_manager.param.yaml rename to planning/behavior_path_planner/autoware_behavior_path_planner/config/scene_module_manager.param.yaml diff --git a/planning/autoware_behavior_path_planner/docs/behavior_path_planner_interface_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_interface_design.md similarity index 100% rename from planning/autoware_behavior_path_planner/docs/behavior_path_planner_interface_design.md rename to planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_interface_design.md diff --git a/planning/autoware_behavior_path_planner/docs/behavior_path_planner_limitations.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_limitations.md similarity index 100% rename from planning/autoware_behavior_path_planner/docs/behavior_path_planner_limitations.md rename to planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_limitations.md diff --git a/planning/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md similarity index 100% rename from planning/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md rename to planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md diff --git a/planning/autoware_behavior_path_planner/image/checking_module_transition.png b/planning/behavior_path_planner/autoware_behavior_path_planner/image/checking_module_transition.png similarity index 100% rename from planning/autoware_behavior_path_planner/image/checking_module_transition.png rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/checking_module_transition.png diff --git a/planning/autoware_behavior_path_planner/image/limitations/limitation-chattering_shifts.png b/planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation-chattering_shifts.png similarity index 100% rename from planning/autoware_behavior_path_planner/image/limitations/limitation-chattering_shifts.png rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation-chattering_shifts.png diff --git a/planning/autoware_behavior_path_planner/image/limitations/limitation-corner.png b/planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation-corner.png similarity index 100% rename from planning/autoware_behavior_path_planner/image/limitations/limitation-corner.png rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation-corner.png diff --git a/planning/autoware_behavior_path_planner/image/limitations/limitation-intersection.png b/planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation-intersection.png similarity index 100% rename from planning/autoware_behavior_path_planner/image/limitations/limitation-intersection.png rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation-intersection.png diff --git a/planning/autoware_behavior_path_planner/image/limitations/limitation01-01.png b/planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation01-01.png similarity index 100% rename from planning/autoware_behavior_path_planner/image/limitations/limitation01-01.png rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation01-01.png diff --git a/planning/autoware_behavior_path_planner/image/limitations/limitation01-02-not-work.png b/planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation01-02-not-work.png similarity index 100% rename from planning/autoware_behavior_path_planner/image/limitations/limitation01-02-not-work.png rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation01-02-not-work.png diff --git a/planning/autoware_behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png b/planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png similarity index 100% rename from planning/autoware_behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png diff --git a/planning/autoware_behavior_path_planner/image/limitations/limitation01-04-equal-length.png b/planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation01-04-equal-length.png similarity index 100% rename from planning/autoware_behavior_path_planner/image/limitations/limitation01-04-equal-length.png rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation01-04-equal-length.png diff --git a/planning/autoware_behavior_path_planner/image/limitations/limitation01-04-not-equal.png b/planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation01-04-not-equal.png similarity index 100% rename from planning/autoware_behavior_path_planner/image/limitations/limitation01-04-not-equal.png rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/limitations/limitation01-04-not-equal.png diff --git a/planning/autoware_behavior_path_planner/image/manager/approved_modules_stack.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/approved_modules_stack.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/approved_modules_stack.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/approved_modules_stack.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/avoidance.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/avoidance.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/avoidance.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/avoidance.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/candidate_modules_stack.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/candidate_modules_stack.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/candidate_modules_stack.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/candidate_modules_stack.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/current_route_lanelet.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/current_route_lanelet.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/current_route_lanelet.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/current_route_lanelet.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/example_behavior.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/example_behavior.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/example_behavior.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/example_behavior.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/failure_modules.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/failure_modules.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/lane_change.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/lane_change.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/lane_change_remove.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/lane_change_remove.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/lane_change_skip.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/lane_change_skip.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/manager_overview.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/manager_overview.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/manager_overview.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/manager_overview.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/module_select.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/module_select.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/multiple_candidates.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/multiple_candidates.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/output_module.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/output_module.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/process_step1.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/process_step1.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/process_step2.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/process_step2.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/process_step4.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/process_step4.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/process_step5.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/process_step5.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/process_step6.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/process_step6.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/request_step1.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/request_step1.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/request_step2-2.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-2.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/request_step2-2.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-2.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/request_step2-3.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/request_step2-3.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/request_step2.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/request_step2.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/request_step3.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/request_step3.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/request_step4-2.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/request_step4-2.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/request_step4-3.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/request_step4-3.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/request_step4.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/request_step4.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/request_step5.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/request_step5.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/root_generation.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/root_generation.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/root_generation.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/root_generation.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/scene_module.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/scene_module.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/scene_module.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/scene_module.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/sub_managers.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/sub_managers.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/sub_managers.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/sub_managers.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/success_modules_remove.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/success_modules_remove.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/success_modules_skip.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_skip.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/success_modules_skip.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_skip.svg diff --git a/planning/autoware_behavior_path_planner/image/manager/waiting_approve.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/manager/waiting_approve.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.svg diff --git a/planning/autoware_behavior_path_planner/image/static_drivable_area_after_expansion.png b/planning/behavior_path_planner/autoware_behavior_path_planner/image/static_drivable_area_after_expansion.png similarity index 100% rename from planning/autoware_behavior_path_planner/image/static_drivable_area_after_expansion.png rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/static_drivable_area_after_expansion.png diff --git a/planning/autoware_behavior_path_planner/image/static_drivable_area_before_expansion.png b/planning/behavior_path_planner/autoware_behavior_path_planner/image/static_drivable_area_before_expansion.png similarity index 100% rename from planning/autoware_behavior_path_planner/image/static_drivable_area_before_expansion.png rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/static_drivable_area_before_expansion.png diff --git a/planning/autoware_behavior_path_planner/image/supported_module_avoidance.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_avoidance.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/supported_module_avoidance.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_avoidance.svg diff --git a/planning/autoware_behavior_path_planner/image/supported_module_avoidance_by_lane_change.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_avoidance_by_lane_change.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/supported_module_avoidance_by_lane_change.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_avoidance_by_lane_change.svg diff --git a/planning/autoware_behavior_path_planner/image/supported_module_goal_planner.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_goal_planner.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/supported_module_goal_planner.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_goal_planner.svg diff --git a/planning/autoware_behavior_path_planner/image/supported_module_lane_change.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_lane_change.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/supported_module_lane_change.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_lane_change.svg diff --git a/planning/autoware_behavior_path_planner/image/supported_module_lane_following.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_lane_following.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/supported_module_lane_following.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_lane_following.svg diff --git a/planning/autoware_behavior_path_planner/image/supported_module_start_planner.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_start_planner.svg similarity index 100% rename from planning/autoware_behavior_path_planner/image/supported_module_start_planner.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/supported_module_start_planner.svg diff --git a/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp similarity index 74% rename from planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/behavior_path_planner_node.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index 71e322cd4625d..3c62cc241b323 100644 --- a/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ -#include "autoware_behavior_path_planner/planner_manager.hpp" -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/interface/steering_factor_interface.hpp" +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "planner_manager.hpp" -#include +#include +#include #include #include @@ -86,17 +87,34 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::vector getRunningModules(); private: - rclcpp::Subscription::SharedPtr route_subscriber_; - rclcpp::Subscription::SharedPtr vector_map_subscriber_; - rclcpp::Subscription::SharedPtr velocity_subscriber_; - rclcpp::Subscription::SharedPtr acceleration_subscriber_; - rclcpp::Subscription::SharedPtr scenario_subscriber_; - rclcpp::Subscription::SharedPtr perception_subscriber_; - rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; - rclcpp::Subscription::SharedPtr costmap_subscriber_; - rclcpp::Subscription::SharedPtr traffic_signals_subscriber_; - rclcpp::Subscription::SharedPtr lateral_offset_subscriber_; - rclcpp::Subscription::SharedPtr operation_mode_subscriber_; + // subscriber + autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ + this, "~/input/route", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ + this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber velocity_subscriber_{ + this, "~/input/odometry"}; + autoware::universe_utils::InterProcessPollingSubscriber + acceleration_subscriber_{this, "~/input/accel"}; + autoware::universe_utils::InterProcessPollingSubscriber scenario_subscriber_{ + this, "~/input/scenario"}; + autoware::universe_utils::InterProcessPollingSubscriber perception_subscriber_{ + this, "~/input/perception"}; + autoware::universe_utils::InterProcessPollingSubscriber occupancy_grid_subscriber_{ + this, "~/input/occupancy_grid_map"}; + autoware::universe_utils::InterProcessPollingSubscriber costmap_subscriber_{ + this, "~/input/costmap"}; + autoware::universe_utils::InterProcessPollingSubscriber + traffic_signals_subscriber_{this, "~/input/traffic_signals"}; + autoware::universe_utils::InterProcessPollingSubscriber lateral_offset_subscriber_{ + this, "~/input/lateral_offset"}; + autoware::universe_utils::InterProcessPollingSubscriber + operation_mode_subscriber_{ + this, "/system/operation_mode/state", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber + external_limit_max_velocity_subscriber_{this, "/planning/scenario_planning/max_velocity"}; + + // publisher rclcpp::Publisher::SharedPtr path_publisher_; rclcpp::Publisher::SharedPtr turn_signal_publisher_; rclcpp::Publisher::SharedPtr hazard_signal_publisher_; @@ -104,31 +122,27 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr stop_reason_publisher_; rclcpp::Publisher::SharedPtr reroute_availability_publisher_; - rclcpp::Subscription::SharedPtr - external_limit_max_velocity_subscriber_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; std::map::SharedPtr> path_reference_publishers_; std::shared_ptr planner_data_; - - std::shared_ptr planner_manager_; - - std::unique_ptr steering_factor_interface_ptr_; - Scenario::SharedPtr current_scenario_{nullptr}; - + Scenario::ConstSharedPtr current_scenario_{nullptr}; LaneletMapBin::ConstSharedPtr map_ptr_{nullptr}; LaneletRoute::ConstSharedPtr route_ptr_{nullptr}; bool has_received_map_{false}; bool has_received_route_{false}; + std::shared_ptr planner_manager_; + + std::unique_ptr steering_factor_interface_ptr_; + std::mutex mutex_pd_; // mutex for planner_data_ std::mutex mutex_manager_; // mutex for bt_manager_ or planner_manager_ - std::mutex mutex_map_; // mutex for has_received_map_ and map_ptr_ - std::mutex mutex_route_; // mutex for has_received_route_ and route_ptr_ // setup + void takeData(); bool isDataReady(); // parameters @@ -222,10 +236,10 @@ class BehaviorPathPlannerNode : public rclcpp::Node const std::shared_ptr & path_candidate_ptr, const bool is_ready, const std::shared_ptr & planner_data); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ diff --git a/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp similarity index 93% rename from planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/planner_manager.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index 3e2afc2da6a95..a76284b70b7d8 100644 --- a/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "tier4_autoware_utils/ros/debug_publisher.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -39,12 +39,12 @@ namespace autoware::behavior_path_planner { -using tier4_autoware_utils::StopWatch; +using autoware::universe_utils::StopWatch; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopReasonArray; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; -using DebugPublisher = tier4_autoware_utils::DebugPublisher; +using DebugPublisher = autoware::universe_utils::DebugPublisher; using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; using DebugStringMsg = tier4_debug_msgs::msg::StringStamped; @@ -299,6 +299,12 @@ class PlannerManager return result; } + /** + * @brief find and set the closest lanelet within the route to current route lanelet + * @param planner data. + */ + void updateCurrentRouteLanelet(const std::shared_ptr & data); + void generateCombinedDrivableArea( BehaviorModuleOutput & output, const std::shared_ptr & data) const; @@ -307,12 +313,12 @@ class PlannerManager * @param planner data. * @return reference path. */ - BehaviorModuleOutput getReferencePath(const std::shared_ptr & data); + BehaviorModuleOutput getReferencePath(const std::shared_ptr & data) const; /** * @brief publish the root reference path and current route lanelet */ - void publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path); + void publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path) const; /** * @brief stop and unregister the module from manager. @@ -471,4 +477,4 @@ class PlannerManager }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_planner/launch/behavior_path_planner.launch.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/launch/behavior_path_planner.launch.xml similarity index 100% rename from planning/autoware_behavior_path_planner/launch/behavior_path_planner.launch.xml rename to planning/behavior_path_planner/autoware_behavior_path_planner/launch/behavior_path_planner.launch.xml diff --git a/planning/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml similarity index 97% rename from planning/autoware_behavior_path_planner/package.xml rename to planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 2efc32531432b..a4eeba6051b61 100644 --- a/planning/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -41,10 +41,12 @@ autoware_freespace_planning_algorithms autoware_frenet_planner autoware_lane_departure_checker + autoware_motion_utils autoware_path_sampler autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs behaviortree_cpp_v3 @@ -54,7 +56,6 @@ libboost-dev libopencv-dev magic_enum - motion_utils object_recognition_utils pluginlib rclcpp @@ -65,7 +66,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp similarity index 80% rename from planning/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index d9478ab4c1e93..0e586211bc9ee 100644 --- a/planning/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" -#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "motion_utils/trajectory/conversion.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include #include -#include #include @@ -28,20 +28,6 @@ #include #include -namespace -{ -rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) -{ - rclcpp::CallbackGroup::SharedPtr callback_group = - node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - auto sub_opt = rclcpp::SubscriptionOptions(); - sub_opt.callback_group = callback_group; - - return sub_opt; -} -} // namespace - namespace autoware::behavior_path_planner { using autoware::vehicle_info_utils::VehicleInfoUtils; @@ -78,54 +64,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod bound_publisher_ = create_publisher("~/debug/bound", 1); - const auto qos_transient_local = rclcpp::QoS{1}.transient_local(); - // subscriber - velocity_subscriber_ = create_subscription( - "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onOdometry, this, _1), - createSubscriptionOptions(this)); - acceleration_subscriber_ = create_subscription( - "~/input/accel", 1, std::bind(&BehaviorPathPlannerNode::onAcceleration, this, _1), - createSubscriptionOptions(this)); - perception_subscriber_ = create_subscription( - "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1), - createSubscriptionOptions(this)); - occupancy_grid_subscriber_ = create_subscription( - "~/input/occupancy_grid_map", 1, std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1), - createSubscriptionOptions(this)); - costmap_subscriber_ = create_subscription( - "~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1), - createSubscriptionOptions(this)); - traffic_signals_subscriber_ = - this->create_subscription( - "~/input/traffic_signals", 1, std::bind(&BehaviorPathPlannerNode::onTrafficSignals, this, _1), - createSubscriptionOptions(this)); - lateral_offset_subscriber_ = this->create_subscription( - "~/input/lateral_offset", 1, std::bind(&BehaviorPathPlannerNode::onLateralOffset, this, _1), - createSubscriptionOptions(this)); - operation_mode_subscriber_ = create_subscription( - "/system/operation_mode/state", qos_transient_local, - std::bind(&BehaviorPathPlannerNode::onOperationMode, this, _1), - createSubscriptionOptions(this)); - scenario_subscriber_ = create_subscription( - "~/input/scenario", 1, - [this](const Scenario::ConstSharedPtr msg) { - current_scenario_ = std::make_shared(*msg); - }, - createSubscriptionOptions(this)); - external_limit_max_velocity_subscriber_ = - create_subscription( - "/planning/scenario_planning/max_velocity", 1, - std::bind(&BehaviorPathPlannerNode::on_external_velocity_limiter, this, _1), - createSubscriptionOptions(this)); - - // route_handler - vector_map_subscriber_ = create_subscription( - "~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1), - createSubscriptionOptions(this)); - route_subscriber_ = create_subscription( - "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1), - createSubscriptionOptions(this)); - { const std::string path_candidate_name_space = "/planning/path_candidate/"; const std::string path_reference_name_space = "/planning/path_reference/"; @@ -176,8 +114,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod this, get_clock(), period_ns, std::bind(&BehaviorPathPlannerNode::run, this)); } - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() @@ -274,6 +213,100 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() return p; } +void BehaviorPathPlannerNode::takeData() +{ + // route + { + const auto msg = route_subscriber_.takeNewData(); + if (msg) { + if (msg->segments.empty()) { + RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); + } else { + route_ptr_ = msg; + has_received_route_ = true; + } + } + } + // map + { + const auto msg = vector_map_subscriber_.takeNewData(); + if (msg) { + map_ptr_ = msg; + has_received_map_ = true; + } + } + // velocity + { + const auto msg = velocity_subscriber_.takeData(); + if (msg) { + planner_data_->self_odometry = msg; + } + } + // acceleration + { + const auto msg = acceleration_subscriber_.takeData(); + if (msg) { + planner_data_->self_acceleration = msg; + } + } + // scenario + { + const auto msg = scenario_subscriber_.takeData(); + if (msg) { + current_scenario_ = msg; + } + } + // perception + { + const auto msg = perception_subscriber_.takeData(); + if (msg) { + planner_data_->dynamic_object = msg; + } + } + // occupancy_grid + { + const auto msg = occupancy_grid_subscriber_.takeData(); + if (msg) { + planner_data_->occupancy_grid = msg; + } + } + // costmap + { + const auto msg = costmap_subscriber_.takeData(); + if (msg) { + planner_data_->costmap = msg; + } + } + // traffic_signal + { + const auto msg = traffic_signals_subscriber_.takeData(); + if (msg) { + onTrafficSignals(msg); + } + } + // lateral_offset + { + const auto msg = lateral_offset_subscriber_.takeData(); + if (msg) { + onLateralOffset(msg); + } + } + // operation_mode + { + const auto msg = operation_mode_subscriber_.takeData(); + if (msg) { + planner_data_->operation_mode = msg; + } + } + // external_velocity_limiter + { + const auto msg = external_limit_max_velocity_subscriber_.takeData(); + if (msg) { + planner_data_->external_limit_max_velocity = msg; + } + } +} + // wait until mandatory data is ready bool BehaviorPathPlannerNode::isDataReady() { @@ -287,21 +320,17 @@ bool BehaviorPathPlannerNode::isDataReady() } { - std::lock_guard lk_route(mutex_route_); if (!route_ptr_) { return missing("route"); } } { - std::lock_guard lk_map(mutex_map_); if (!map_ptr_) { return missing("map"); } } - const std::lock_guard lock(mutex_pd_); // for planner_data_ - if (!planner_data_->dynamic_object) { return missing("dynamic_object"); } @@ -327,6 +356,8 @@ bool BehaviorPathPlannerNode::isDataReady() void BehaviorPathPlannerNode::run() { + takeData(); + if (!isDataReady()) { return; } @@ -341,7 +372,6 @@ void BehaviorPathPlannerNode::run() // check for map update LaneletMapBin::ConstSharedPtr map_ptr{nullptr}; { - std::lock_guard lk_map(mutex_map_); // for has_received_map_ and map_ptr_ if (has_received_map_) { // Note: duplicating the shared_ptr prevents the data from being deleted by another thread! map_ptr = map_ptr_; @@ -352,7 +382,6 @@ void BehaviorPathPlannerNode::run() // check for route update LaneletRoute::ConstSharedPtr route_ptr{nullptr}; { - std::lock_guard lk_route(mutex_route_); // for has_received_route_ and route_ptr_ if (has_received_route_) { // Note: duplicating the shared_ptr prevents the data from being deleted by another thread! route_ptr = route_ptr_; @@ -389,7 +418,9 @@ void BehaviorPathPlannerNode::run() const auto controlled_by_autoware_autonomously = planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && planner_data_->operation_mode->is_autoware_control_enabled; - if (!controlled_by_autoware_autonomously && !planner_manager_->hasApprovedModules()) + if ( + !controlled_by_autoware_autonomously && + !planner_manager_->hasNonAlwaysExecutableApprovedModules()) planner_manager_->resetCurrentRouteLanelet(planner_data_); // run behavior planner @@ -414,7 +445,7 @@ void BehaviorPathPlannerNode::run() const auto current_pose = planner_data_->self_odometry->pose.pose; if (!path->points.empty()) { const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(path->points); - path->points = motion_utils::cropPoints( + path->points = autoware::motion_utils::cropPoints( path->points, current_pose.position, current_seg_idx, planner_data_->parameters.forward_path_length, planner_data_->parameters.backward_path_length + @@ -441,7 +472,7 @@ void BehaviorPathPlannerNode::run() if ( output.modified_goal && /* has changed modified goal */ ( - !planner_data_->prev_modified_goal || tier4_autoware_utils::calcDistance2d( + !planner_data_->prev_modified_goal || autoware::universe_utils::calcDistance2d( planner_data_->prev_modified_goal->pose.position, output.modified_goal->pose.position) > 0.01)) { PoseWithUuidStamped modified_goal = *(output.modified_goal); @@ -535,27 +566,29 @@ void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDeb constexpr double scale_x = 1.0; constexpr double scale_y = 1.0; constexpr double scale_z = 1.0; - const auto scale = tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z); - const auto desired_section_color = tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); - const auto required_section_color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.999); + const auto scale = autoware::universe_utils::createMarkerScale(scale_x, scale_y, scale_z); + const auto desired_section_color = + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); + const auto required_section_color = + autoware::universe_utils::createMarkerColor(1.0, 0.0, 1.0, 0.999); // intersection turn signal info { const auto & turn_signal_info = debug_data.intersection_turn_signal_info; - auto desired_start_marker = tier4_autoware_utils::createDefaultMarker( + auto desired_start_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_desired_start", 0L, Marker::SPHERE, scale, desired_section_color); - auto desired_end_marker = tier4_autoware_utils::createDefaultMarker( + auto desired_end_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_desired_end", 0L, Marker::SPHERE, scale, desired_section_color); desired_start_marker.pose = turn_signal_info.desired_start_point; desired_end_marker.pose = turn_signal_info.desired_end_point; - auto required_start_marker = tier4_autoware_utils::createDefaultMarker( + auto required_start_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_required_start", 0L, Marker::SPHERE, scale, required_section_color); - auto required_end_marker = tier4_autoware_utils::createDefaultMarker( + auto required_end_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "intersection_turn_signal_required_end", 0L, Marker::SPHERE, scale, required_section_color); required_start_marker.pose = turn_signal_info.required_start_point; @@ -571,19 +604,19 @@ void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDeb { const auto & turn_signal_info = debug_data.behavior_turn_signal_info; - auto desired_start_marker = tier4_autoware_utils::createDefaultMarker( + auto desired_start_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_desired_start", 0L, Marker::CUBE, scale, desired_section_color); - auto desired_end_marker = tier4_autoware_utils::createDefaultMarker( + auto desired_end_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_desired_end", 0L, Marker::CUBE, scale, desired_section_color); desired_start_marker.pose = turn_signal_info.desired_start_point; desired_end_marker.pose = turn_signal_info.desired_end_point; - auto required_start_marker = tier4_autoware_utils::createDefaultMarker( + auto required_start_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_required_start", 0L, Marker::CUBE, scale, required_section_color); - auto required_end_marker = tier4_autoware_utils::createDefaultMarker( + auto required_end_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "behavior_turn_signal_required_end", 0L, Marker::CUBE, scale, required_section_color); required_start_marker.pose = turn_signal_info.required_start_point; @@ -609,18 +642,18 @@ void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path) constexpr double color_a = 0.999; const auto current_time = path.header.stamp; - auto left_marker = tier4_autoware_utils::createDefaultMarker( + auto left_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "left_bound", 0L, Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware::universe_utils::createMarkerScale(scale_x, scale_y, scale_z), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto lb : path.left_bound) { left_marker.points.push_back(lb); } - auto right_marker = tier4_autoware_utils::createDefaultMarker( + auto right_marker = autoware::universe_utils::createDefaultMarker( "map", current_time, "right_bound", 0L, Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + autoware::universe_utils::createMarkerScale(scale_x, scale_y, scale_z), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); for (const auto rb : path.right_bound) { right_marker.points.push_back(rb); } @@ -711,8 +744,8 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = - motion_utils::convertToPath(*path_candidate_ptr); + output = autoware::motion_utils::convertToPath( + *path_candidate_ptr); // header is replaced by the input one, so it is substituted again output.header = planner_data->route_handler->getRouteHeader(); output.header.stamp = this->now(); @@ -765,35 +798,8 @@ bool BehaviorPathPlannerNode::keepInputPoints( return false; } -void BehaviorPathPlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) -{ - const std::lock_guard lock(mutex_pd_); - planner_data_->self_odometry = msg; -} -void BehaviorPathPlannerNode::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg) -{ - const std::lock_guard lock(mutex_pd_); - planner_data_->self_acceleration = msg; -} -void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg) -{ - const std::lock_guard lock(mutex_pd_); - planner_data_->dynamic_object = msg; -} -void BehaviorPathPlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg) -{ - const std::lock_guard lock(mutex_pd_); - planner_data_->occupancy_grid = msg; -} -void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg) -{ - const std::lock_guard lock(mutex_pd_); - planner_data_->costmap = msg; -} void BehaviorPathPlannerNode::onTrafficSignals(const TrafficLightGroupArray::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); - planner_data_->traffic_light_id_map.clear(); for (const auto & signal : msg->traffic_light_groups) { TrafficSignalStamped traffic_signal; @@ -802,45 +808,9 @@ void BehaviorPathPlannerNode::onTrafficSignals(const TrafficLightGroupArray::Con planner_data_->traffic_light_id_map[signal.traffic_light_group_id] = traffic_signal; } } -void BehaviorPathPlannerNode::onMap(const LaneletMapBin::ConstSharedPtr msg) -{ - const std::lock_guard lock(mutex_map_); - map_ptr_ = msg; - has_received_map_ = true; -} -void BehaviorPathPlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) -{ - if (msg->segments.empty()) { - RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); - return; - } - const std::lock_guard lock(mutex_route_); - route_ptr_ = msg; - has_received_route_ = true; -} -void BehaviorPathPlannerNode::onOperationMode(const OperationModeState::ConstSharedPtr msg) -{ - const std::lock_guard lock(mutex_pd_); - planner_data_->operation_mode = msg; -} - -void BehaviorPathPlannerNode::on_external_velocity_limiter( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) -{ - // Note: Using this parameter during path planning might cause unexpected deceleration or jerk. - // Therefore, do not use it for anything other than safety checks. - if (!msg) { - return; - } - - const std::lock_guard lock(mutex_pd_); - planner_data_->external_limit_max_velocity = msg; -} void BehaviorPathPlannerNode::onLateralOffset(const LateralOffset::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); - if (!planner_data_->lateral_offset) { planner_data_->lateral_offset = msg; return; @@ -860,7 +830,7 @@ void BehaviorPathPlannerNode::onLateralOffset(const LateralOffset::ConstSharedPt SetParametersResult BehaviorPathPlannerNode::onSetParam( const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; rcl_interfaces::msg::SetParametersResult result; diff --git a/planning/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp similarity index 96% rename from planning/autoware_behavior_path_planner/src/planner_manager.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 3f440615bdc2c..e66eec2a1e0a4 100644 --- a/planning/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner/planner_manager.hpp" +#include "autoware/behavior_path_planner/planner_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "tier4_autoware_utils/ros/debug_publisher.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -113,8 +113,11 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da const bool is_any_module_running = is_any_approved_module_running || is_any_candidate_module_running_or_idle; + updateCurrentRouteLanelet(data); + const bool is_out_of_route = utils::isEgoOutOfRoute( - data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler); + data->self_odometry->pose.pose, current_route_lanelet_.value(), data->prev_modified_goal, + data->route_handler); if (!is_any_module_running && is_out_of_route) { BehaviorModuleOutput output = utils::createGoalAroundPath(data); @@ -216,7 +219,7 @@ void PlannerManager::generateCombinedDrivableArea( const auto & di = output.drivable_area_info; constexpr double epsilon = 1e-3; - const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path.points); + const auto is_driving_forward_opt = autoware::motion_utils::isDrivingForward(output.path.points); const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (epsilon < std::abs(di.drivable_margin)) { @@ -416,7 +419,7 @@ BehaviorModuleOutput PlannerManager::runKeepLastModules( return output; } -BehaviorModuleOutput PlannerManager::getReferencePath(const std::shared_ptr & data) +void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr & data) { const auto & route_handler = data->route_handler; const auto & pose = data->self_odometry->pose.pose; @@ -426,10 +429,18 @@ BehaviorModuleOutput PlannerManager::getReferencePath(const std::shared_ptrgetClosestRouteLaneletFromLanelet( + pose, current_route_lanelet_.value(), &closest_lane, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold)) { + current_route_lanelet_ = closest_lane; + return; + } + const auto lanelet_sequence = route_handler->getLaneletSequence( current_route_lanelet_.value(), pose, backward_length, p.forward_path_length); - lanelet::ConstLanelet closest_lane{}; const auto could_calculate_closest_lanelet = lanelet::utils::query::getClosestLaneletWithConstrains( lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, @@ -440,20 +451,25 @@ BehaviorModuleOutput PlannerManager::getReferencePath(const std::shared_ptr & data) const +{ const auto reference_path = utils::getReferencePath(*current_route_lanelet_, data); publishDebugRootReferencePath(reference_path); return reference_path; } -void PlannerManager::publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path) +void PlannerManager::publishDebugRootReferencePath( + const BehaviorModuleOutput & reference_path) const { using visualization_msgs::msg::Marker; MarkerArray array; - Marker m = tier4_autoware_utils::createDefaultMarker( + Marker m = autoware::universe_utils::createDefaultMarker( "map", clock_.now(), "root_reference_path", 0UL, Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0)); + autoware::universe_utils::createMarkerScale(1.0, 1.0, 1.0), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0)); for (const auto & p : reference_path.path.points) m.points.push_back(p.point.pose.position); array.markers.push_back(m); m.points.clear(); diff --git a/planning/autoware_behavior_path_planner/test/input.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.cpp similarity index 100% rename from planning/autoware_behavior_path_planner/test/input.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner/test/input.cpp diff --git a/planning/autoware_behavior_path_planner/test/input.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/input.hpp similarity index 100% rename from planning/autoware_behavior_path_planner/test/input.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner/test/input.hpp diff --git a/planning/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp similarity index 98% rename from planning/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp index e70187754599a..b7d992d9254dc 100644 --- a/planning/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" #include #include diff --git a/planning/autoware_behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp similarity index 93% rename from planning/autoware_behavior_path_planner/test/test_utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp index cda78df2e8273..da8657c82c58e 100644 --- a/planning/autoware_behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp @@ -11,14 +11,14 @@ // 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 "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" #include "input.hpp" #include "lanelet2_core/Attribute.h" #include "lanelet2_core/geometry/LineString.h" #include "lanelet2_core/geometry/Point.h" #include "lanelet2_core/primitives/Lanelet.h" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -34,8 +34,9 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnStraightLin autoware::behavior_path_planner::generateStraightSamplePathWithLaneId(0.0f, 1.0f, 10u); Pose vehicle_pose = autoware::behavior_path_planner::generateEgoSamplePose(10.7f, -1.7f, 0.0); - const size_t vehicle_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, vehicle_pose, 3.0, 1.0); + const size_t vehicle_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, vehicle_pose, 3.0, 1.0); const auto vehicle_pose_frenet = autoware::behavior_path_planner::utils::convertToFrenetPoint( path.points, vehicle_pose.position, vehicle_seg_idx); @@ -49,8 +50,9 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnDiagonalLin autoware::behavior_path_planner::generateDiagonalSamplePathWithLaneId(0.0f, 1.0f, 10u); Pose vehicle_pose = autoware::behavior_path_planner::generateEgoSamplePose(0.1f, 0.1f, 0.0); - const size_t vehicle_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, vehicle_pose, 3.0, 1.0); + const size_t vehicle_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, vehicle_pose, 3.0, 1.0); const auto vehicle_pose_frenet = autoware::behavior_path_planner::utils::convertToFrenetPoint( path.points, vehicle_pose.position, vehicle_seg_idx); diff --git a/planning/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_path_planner_common/CMakeLists.txt rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt diff --git a/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md similarity index 100% rename from planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md diff --git a/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md similarity index 100% rename from planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md diff --git a/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md similarity index 100% rename from planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md diff --git a/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md similarity index 100% rename from planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/path_shifter/path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/path_shifter/path_shifter.png similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/path_shifter/path_shifter.png rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/path_shifter/path_shifter.png diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp similarity index 93% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/data_manager.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp index 214813e68fb72..0ec7841f77cfe 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ -#include "autoware_behavior_path_planner_common/parameters.hpp" -#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/behavior_path_planner_common/parameters.hpp" +#include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include +#include #include #include #include @@ -102,7 +102,7 @@ struct DrivableAreaInfo struct Obstacle { geometry_msgs::msg::Pose pose; - tier4_autoware_utils::Polygon2d poly; + autoware::universe_utils::Polygon2d poly; bool is_left{true}; }; std::vector drivable_lanes{}; @@ -254,7 +254,7 @@ struct PlannerData template size_t findEgoIndex(const std::vector & points) const { - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( points, self_odometry->pose.pose, parameters.ego_nearest_dist_threshold, parameters.ego_nearest_yaw_threshold); } @@ -262,7 +262,7 @@ struct PlannerData template size_t findEgoSegmentIndex(const std::vector & points) const { - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, self_odometry->pose.pose, parameters.ego_nearest_dist_threshold, parameters.ego_nearest_yaw_threshold); } @@ -270,4 +270,4 @@ struct PlannerData } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp similarity index 92% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index a9af21a34cab3..d11c3181526d2 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -12,27 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ - -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" - -#include -#include -#include -#include -#include +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ + +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include -#include -#include #include -#include -#include -#include #include #include @@ -59,11 +59,11 @@ namespace autoware::behavior_path_planner using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::rtc_interface::RTCInterface; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::generateUUID; using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::SteeringFactor; using steering_factor_interface::SteeringFactorInterface; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; @@ -146,7 +146,7 @@ class SceneModuleInterface updateData(); const auto output = isWaitingApproval() ? planWaitingApproval() : plan(); try { - motion_utils::validateNonEmpty(output.path.points); + autoware::motion_utils::validateNonEmpty(output.path.points); } catch (const std::exception & ex) { throw std::invalid_argument("[" + name_ + "]" + ex.what()); } @@ -566,8 +566,8 @@ class SceneModuleInterface StopFactor stop_factor; stop_factor.stop_pose = stop_pose_.value(); - stop_factor.dist_to_stop_pose = - motion_utils::calcSignedArcLength(path.points, getEgoPosition(), stop_pose_.value().position); + stop_factor.dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( + path.points, getEgoPosition(), stop_pose_.value().position); stop_reason_.stop_factors.push_back(stop_factor); } @@ -645,4 +645,4 @@ class SceneModuleInterface } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp similarity index 93% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index bc4505710a6cc..5b9c0389e29c5 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -13,11 +13,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" #include #include @@ -36,10 +36,10 @@ namespace autoware::behavior_path_planner { -using motion_utils::createDeadLineVirtualWallMarker; -using motion_utils::createSlowDownVirtualWallMarker; -using motion_utils::createStopVirtualWallMarker; -using tier4_autoware_utils::toHexString; +using autoware::motion_utils::createDeadLineVirtualWallMarker; +using autoware::motion_utils::createSlowDownVirtualWallMarker; +using autoware::motion_utils::createStopVirtualWallMarker; +using autoware::universe_utils::toHexString; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; using SceneModuleObserver = std::weak_ptr; @@ -111,7 +111,7 @@ class SceneModuleManagerInterface void publishVirtualWall() const { - using tier4_autoware_utils::appendMarkerArray; + using autoware::universe_utils::appendMarkerArray; MarkerArray markers{}; @@ -155,7 +155,7 @@ class SceneModuleManagerInterface void publishMarker() const { - using tier4_autoware_utils::appendMarkerArray; + using autoware::universe_utils::appendMarkerArray; MarkerArray info_markers{}; MarkerArray debug_markers{}; @@ -271,7 +271,7 @@ class SceneModuleManagerInterface protected: void initInterface(rclcpp::Node * node, const std::vector & rtc_types) { - using tier4_autoware_utils::getOrDeclareParameter; + using autoware::universe_utils::getOrDeclareParameter; // init manager configuration { @@ -348,4 +348,4 @@ class SceneModuleManagerInterface } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp similarity index 84% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp index 384c74243fcb2..e2d5d276070ee 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" @@ -44,4 +44,4 @@ class SceneModuleVisitor }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/steering_factor_interface.hpp similarity index 85% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/steering_factor_interface.hpp index 31f169471cc9e..385eb2a8ad0ae 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/steering_factor_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ #include @@ -52,4 +52,4 @@ class SteeringFactorInterface } // namespace steering_factor_interface -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/colors.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp similarity index 57% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/colors.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp index ebc48f9c16cc8..f2318e9d5c1ad 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/colors.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/colors.hpp @@ -11,10 +11,10 @@ // 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 AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ -#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" #include "std_msgs/msg/detail/color_rgba__struct.hpp" @@ -26,62 +26,62 @@ using std_msgs::msg::ColorRGBA; inline ColorRGBA red(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 0., 0., a); + return autoware::universe_utils::createMarkerColor(1., 0., 0., a); } inline ColorRGBA green(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(0., 1., 0., a); + return autoware::universe_utils::createMarkerColor(0., 1., 0., a); } inline ColorRGBA blue(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(0., 0., 1., a); + return autoware::universe_utils::createMarkerColor(0., 0., 1., a); } inline ColorRGBA yellow(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 1., 0., a); + return autoware::universe_utils::createMarkerColor(1., 1., 0., a); } inline ColorRGBA aqua(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(0., 1., 1., a); + return autoware::universe_utils::createMarkerColor(0., 1., 1., a); } inline ColorRGBA magenta(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 0., 1., a); + return autoware::universe_utils::createMarkerColor(1., 0., 1., a); } inline ColorRGBA medium_orchid(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(0.729, 0.333, 0.827, a); + return autoware::universe_utils::createMarkerColor(0.729, 0.333, 0.827, a); } inline ColorRGBA light_pink(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 0.713, 0.756, a); + return autoware::universe_utils::createMarkerColor(1., 0.713, 0.756, a); } inline ColorRGBA light_yellow(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 1., 0.878, a); + return autoware::universe_utils::createMarkerColor(1., 1., 0.878, a); } inline ColorRGBA light_steel_blue(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(0.690, 0.768, 0.870, a); + return autoware::universe_utils::createMarkerColor(0.690, 0.768, 0.870, a); } inline ColorRGBA white(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(1., 1., 1., a); + return autoware::universe_utils::createMarkerColor(1., 1., 1., a); } inline ColorRGBA grey(float a = 0.99) { - return tier4_autoware_utils::createMarkerColor(.5, .5, .5, a); + return autoware::universe_utils::createMarkerColor(.5, .5, .5, a); } inline std::vector colors_list(float a = 0.99) @@ -91,4 +91,4 @@ inline std::vector colors_list(float a = 0.99) } } // namespace marker_utils::colors -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp similarity index 91% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp index 871820489dc9c..819800281fcc5 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp @@ -11,12 +11,12 @@ // 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 AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include @@ -40,6 +40,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::CollisionChec using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Point; @@ -48,7 +49,6 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::Polygon2d; using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -122,4 +122,4 @@ MarkerArray showFilteredObjects( const ColorRGBA & color, int32_t id); } // namespace marker_utils -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp similarity index 92% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp index 2f14e227d05ef..bbc249f5a0daf 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ #include @@ -79,4 +79,4 @@ struct BehaviorPathPlannerParameters double base_link2rear; }; -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp similarity index 94% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp index 65ead4019eb1e..94ed8af3c82b4 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include -#include -#include +#include +#include +#include +#include +#include #include -#include -#include #include #include @@ -239,9 +239,9 @@ class TurnSignalDecider const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) const { + using autoware::universe_utils::pose2transform; + using autoware::universe_utils::transformVector; using boost::geometry::intersects; - using tier4_autoware_utils::pose2transform; - using tier4_autoware_utils::transformVector; const auto footprint = vehicle_info.createFootprint(); const auto start_itr = std::next(path.path.points.begin(), shift_line.start_idx); @@ -298,4 +298,4 @@ class TurnSignalDecider }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp similarity index 91% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp index fe7fbef64489f..2f758d1c1bd71 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" -#include +#include #include @@ -134,5 +134,5 @@ std::vector calculate_smoothed_curvatures( } // namespace autoware::behavior_path_planner::drivable_area_expansion // clang-format off -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT // clang-format on diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp similarity index 80% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index f4b7e00ea45ad..c0f953578a213 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" -#include +#include #include #include @@ -46,4 +46,4 @@ MultiPolygon2d create_object_footprints( const autoware_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); } // namespace autoware::behavior_path_planner::drivable_area_expansion -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp similarity index 80% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp index bb66528ba0d3a..0a752a605ebf3 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include @@ -41,4 +41,4 @@ SegmentRtree extract_uncrossable_segments( bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types); } // namespace autoware::behavior_path_planner::drivable_area_expansion -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp similarity index 95% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp index 0355a14ddafde..651b7a0643bca 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ #include #include @@ -126,4 +126,4 @@ struct DrivableAreaExpansionParameters }; } // namespace autoware::behavior_path_planner::drivable_area_expansion -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp similarity index 95% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp index 80b6d178b2627..9296fd22d374b 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include @@ -181,5 +181,5 @@ inline LineString2d sub_linestring( } // namespace autoware::behavior_path_planner::drivable_area_expansion // clang-format off -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ // NOLINT +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ // NOLINT // clang-format on diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp similarity index 91% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index 261c5c986a116..f393ec18f8a32 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT -#include +#include #include #include @@ -103,5 +103,5 @@ std::vector combineDrivableLanes( } // namespace autoware::behavior_path_planner::utils // clang-format off -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT // clang-format on diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp similarity index 74% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index 7021d31e58aac..afe88ac04f302 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include #include @@ -36,13 +36,13 @@ using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::MultiLineString2d; -using tier4_autoware_utils::MultiPoint2d; -using tier4_autoware_utils::MultiPolygon2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; -using tier4_autoware_utils::Segment2d; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::MultiLineString2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPolygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Segment2d; using SegmentRtree = boost::geometry::index::rtree>; @@ -71,4 +71,4 @@ struct Expansion std::vector min_lane_widths; }; } // namespace autoware::behavior_path_planner::drivable_area_expansion -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp similarity index 90% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index 91ede1013198d..fa04ff04c8e44 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ -#include +#include #include #include @@ -145,4 +145,4 @@ class OccupancyGridBasedCollisionDetector } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp similarity index 80% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index c2b306a4e91db..32e6f71f7157a 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -42,4 +42,4 @@ struct StartGoalPlannerData } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp similarity index 90% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 739e61481ef2e..518d1108f0ee1 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/parameters.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/parameters.hpp" -#include +#include #include #include @@ -60,7 +60,7 @@ struct ParallelParkingParameters // pull_out double pull_out_velocity{0.0}; double pull_out_lane_departure_margin{0.0}; - double pull_out_path_interval{0.0}; + double pull_out_arc_path_interval{0.0}; double pull_out_max_steer_angle{0.0}; }; @@ -151,5 +151,5 @@ class GeometricParallelParking } // namespace autoware::behavior_path_planner // clang-format off -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT // clang-format on diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp similarity index 88% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp index 781270666f85d..e5066932df88e 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -83,4 +83,4 @@ std::pair calcEndArcLength( } // namespace autoware::behavior_path_planner::utils::parking_departure -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp similarity index 95% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 4cd9361e10576..4fd6fce325f49 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ // NOLINT +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ // NOLINT -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -70,7 +70,7 @@ bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::Cons bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); bool isPolygonOverlapLanelet( - const PredictedObject & object, const tier4_autoware_utils::Polygon2d & lanelet_polygon); + const PredictedObject & object, const autoware::universe_utils::Polygon2d & lanelet_polygon); bool isPolygonOverlapLanelet( const PredictedObject & object, const lanelet::BasicPolygon2d & lanelet_polygon); @@ -320,4 +320,6 @@ void filterObjects(std::vector & objects, Func filter) } } // namespace autoware::behavior_path_planner::utils::path_safety_checker -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +// clang-format off +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ // NOLINT +// clang-format on diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp similarity index 95% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index e0e01a3a9fbd6..313f9df471938 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT -#include +#include #include #include @@ -31,10 +31,10 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker { +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_autoware_utils::Polygon2d; struct PoseWithVelocity { @@ -239,5 +239,5 @@ using CollisionCheckDebugMap = } // namespace autoware::behavior_path_planner::utils::path_safety_checker // clang-format off -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT // clang-format on diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp similarity index 91% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 19eebff1b37ed..3f1094dfd70b6 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include +#include +#include #include #include @@ -32,15 +32,15 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker { using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using autoware::universe_utils::calcYawDeviation; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_autoware_utils::calcYawDeviation; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; bool isTargetObjectOncoming( const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose, @@ -157,4 +157,4 @@ void updateCollisionCheckDebugMap( CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); } // namespace autoware::behavior_path_planner::utils::path_safety_checker -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp similarity index 95% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 33265d8cef832..8164ef659b7b1 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#include #include #include -#include #include #include @@ -29,9 +29,9 @@ #include namespace autoware::behavior_path_planner { +using autoware::universe_utils::generateUUID; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; @@ -248,4 +248,4 @@ class PathShifter } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp similarity index 90% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp index c1983f92f54dd..3b06c7e03cae1 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -113,4 +113,4 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr +#include #include #include #include @@ -109,4 +109,4 @@ bool isTrafficSignalStop( const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data); } // namespace autoware::behavior_path_planner::utils::traffic_light -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp similarity index 90% rename from planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index e4b8020404b50..5c1eee92c2a5d 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include -#include +#include +#include #include #include @@ -47,11 +47,11 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using autoware::route_handler::RouteHandler; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Vector3; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::Polygon2d; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; @@ -93,9 +93,11 @@ FrenetPoint convertToFrenetPoint( FrenetPoint frenet_point; const double longitudinal_length = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, search_point_geom); - frenet_point.length = motion_utils::calcSignedArcLength(points, 0, seg_idx) + longitudinal_length; - frenet_point.distance = motion_utils::calcLateralOffset(points, search_point_geom, seg_idx); + autoware::motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, search_point_geom); + frenet_point.length = + autoware::motion_utils::calcSignedArcLength(points, 0, seg_idx) + longitudinal_length; + frenet_point.distance = + autoware::motion_utils::calcLateralOffset(points, search_point_geom, seg_idx); return frenet_point; } @@ -145,7 +147,7 @@ bool checkCollisionWithExtraStoppingMargin( * @return Has collision or not */ bool checkCollisionBetweenPathFootprintsAndObjects( - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const PathWithLaneId & ego_path, + const autoware::universe_utils::LinearRing2d & vehicle_footprint, const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double margin); /** @@ -153,7 +155,7 @@ bool checkCollisionBetweenPathFootprintsAndObjects( * @return Has collision or not */ bool checkCollisionBetweenFootprintAndObjects( - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose, + const autoware::universe_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose, const PredictedObjects & dynamic_objects, const double margin); /** @@ -231,7 +233,8 @@ bool isInLaneletWithYawThreshold( const double radius = 0.0); bool isEgoOutOfRoute( - const Pose & self_pose, const std::optional & modified_goal, + const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane, + const std::optional & modified_goal, const std::shared_ptr & route_handler); bool isEgoWithinOriginalLane( @@ -341,13 +344,13 @@ size_t findNearestSegmentIndex( const double yaw_threshold) { const auto nearest_idx = - motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); + autoware::motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); if (nearest_idx) { return nearest_idx.value(); } - return motion_utils::findNearestSegmentIndex(points, pose.position); + return autoware::motion_utils::findNearestSegmentIndex(points, pose.position); } } // namespace autoware::behavior_path_planner::utils -#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml similarity index 97% rename from planning/autoware_behavior_path_planner_common/package.xml rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index 6b20027610951..46ee55c07fe09 100644 --- a/planning/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -45,21 +45,21 @@ autoware_adapi_v1_msgs autoware_freespace_planning_algorithms autoware_lane_departure_checker + autoware_motion_utils autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_planning_msgs autoware_route_handler autoware_rtc_interface + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation lanelet2_extension magic_enum - motion_utils object_recognition_utils rclcpp tf2 - tier4_autoware_utils tier4_planning_msgs traffic_light_utils visualization_msgs diff --git a/planning/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp similarity index 86% rename from planning/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp index f83c73b6a0281..454d8753b0e62 100644 --- a/planning/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" namespace autoware::behavior_path_planner { diff --git a/planning/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp similarity index 97% rename from planning/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp index a231f00d75fdc..daebfa31be311 100644 --- a/planning/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp" +#include "autoware/behavior_path_planner_common/interface/steering_factor_interface.hpp" namespace steering_factor_interface { diff --git a/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp similarity index 93% rename from planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index ce2d412e590b2..8c9b4ef35d5af 100644 --- a/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware_behavior_path_planner_common/marker_utils/colors.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/colors.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -31,13 +31,13 @@ namespace marker_utils { using autoware::behavior_path_planner::utils::calcPathArcLengthArray; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; void addFootprintMarker( @@ -49,13 +49,13 @@ void addFootprintMarker( const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); marker.points.push_back(marker.points.front()); } @@ -214,7 +214,7 @@ MarkerArray createShiftLengthMarkerArray( Marker marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.9)); - marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); marker.points.reserve(shift_distance.size()); for (size_t i = 0; i < shift_distance.size(); ++i) { @@ -271,7 +271,7 @@ MarkerArray createLaneletsAreaMarkerArray( "map", current_time, ns, static_cast(lanelet.id()), Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); - marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); if (!lanelet.polygon3d().empty()) { marker.points.reserve(lanelet.polygon3d().size() + 1); @@ -299,7 +299,7 @@ MarkerArray createPolygonMarkerArray( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, static_cast(lane_id), Marker::LINE_STRIP, createMarkerScale(w, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); - marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); if (!polygon.points.empty()) { marker.points.reserve(polygon.points.size() + 1); diff --git a/planning/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp similarity index 90% rename from planning/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index f3a6e91818617..73f81ce59844c 100644 --- a/planning/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include -#include #include #include @@ -33,15 +33,16 @@ namespace autoware::behavior_path_planner { -using motion_utils::calcSignedArcLength; +using autoware::motion_utils::calcSignedArcLength; double calc_distance( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const Pose & input_point, const double nearest_dist_threshold, const double nearest_yaw_threshold) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return motion_utils::calcSignedArcLength( + const size_t nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); } @@ -82,8 +83,9 @@ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( } // Closest ego segment - const size_t ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - extended_path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const size_t ego_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + extended_path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); // Get closest intersection turn signal if exists const auto intersection_turn_signal_info = getIntersectionTurnSignalInfo( @@ -230,28 +232,26 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( } const size_t front_nearest_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, lane_front_pose, nearest_dist_threshold, nearest_yaw_threshold); const size_t back_nearest_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, lane_back_pose, nearest_dist_threshold, nearest_yaw_threshold); // Distance from ego vehicle front pose to front point of the lane - const double dist_to_front_point = motion_utils::calcSignedArcLength( + const double dist_to_front_point = autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, lane_front_pose.position, front_nearest_seg_idx) - base_link2front_; // Distance from ego vehicle base link to the terminal point of the lane - const double dist_to_back_point = motion_utils::calcSignedArcLength( + const double dist_to_back_point = autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, lane_back_pose.position, back_nearest_seg_idx); if (dist_to_back_point < 0.0) { // Vehicle is already passed this lane - if (desired_start_point_map_.find(lane_id) != desired_start_point_map_.end()) { - desired_start_point_map_.erase(lane_id); - } + desired_start_point_map_.erase(lane_id); continue; } else if (search_distance <= dist_to_front_point) { continue; @@ -280,9 +280,10 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( const auto & turn_signal_info = signal_queue.front(); const auto & required_end_point = turn_signal_info.required_end_point; - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, required_end_point, nearest_dist_threshold, nearest_yaw_threshold); - const double dist_to_end_point = motion_utils::calcSignedArcLength( + const size_t nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, required_end_point, nearest_dist_threshold, nearest_yaw_threshold); + const double dist_to_end_point = autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, required_end_point.position, nearest_seg_idx); @@ -406,9 +407,10 @@ TurnSignalInfo TurnSignalDecider::overwrite_turn_signal( } const auto get_distance = [&](const Pose & input_point) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return motion_utils::calcSignedArcLength( + const size_t nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx) - base_link2front_; @@ -432,9 +434,10 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( const double nearest_dist_threshold, const double nearest_yaw_threshold) { const auto get_distance = [&](const Pose & input_point) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return motion_utils::calcSignedArcLength( + const size_t nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); }; @@ -555,9 +558,9 @@ geometry_msgs::msg::Pose TurnSignalDecider::get_required_end_point( for (size_t i = 0; i < centerline.size(); ++i) { converted_centerline.at(i).position = lanelet::utils::conversion::toGeomMsgPt(centerline[i]); } - motion_utils::insertOrientation(converted_centerline, true); + autoware::motion_utils::insertOrientation(converted_centerline, true); - const double length = motion_utils::calcArcLength(converted_centerline); + const double length = autoware::motion_utils::calcArcLength(converted_centerline); // Create resampling intervals const double resampling_interval = 1.0; @@ -567,20 +570,21 @@ geometry_msgs::msg::Pose TurnSignalDecider::get_required_end_point( } // Insert terminal point - if (length - resampling_arclength.back() < motion_utils::overlap_threshold) { + if (length - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { resampling_arclength.back() = length; } else { resampling_arclength.push_back(length); } const auto resampled_centerline = - motion_utils::resamplePoseVector(converted_centerline, resampling_arclength); + autoware::motion_utils::resamplePoseVector(converted_centerline, resampling_arclength); const double terminal_yaw = tf2::getYaw(resampled_centerline.back().orientation); for (size_t i = 0; i < resampled_centerline.size(); ++i) { const double yaw = tf2::getYaw(resampled_centerline.at(i).orientation); - const double yaw_diff = tier4_autoware_utils::normalizeRadian(yaw - terminal_yaw); - if (std::fabs(yaw_diff) < tier4_autoware_utils::deg2rad(intersection_angle_threshold_deg_)) { + const double yaw_diff = autoware::universe_utils::normalizeRadian(yaw - terminal_yaw); + if ( + std::fabs(yaw_diff) < autoware::universe_utils::deg2rad(intersection_angle_threshold_deg_)) { return resampled_centerline.at(i); } } @@ -594,9 +598,10 @@ void TurnSignalDecider::set_intersection_info( const double nearest_yaw_threshold) { const auto get_distance = [&](const Pose & input_point) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return motion_utils::calcSignedArcLength( + const size_t nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return autoware::motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); }; @@ -636,9 +641,9 @@ void TurnSignalDecider::initialize_intersection_info() geometry_msgs::msg::Quaternion TurnSignalDecider::calc_orientation( const Point & src_point, const Point & dst_point) { - const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); - return tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + const double pitch = autoware::universe_utils::calcElevationAngle(src_point, dst_point); + const double yaw = autoware::universe_utils::calcAzimuthAngle(src_point, dst_point); + return autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); } std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( @@ -649,7 +654,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check, const bool is_pull_out) const { - using tier4_autoware_utils::getPose; + using autoware::universe_utils::getPose; const auto & p = parameters; const auto & rh = route_handler; diff --git a/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp similarity index 89% rename from planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index dd880ecb009c6..6e028cec2ed1b 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include +#include +#include +#include #include -#include -#include -#include -#include #include @@ -48,10 +48,11 @@ void reuse_previous_poses( { std::vector cropped_poses; std::vector cropped_curvatures; - const auto ego_is_behind = prev_poses.size() > 1 && motion_utils::calcLongitudinalOffsetToSegment( - prev_poses, 0, ego_point) < 0.0; - const auto ego_is_far = !prev_poses.empty() && - tier4_autoware_utils::calcDistance2d(ego_point, prev_poses.front()) < 0.0; + const auto ego_is_behind = + prev_poses.size() > 1 && + autoware::motion_utils::calcLongitudinalOffsetToSegment(prev_poses, 0, ego_point) < 0.0; + const auto ego_is_far = !prev_poses.empty() && autoware::universe_utils::calcDistance2d( + ego_point, prev_poses.front()) < 0.0; // make sure the reused points are not behind the current original drivable area LineString2d left_bound; LineString2d right_bound; @@ -64,9 +65,9 @@ void reuse_previous_poses( if (!ego_is_behind && !ego_is_far && prev_poses.size() > 1 && !prev_poses_across_bounds) { const auto first_idx = - motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); - const auto deviation = - motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); + autoware::motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); + const auto deviation = autoware::motion_utils::calcLateralOffset( + prev_poses, path.points.front().point.pose.position); if (first_idx && deviation < params.max_reuse_deviation) { LineString2d path_ls; for (const auto & p : path.points) path_ls.push_back(convert_point(p.point.pose.position)); @@ -86,27 +87,29 @@ void reuse_previous_poses( } if (cropped_poses.empty()) { const auto resampled_path_points = - motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; + autoware::motion_utils::resamplePath(path, params.resample_interval, true, true, false) + .points; const auto cropped_path = params.max_path_arc_length <= 0.0 ? resampled_path_points - : motion_utils::cropForwardPoints( + : autoware::motion_utils::cropForwardPoints( resampled_path_points, resampled_path_points.front().point.pose.position, 0, params.max_path_arc_length); for (const auto & p : cropped_path) cropped_poses.push_back(p.point.pose); } else { - const auto initial_arc_length = motion_utils::calcArcLength(cropped_poses); - const auto max_path_arc_length = motion_utils::calcArcLength(path.points); - const auto first_arc_length = motion_utils::calcSignedArcLength( + const auto initial_arc_length = autoware::motion_utils::calcArcLength(cropped_poses); + const auto max_path_arc_length = autoware::motion_utils::calcArcLength(path.points); + const auto first_arc_length = autoware::motion_utils::calcSignedArcLength( path.points, path.points.front().point.pose.position, cropped_poses.back().position); for (auto arc_length = first_arc_length + params.resample_interval; (params.max_path_arc_length <= 0.0 || initial_arc_length + (arc_length - first_arc_length) <= params.max_path_arc_length) && arc_length <= max_path_arc_length; arc_length += params.resample_interval) - cropped_poses.push_back(motion_utils::calcInterpolatedPose(path.points, arc_length)); + cropped_poses.push_back( + autoware::motion_utils::calcInterpolatedPose(path.points, arc_length)); } - prev_poses = motion_utils::removeOverlapPoints(cropped_poses); + prev_poses = autoware::motion_utils::removeOverlapPoints(cropped_poses); prev_curvatures = cropped_curvatures; } @@ -161,7 +164,7 @@ void apply_arc_length_range_smoothing( auto arc_length = boost::geometry::distance( bound_projections[path_idx].point, convert_point(bound[bound_idx + 1])); const auto update_arc_length_and_bound_expansions = [&](auto idx) { - arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); + arc_length += autoware::universe_utils::calcDistance2d(bound[idx - 1], bound[idx]); bound_expansions[idx] = std::max(bound_expansions[idx], original_expansions[bound_idx]); }; for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { @@ -200,7 +203,7 @@ void apply_bound_change_rate_limit( if (distances.empty()) return; const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { if (exp[from] > exp[to]) { - const auto arc_length = tier4_autoware_utils::calcDistance2d(bound[from], bound[to]); + const auto arc_length = autoware::universe_utils::calcDistance2d(bound[from], bound[to]); const auto smoothed_dist = exp[from] - arc_length * max_rate; exp[to] = std::max(exp[to], smoothed_dist); } @@ -284,12 +287,12 @@ void expand_bound( for (auto idx = 1LU; idx < bound.size(); ++idx) { bool is_intersecting = false; for (auto succ_idx = idx + 1; succ_idx < bound.size(); ++succ_idx) { - const auto intersection = tier4_autoware_utils::intersect( + const auto intersection = autoware::universe_utils::intersect( bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]); if ( intersection && - tier4_autoware_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && - tier4_autoware_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { + autoware::universe_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && + autoware::universe_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { idx = succ_idx; is_intersecting = true; } @@ -302,7 +305,7 @@ void expand_bound( std::vector calculate_smoothed_curvatures( const std::vector & poses, const size_t smoothing_window_size) { - const auto curvatures = motion_utils::calcCurvature(poses); + const auto curvatures = autoware::motion_utils::calcCurvature(poses); std::vector smoothed_curvatures(curvatures.size()); for (auto i = 0UL; i < curvatures.size(); ++i) { auto sum = 0.0; @@ -363,7 +366,7 @@ void expand_drivable_area( { // skip if no bounds or not enough points to calculate path curvature if (path.points.size() < 3 || path.left_bound.empty() || path.right_bound.empty()) return; - tier4_autoware_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; stop_watch.tic("overall"); stop_watch.tic("preprocessing"); const auto & params = planner_data->drivable_area_expansion_parameters; diff --git a/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp similarity index 87% rename from planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index 5357df6ac1ed0..a25a33c1668fa 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include -#include +#include +#include #include @@ -37,7 +37,8 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 { const auto angle = tf2::getYaw(pose.orientation); return translate_polygon( - tier4_autoware_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); + autoware::universe_utils::rotatePolygon(base_footprint, angle), pose.position.x, + pose.position.y); } MultiPolygon2d create_object_footprints( diff --git a/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp similarity index 97% rename from planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp index 0dd4d901a5e72..280060334cfd4 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" #include diff --git a/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp similarity index 94% rename from planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 70304d4be7975..f304fff9ce450 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -11,17 +11,17 @@ // 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 "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include +#include +#include #include #include #include -#include -#include -#include #include @@ -55,8 +55,8 @@ std::vector removeSharpPoints(const std::vector & points) const auto product = std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2); - const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); + const auto dist_1to2 = autoware::universe_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = autoware::universe_utils::calcDistance3d(p3, p2); constexpr double epsilon = 1e-3; @@ -82,15 +82,15 @@ template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Point & target_point) { - using tier4_autoware_utils::calcAzimuthAngle; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::normalizeRadian; + using autoware::universe_utils::calcAzimuthAngle; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::normalizeRadian; std::optional closest_idx{std::nullopt}; double min_lateral_dist = std::numeric_limits::max(); for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const double lon_dist = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); + autoware::motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); const double lat_dist = [&]() { if (lon_dist < 0.0) { @@ -99,7 +99,7 @@ size_t findNearestSegmentIndexFromLateralDistance( if (segment_length < lon_dist) { return calcDistance2d(points.at(seg_idx + 1), target_point); } - return std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); + return std::abs(autoware::motion_utils::calcLateralOffset(points, target_point, seg_idx)); }(); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; @@ -111,7 +111,7 @@ size_t findNearestSegmentIndexFromLateralDistance( return *closest_idx; } - return motion_utils::findNearestSegmentIndex(points, target_point); + return autoware::motion_utils::findNearestSegmentIndex(points, target_point); } template @@ -119,9 +119,9 @@ size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Pose & target_point, const double yaw_threshold) { - using tier4_autoware_utils::calcAzimuthAngle; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::normalizeRadian; + using autoware::universe_utils::calcAzimuthAngle; + using autoware::universe_utils::calcDistance2d; + using autoware::universe_utils::normalizeRadian; std::optional closest_idx{std::nullopt}; double min_lateral_dist = std::numeric_limits::max(); @@ -132,8 +132,8 @@ size_t findNearestSegmentIndexFromLateralDistance( if (yaw_threshold < std::abs(yaw)) { continue; } - const double lon_dist = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point.position); + const double lon_dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + points, seg_idx, target_point.position); const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); const double lat_dist = [&]() { if (lon_dist < 0.0) { @@ -142,7 +142,8 @@ size_t findNearestSegmentIndexFromLateralDistance( if (segment_length < lon_dist) { return calcDistance2d(points.at(seg_idx + 1), target_point.position); } - return std::abs(motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); + return std::abs( + autoware::motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); }(); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; @@ -154,7 +155,7 @@ size_t findNearestSegmentIndexFromLateralDistance( return *closest_idx; } - return motion_utils::findNearestSegmentIndex(points, target_point.position); + return autoware::motion_utils::findNearestSegmentIndex(points, target_point.position); } bool checkHasSameLane( @@ -176,10 +177,10 @@ geometry_msgs::msg::Point calcLongitudinalOffsetStartPoint( const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) { - const double offset_length = - motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); - const auto offset_point = - motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); + const double offset_length = autoware::motion_utils::calcLongitudinalOffsetToSegment( + points, nearest_segment_idx, pose.position); + const auto offset_point = autoware::motion_utils::calcLongitudinalOffsetPoint( + points, nearest_segment_idx, offset_length + offset); return offset_point ? offset_point.value() : points.at(nearest_segment_idx); } @@ -188,10 +189,10 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) { - const double offset_length = - motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); - const auto offset_point = - motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); + const double offset_length = autoware::motion_utils::calcLongitudinalOffsetToSegment( + points, nearest_segment_idx, pose.position); + const auto offset_point = autoware::motion_utils::calcLongitudinalOffsetPoint( + points, nearest_segment_idx, offset_length + offset); return offset_point ? offset_point.value() : points.at(nearest_segment_idx + 1); } @@ -245,7 +246,7 @@ std::optional> intersectBound( static_cast(bound.size()) - 1, static_cast(std::max(seg_idx1, seg_idx2)) + 1 + 5)); for (int i = start_idx; i < static_cast(end_idx); ++i) { const auto intersect_point = - tier4_autoware_utils::intersect(p1, p2, bound.at(i), bound.at(i + 1)); + autoware::universe_utils::intersect(p1, p2, bound.at(i), bound.at(i + 1)); if (intersect_point) { std::pair result; result.first = static_cast(i); @@ -261,7 +262,7 @@ double calcSquaredDistanceFromPointToSegment( const geometry_msgs::msg::Point & segment_end_point, const geometry_msgs::msg::Point & target_point) { - using tier4_autoware_utils::calcSquaredDistance2d; + using autoware::universe_utils::calcSquaredDistance2d; const auto & a = segment_start_point; const auto & b = segment_end_point; @@ -293,10 +294,10 @@ PolygonPoint transformBoundFrenetCoordinate( const size_t min_dist_seg_idx = std::distance( dist_to_bound_segment_vec.begin(), std::min_element(dist_to_bound_segment_vec.begin(), dist_to_bound_segment_vec.end())); - const double lon_dist_to_segment = - motion_utils::calcLongitudinalOffsetToSegment(bound_points, min_dist_seg_idx, target_point); + const double lon_dist_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( + bound_points, min_dist_seg_idx, target_point); const double lat_dist_to_segment = - motion_utils::calcLateralOffset(bound_points, target_point, min_dist_seg_idx); + autoware::motion_utils::calcLateralOffset(bound_points, target_point, min_dist_seg_idx); return PolygonPoint{target_point, min_dist_seg_idx, lon_dist_to_segment, lat_dist_to_segment}; } @@ -342,7 +343,7 @@ std::vector generatePolygonInsideBounds( if (!intersection) { continue; } - const double lon_dist = motion_utils::calcLongitudinalOffsetToSegment( + const double lon_dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( bound, intersection->first, intersection->second); const auto intersect_point = PolygonPoint{intersection->second, intersection->first, lon_dist, 0.0}; @@ -356,7 +357,6 @@ std::vector generatePolygonInsideBounds( // Here is if (!is_prev_outside && is_curr_outside). inside_poly.push_back(prev_poly); inside_poly.push_back(intersect_point); - continue; } if (has_intersection) { return inside_poly; @@ -437,7 +437,7 @@ std::vector concatenateTwoPolygons( double min_dist_to_intersection = std::numeric_limits::max(); PolygonPoint closest_intersect_point; for (size_t i = 0; i < get_in_poly().size() - 1; ++i) { - const auto intersection = tier4_autoware_utils::intersect( + const auto intersection = autoware::universe_utils::intersect( get_out_poly().at(curr_idx).point, get_out_poly().at(next_idx).point, get_in_poly().at(i).point, get_in_poly().at(i + 1).point); if (!intersection) { @@ -454,7 +454,7 @@ std::vector concatenateTwoPolygons( const auto intersect_point = PolygonPoint{*intersection, 0, 0.0, 0.0}; const double dist_to_intersection = - tier4_autoware_utils::calcDistance2d(get_out_poly().at(curr_idx).point, *intersection); + autoware::universe_utils::calcDistance2d(get_out_poly().at(curr_idx).point, *intersection); if (dist_to_intersection < min_dist_to_intersection) { closest_idx = i; min_dist_to_intersection = dist_to_intersection; @@ -568,7 +568,7 @@ std::vector getPolygonPointsInsideBounds( // add start and end points projected to bound if necessary if (inside_polygon.at(start_idx).lat_dist_to_bound != 0.0) { // not on bound auto start_point = inside_polygon.at(start_idx); - const auto start_point_on_bound = motion_utils::calcLongitudinalOffsetPoint( + const auto start_point_on_bound = autoware::motion_utils::calcLongitudinalOffsetPoint( bound, start_point.bound_seg_idx, start_point.lon_dist_to_segment); if (start_point_on_bound) { start_point.point = start_point_on_bound.value(); @@ -577,7 +577,7 @@ std::vector getPolygonPointsInsideBounds( } if (inside_polygon.at(end_idx).lat_dist_to_bound != 0.0) { // not on bound auto end_point = inside_polygon.at(end_idx); - const auto end_point_on_bound = motion_utils::calcLongitudinalOffsetPoint( + const auto end_point_on_bound = autoware::motion_utils::calcLongitudinalOffsetPoint( bound, end_point.bound_seg_idx, end_point.lon_dist_to_segment); if (end_point_on_bound) { end_point.point = end_point_on_bound.value(); @@ -605,7 +605,7 @@ std::vector updateBoundary( const auto & start_poly = polygon.front(); const auto & end_poly = polygon.back(); - const double front_offset = motion_utils::calcLongitudinalOffsetToSegment( + const double front_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( updated_bound, start_poly.bound_seg_idx, start_poly.point); const size_t removed_start_idx = @@ -640,7 +640,7 @@ std::vector updateBoundary( namespace autoware::behavior_path_planner::utils { -using tier4_autoware_utils::Point2d; +using autoware::universe_utils::Point2d; std::optional getOverlappedLaneletId(const std::vector & lanes) { @@ -852,7 +852,7 @@ void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, const bool is_driving_forward) { - using tier4_autoware_utils::calcOffsetPose; + using autoware::universe_utils::calcOffsetPose; // remove path points which is close to the previous point PathWithLaneId resampled_path{}; @@ -864,7 +864,7 @@ void generateDrivableArea( const auto & prev_point = resampled_path.points.back().point.pose.position; const auto & curr_point = path.points.at(i).point.pose.position; const double signed_arc_length = - motion_utils::calcSignedArcLength(path.points, prev_point, curr_point); + autoware::motion_utils::calcSignedArcLength(path.points, prev_point, curr_point); if (signed_arc_length > resample_interval) { resampled_path.points.push_back(path.points.at(i)); } @@ -873,7 +873,7 @@ void generateDrivableArea( // add last point of path if enough far from the one of resampled path constexpr double th_last_point_distance = 0.3; if ( - tier4_autoware_utils::calcDistance2d( + autoware::universe_utils::calcDistance2d( resampled_path.points.back().point.pose.position, path.points.back().point.pose.position) > th_last_point_distance) { resampled_path.points.push_back(path.points.back()); @@ -946,7 +946,7 @@ void generateDrivableArea( p_line.push_back(p2); bool intersection_found = false; for (size_t j = i + 2; j < bound.size() - 1; j++) { - const double distance = tier4_autoware_utils::calcDistance2d(bound.at(i), bound.at(j)); + const double distance = autoware::universe_utils::calcDistance2d(bound.at(i), bound.at(j)); if (distance > intersection_check_distance) { break; } @@ -1040,12 +1040,12 @@ void extractObstaclesFromDrivableArea( const auto & obj_pos = obstacle.pose.position; // get edge points of the object - const size_t nearest_path_idx = - motion_utils::findNearestIndex(path.points, obj_pos); // to get z for object polygon + const size_t nearest_path_idx = autoware::motion_utils::findNearestIndex( + path.points, obj_pos); // to get z for object polygon std::vector edge_points; for (int i = 0; i < static_cast(obstacle.poly.outer().size()) - 1; ++i) { // NOTE: There is a duplicated points - edge_points.push_back(tier4_autoware_utils::createPoint( + edge_points.push_back(autoware::universe_utils::createPoint( obstacle.poly.outer().at(i).x(), obstacle.poly.outer().at(i).y(), path.points.at(nearest_path_idx).point.pose.position.z)); } @@ -1302,12 +1302,12 @@ std::pair, bool> getBoundWithFreeSpaceAreas( const std::vector & other_side_bound, const std::shared_ptr planner_data, const bool is_left) { + using autoware::universe_utils::getPose; + using autoware::universe_utils::pose2transform; + using autoware::universe_utils::transformVector; using lanelet::utils::to2D; using lanelet::utils::conversion::toGeomMsgPt; using lanelet::utils::conversion::toLaneletPoint; - using tier4_autoware_utils::getPose; - using tier4_autoware_utils::pose2transform; - using tier4_autoware_utils::transformVector; const auto & route_handler = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; @@ -1382,12 +1382,12 @@ std::pair, bool> getBoundWithFreeSpaceAreas( return bound; } - const auto p_offset = tier4_autoware_utils::calcOffsetPose( + const auto p_offset = autoware::universe_utils::calcOffsetPose( ego_pose, (trim_behind_bound ? -100.0 : 100.0), (is_left ? 0.1 : -0.1), 0.0); std::vector ret; for (size_t i = 1; i < bound.size(); ++i) { - const auto intersect = tier4_autoware_utils::intersect( + const auto intersect = autoware::universe_utils::intersect( ego_pose.position, p_offset.position, toGeomMsgPt(bound.at(i - 1)), toGeomMsgPt(bound.at(i))); @@ -1492,7 +1492,7 @@ std::vector postProcess( const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); if (bound.empty()) { bound.push_back(cp); - } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + } else if (autoware::universe_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { bound.push_back(cp); } } @@ -1557,7 +1557,7 @@ std::vector postProcess( const auto start_idx = [&]() { const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); - const auto cropped_path_points = motion_utils::cropPoints( + const auto cropped_path_points = autoware::motion_utils::cropPoints( path.points, current_pose.position, current_seg_idx, planner_data->parameters.forward_path_length, planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); @@ -1596,7 +1596,8 @@ std::vector postProcess( // Insert middle points for (size_t i = start_idx + 1; i <= goal_idx; ++i) { const auto & next_point = tmp_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(processed_bound.back(), next_point); + const double dist = + autoware::universe_utils::calcDistance2d(processed_bound.back(), next_point); if (dist > overlap_threshold) { processed_bound.push_back(next_point); } @@ -1604,7 +1605,8 @@ std::vector postProcess( // Insert a goal point if ( - tier4_autoware_utils::calcDistance2d(processed_bound.back(), goal_point) > overlap_threshold) { + autoware::universe_utils::calcDistance2d(processed_bound.back(), goal_point) > + overlap_threshold) { processed_bound.push_back(goal_point); } @@ -1618,7 +1620,7 @@ std::vector calcBound( const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward) { - using motion_utils::removeOverlapPoints; + using autoware::motion_utils::removeOverlapPoints; const auto & route_handler = planner_data->route_handler; diff --git a/planning/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp similarity index 95% rename from planning/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index d9cadb1678cb3..e4f1919b08dbc 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include -#include +#include +#include #include namespace autoware::behavior_path_planner { -using tier4_autoware_utils::createQuaternionFromYaw; -using tier4_autoware_utils::normalizeRadian; -using tier4_autoware_utils::transformPose; +using autoware::universe_utils::createQuaternionFromYaw; +using autoware::universe_utils::normalizeRadian; +using autoware::universe_utils::transformPose; int discretizeAngle(const double theta, const int theta_size) { diff --git a/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp similarity index 94% rename from planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index f4c5b1ce36872..113b87f4c0032 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" #include #include @@ -31,14 +31,14 @@ #include #include +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::inverseTransformPoint; +using autoware::universe_utils::normalizeRadian; +using autoware::universe_utils::transformPose; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::utils::getArcCoordinates; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::inverseTransformPoint; -using tier4_autoware_utils::normalizeRadian; -using tier4_autoware_utils::transformPose; using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner @@ -70,7 +70,7 @@ PathWithLaneId GeometricParallelParking::getFullPath() const } PathWithLaneId filtered_path = path; - filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); + filtered_path.points = autoware::motion_utils::removeOverlapPoints(filtered_path.points); return filtered_path; } @@ -132,10 +132,10 @@ std::vector GeometricParallelParking::generatePullOverPaths( // check the continuity of straight path and arc path const Pose & road_path_last_pose = straight_path.points.back().point.pose; const Pose & arc_path_first_pose = arc_paths.front().points.front().point.pose; - const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian( + const double yaw_diff = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(road_path_last_pose.orientation) - tf2::getYaw(arc_path_first_pose.orientation))); const double distance = calcDistance2d(road_path_last_pose, arc_path_first_pose); - if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { + if (yaw_diff > autoware::universe_utils::deg2rad(5.0) || distance > 0.1) { return std::vector{}; } @@ -244,7 +244,7 @@ bool GeometricParallelParking::planPullOut( auto arc_paths = planOneTrial( *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start, start_pose_offset, parameters_.pull_out_lane_departure_margin, - parameters_.pull_out_path_interval, lane_departure_checker); + parameters_.pull_out_arc_path_interval, lane_departure_checker); if (arc_paths.empty()) { // not found path continue; @@ -282,10 +282,10 @@ bool GeometricParallelParking::planPullOut( // check the continuity of straight path and arc path const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose; const Pose & arc_path_last_pose = arc_paths.back().points.back().point.pose; - const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian( + const double yaw_diff = std::abs(autoware::universe_utils::normalizeRadian( tf2::getYaw(road_path_first_pose.orientation) - tf2::getYaw(arc_path_last_pose.orientation))); const double distance = calcDistance2d(road_path_first_pose, arc_path_last_pose); - if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { + if (yaw_diff > autoware::universe_utils::deg2rad(5.0) || distance > 0.1) { continue; } @@ -299,7 +299,7 @@ bool GeometricParallelParking::planPullOut( paths.back().points.end(), road_center_line_path.points.begin() + 1, // to avoid overlapped point road_center_line_path.points.end()); - paths.back().points = motion_utils::removeOverlapPoints(paths.back().points); + paths.back().points = autoware::motion_utils::removeOverlapPoints(paths.back().points); // if the end point is the goal, set the velocity to 0 if (path_terminal_is_goal) { @@ -399,7 +399,7 @@ std::vector GeometricParallelParking::planOneTrial( // combine road and shoulder lanes // cut the road lanes up to start_pose to prevent unintended processing for overlapped lane lanelet::ConstLanelets lanes{}; - tier4_autoware_utils::Point2d start_point2d(start_pose.position.x, start_pose.position.y); + autoware::universe_utils::Point2d start_point2d(start_pose.position.x, start_pose.position.y); for (const auto & lane : road_lanes) { if (boost::geometry::within(start_point2d, lane.polygon2d().basicPolygon())) { lanes.push_back(lane); diff --git a/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp similarity index 93% rename from planning/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp index 642ad10b341dd..69b0deb8c4c5d 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include -#include namespace autoware::behavior_path_planner::utils::parking_departure { -using motion_utils::calcDecelDistWithJerkAndAccConstraints; +using autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints; std::optional calcFeasibleDecelDistance( std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, @@ -58,7 +58,7 @@ void modifyVelocityByDirection( auto pair_itr = std::begin(terminal_vel_acc_pairs); for (; path_itr != std::end(paths); ++path_itr, ++pair_itr) { - const auto is_driving_forward = motion_utils::isDrivingForward(path_itr->points); + const auto is_driving_forward = autoware::motion_utils::isDrivingForward(path_itr->points); // If the number of points in the path is less than 2, don't insert stop velocity and // set pairs_terminal_velocity_and_accel to 0 @@ -144,7 +144,7 @@ std::optional generateFeasibleStopPath( } // set stop point - const auto stop_idx = motion_utils::insertStopPoint( + const auto stop_idx = autoware::motion_utils::insertStopPoint( planner_data->self_odometry->pose.pose, *min_stop_distance, current_path.points); if (!stop_idx) { diff --git a/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp similarity index 95% rename from planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 6454800dff1f6..469be03eb905c 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -#include -#include +#include +#include #include @@ -39,7 +39,7 @@ bool position_filter( const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance) { - const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( + const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength( path_points, current_pose, object.kinematics.initial_pose_with_covariance.pose.position); return (backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance); @@ -71,16 +71,16 @@ bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::Cons } bool isPolygonOverlapLanelet( - const PredictedObject & object, const tier4_autoware_utils::Polygon2d & lanelet_polygon) + const PredictedObject & object, const autoware::universe_utils::Polygon2d & lanelet_polygon) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object); return !boost::geometry::disjoint(lanelet_polygon, object_polygon); } bool isPolygonOverlapLanelet( const PredictedObject & object, const lanelet::BasicPolygon2d & lanelet_polygon) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object); return !boost::geometry::disjoint(lanelet_polygon, object_polygon); } @@ -273,8 +273,8 @@ std::vector createPredictedPath( length = current_velocity * t_with_delay + 0.5 * acceleration * t_with_delay * t_with_delay; } - const auto pose = - motion_utils::calcInterpolatedPose(path_points, vehicle_pose_frenet.length + length); + const auto pose = autoware::motion_utils::calcInterpolatedPose( + path_points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } @@ -315,7 +315,7 @@ ExtendedPredictedObject transform( for (double t = 0.0; t < safety_check_time_horizon + 1e-3; t += safety_check_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); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths[i].path.emplace_back( t, *obj_pose, obj_velocity, obj_polygon); } diff --git a/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp similarity index 85% rename from planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 8a9415285037d..7bb8c13aa65fb 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/ros/uuid_helper.hpp" #include "interpolation/linear_interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/ros/uuid_helper.hpp" #include #include @@ -55,13 +55,13 @@ bool isTargetObjectFront( { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_offset_pose = - tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); + autoware::universe_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); // check all edges in the polygon const auto obj_polygon_outer = obj_polygon.outer(); for (const auto & obj_edge : obj_polygon_outer) { - const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); - if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { + const auto obj_point = autoware::universe_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (autoware::universe_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { return true; } } @@ -75,13 +75,13 @@ bool isTargetObjectFront( { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_point = - tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; + autoware::universe_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; // check all edges in the polygon const auto obj_polygon_outer = obj_polygon.outer(); for (const auto & obj_edge : obj_polygon_outer) { - const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); - if (motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { + const auto obj_point = autoware::universe_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (autoware::motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { return true; } } @@ -111,13 +111,13 @@ Polygon2d createExtendedPolygon( } const auto p1 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); const auto p2 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); const auto p3 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); const auto p4 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -125,16 +125,16 @@ Polygon2d createExtendedPolygon( appendPointToPolygon(polygon, p3.position); appendPointToPolygon(polygon, p4.position); appendPointToPolygon(polygon, p1.position); - return tier4_autoware_utils::isClockwise(polygon) + return autoware::universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); } Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(obj_pose, shape); if (obj_polygon.outer().empty()) { return obj_polygon; } @@ -145,8 +145,8 @@ Polygon2d createExtendedPolygon( double min_y = std::numeric_limits::max(); const auto obj_polygon_outer = obj_polygon.outer(); for (const auto & polygon_p : obj_polygon_outer) { - const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const auto transformed_p = tier4_autoware_utils::inverseTransformPoint(obj_p, obj_pose); + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto transformed_p = autoware::universe_utils::inverseTransformPoint(obj_p, obj_pose); max_x = std::max(transformed_p.x, max_x); min_x = std::min(transformed_p.x, min_x); @@ -168,13 +168,13 @@ Polygon2d createExtendedPolygon( } const auto p1 = - tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); const auto p2 = - tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); const auto p3 = - tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); const auto p4 = - tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); + autoware::universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -182,9 +182,9 @@ Polygon2d createExtendedPolygon( appendPointToPolygon(polygon, p3.position); appendPointToPolygon(polygon, p4.position); appendPointToPolygon(polygon, p1.position); - return tier4_autoware_utils::isClockwise(polygon) + return autoware::universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); } Polygon2d createExtendedPolygonAlongPath( @@ -208,7 +208,7 @@ Polygon2d createExtendedPolygonAlongPath( debug.lat_offset = lat_offset; } - const auto lon_offset_pose = motion_utils::calcLongitudinalOffsetPose( + const auto lon_offset_pose = autoware::motion_utils::calcLongitudinalOffsetPose( planned_path.points, base_link_pose.position, lon_length); if (!lon_offset_pose.has_value()) { return createExtendedPolygon( @@ -216,57 +216,57 @@ Polygon2d createExtendedPolygonAlongPath( } const size_t start_idx = - motion_utils::findNearestSegmentIndex(planned_path.points, base_link_pose.position); - const size_t end_idx = - motion_utils::findNearestSegmentIndex(planned_path.points, lon_offset_pose.value().position); + autoware::motion_utils::findNearestSegmentIndex(planned_path.points, base_link_pose.position); + const size_t end_idx = autoware::motion_utils::findNearestSegmentIndex( + planned_path.points, lon_offset_pose.value().position); Polygon2d polygon; { - const auto p_offset = - tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + const auto p_offset = autoware::universe_utils::calcOffsetPose( + base_link_pose, backward_lon_offset, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } for (size_t i = start_idx + 1; i < end_idx + 1; ++i) { - const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i)); - const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0); + const auto p = autoware::universe_utils::getPose(planned_path.points.at(i)); + const auto p_offset = autoware::universe_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = - tier4_autoware_utils::calcOffsetPose(lon_offset_pose.value(), base_to_front, lat_offset, 0.0); + const auto p_offset = autoware::universe_utils::calcOffsetPose( + lon_offset_pose.value(), base_to_front, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = tier4_autoware_utils::calcOffsetPose( + const auto p_offset = autoware::universe_utils::calcOffsetPose( lon_offset_pose.value(), base_to_front, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } for (size_t i = end_idx; i > start_idx; --i) { - const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i)); - const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0); + const auto p = autoware::universe_utils::getPose(planned_path.points.at(i)); + const auto p_offset = autoware::universe_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = - tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); + const auto p_offset = autoware::universe_utils::calcOffsetPose( + base_link_pose, backward_lon_offset, -lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } { - const auto p_offset = - tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + const auto p_offset = autoware::universe_utils::calcOffsetPose( + base_link_pose, backward_lon_offset, lat_offset, 0.0); appendPointToPolygon(polygon, p_offset.position); } - return tier4_autoware_utils::isClockwise(polygon) + return autoware::universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); } std::vector createExtendedPolygonsFromPoseWithVelocityStamped( @@ -283,7 +283,7 @@ std::vector createExtendedPolygonsFromPoseWithVelocityStamped( const double width = vehicle_info.vehicle_width_m + lat_margin * 2; const auto polygon = - tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width); + autoware::universe_utils::toFootprint(pose, base_to_front, base_to_rear, width); polygons.push_back(polygon); } @@ -350,7 +350,7 @@ std::optional calcInterpolatedPoseWithVelocity( const double time_step = pt.time - prev_pt.time; const double ratio = std::clamp(offset / time_step, 0.0, 1.0); const auto interpolated_pose = - tier4_autoware_utils::calcInterpolatedPose(prev_pt.pose, pt.pose, ratio, false); + autoware::universe_utils::calcInterpolatedPose(prev_pt.pose, pt.pose, ratio, false); const double interpolated_velocity = interpolation::lerp(prev_pt.velocity, pt.velocity, ratio); return PoseWithVelocityStamped{relative_time, interpolated_pose, interpolated_velocity}; @@ -378,7 +378,7 @@ std::optional getInterpolatedPoseWithVelocity const auto & velocity = interpolation_result->velocity; const auto ego_polygon = - tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width); + autoware::universe_utils::toFootprint(pose, base_to_front, base_to_rear, width); return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, ego_polygon}; } @@ -406,7 +406,7 @@ std::optional getInterpolatedPoseWithVelocity const auto & pose = interpolation_result->pose; const auto & velocity = interpolation_result->velocity; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(pose, shape); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(pose, shape); return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, obj_polygon}; } @@ -691,10 +691,11 @@ CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; debug.current_obj_pose = obj.initial_pose.pose; - debug.extended_obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); + debug.extended_obj_polygon = + autoware::universe_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); debug.obj_shape = obj.shape; debug.current_twist = obj.initial_twist.twist; - return {tier4_autoware_utils::toBoostUUID(obj.uuid), debug}; + return {autoware::universe_utils::toBoostUUID(obj.uuid), debug}; } void updateCollisionCheckDebugMap( diff --git a/planning/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp similarity index 98% rename from planning/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 0b21d500b164b..9572e5a92605f 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include #include #include -#include #include #include @@ -51,10 +51,10 @@ std::string toStr(const std::vector & v) namespace autoware::behavior_path_planner { -using motion_utils::findNearestIndex; -using motion_utils::insertOrientation; -using motion_utils::removeFirstInvalidOrientationPoints; -using motion_utils::removeOverlapPoints; +using autoware::motion_utils::findNearestIndex; +using autoware::motion_utils::insertOrientation; +using autoware::motion_utils::removeFirstInvalidOrientationPoints; +using autoware::motion_utils::removeOverlapPoints; void PathShifter::setPath(const PathWithLaneId & path) { diff --git a/planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp similarity index 92% rename from planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index e4dd9a401253f..46ca155d04daa 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include +#include #include #include #include -#include -#include -#include #include @@ -49,8 +49,8 @@ std::vector calcPathArcLengthArray( out.push_back(sum); for (size_t i = bounded_start + 1; i < bounded_end; ++i) { - sum += - tier4_autoware_utils::calcDistance2d(path.points.at(i).point, path.points.at(i - 1).point); + sum += autoware::universe_utils::calcDistance2d( + path.points.at(i).point, path.points.at(i - 1).point); out.push_back(sum); } return out; @@ -98,8 +98,8 @@ PathWithLaneId resamplePathWithSpline( // Get lane ids that are not duplicated std::vector s_in; std::unordered_set unique_lane_ids; - const auto s_vec = - motion_utils::calcSignedArcLengthPartialSum(transformed_path, 0, transformed_path.size()); + const auto s_vec = autoware::motion_utils::calcSignedArcLengthPartialSum( + transformed_path, 0, transformed_path.size()); for (size_t i = 0; i < path.points.size(); ++i) { const double s = s_vec.at(i); for (const auto & lane_id : path.points.at(i).lane_ids) { @@ -129,7 +129,8 @@ PathWithLaneId resamplePathWithSpline( } // Insert Stop Point - const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint(transformed_path); + const auto closest_stop_dist = + autoware::motion_utils::calcDistanceToForwardStopPoint(transformed_path); if (closest_stop_dist) { const auto close_indices = find_almost_same_values(s_out, *closest_stop_dist); if (close_indices) { @@ -152,7 +153,7 @@ PathWithLaneId resamplePathWithSpline( std::sort(s_out.begin(), s_out.end()); - return motion_utils::resamplePath(path, s_out); + return autoware::motion_utils::resamplePath(path, s_out); } size_t getIdxByArclength( @@ -162,7 +163,7 @@ size_t getIdxByArclength( throw std::runtime_error("[getIdxByArclength] path points must be > 0"); } - using tier4_autoware_utils::calcDistance2d; + using autoware::universe_utils::calcDistance2d; double sum_length = 0.0; if (signed_arc >= 0.0) { for (size_t i = target_idx; i < path.points.size() - 1; ++i) { @@ -184,7 +185,7 @@ size_t getIdxByArclength( return 0; } -// TODO(murooka) This function should be replaced with motion_utils::cropPoints +// TODO(murooka) This function should be replaced with autoware::motion_utils::cropPoints void clipPathLength( PathWithLaneId & path, const size_t target_idx, const double forward, const double backward) { @@ -201,7 +202,7 @@ void clipPathLength( path.points = clipped_points; } -// TODO(murooka) This function should be replaced with motion_utils::cropPoints +// TODO(murooka) This function should be replaced with autoware::motion_utils::cropPoints void clipPathLength( PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params) { @@ -411,7 +412,7 @@ std::vector dividePath( void correctDividedPathVelocity(std::vector & divided_paths) { for (auto & path : divided_paths) { - const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + const auto is_driving_forward = autoware::motion_utils::isDrivingForward(path.points); // If the number of points in the path is less than 2, don't correct the velocity if (!is_driving_forward) { continue; @@ -471,7 +472,7 @@ std::vector interpolatePose( std::vector interpolated_poses{}; // output const std::vector base_s{ - 0, tier4_autoware_utils::calcDistance2d(start_pose.position, end_pose.position)}; + 0, autoware::universe_utils::calcDistance2d(start_pose.position, end_pose.position)}; const std::vector base_x{start_pose.position.x, end_pose.position.x}; const std::vector base_y{start_pose.position.y, end_pose.position.y}; std::vector new_s; @@ -489,7 +490,7 @@ std::vector interpolatePose( std::sin(tf2::getYaw(end_pose.orientation)), new_s); for (size_t i = 0; i < interpolated_x.size(); ++i) { Pose pose{}; - pose = tier4_autoware_utils::calcInterpolatedPose( + pose = autoware::universe_utils::calcInterpolatedPose( end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); pose.position.x = interpolated_x.at(i); pose.position.y = interpolated_y.at(i); @@ -507,13 +508,15 @@ Pose getUnshiftedEgoPose(const Pose & ego_pose, const ShiftedPath & prev_path) } // un-shifted for current ideal pose - const auto closest_idx = motion_utils::findNearestIndex(prev_path.path.points, ego_pose.position); + const auto closest_idx = + autoware::motion_utils::findNearestIndex(prev_path.path.points, ego_pose.position); // NOTE: Considering avoidance by motion, we set unshifted_pose as previous path instead of // ego_pose. - auto unshifted_pose = motion_utils::calcInterpolatedPoint(prev_path.path, ego_pose).point.pose; + auto unshifted_pose = + autoware::motion_utils::calcInterpolatedPoint(prev_path.path, ego_pose).point.pose; - unshifted_pose = tier4_autoware_utils::calcOffsetPose( + unshifted_pose = autoware::universe_utils::calcOffsetPose( unshifted_pose, 0.0, -prev_path.shift_length.at(closest_idx), 0.0); unshifted_pose.orientation = ego_pose.orientation; @@ -571,7 +574,7 @@ PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); PathWithLaneId filtered_path = path; - filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); + filtered_path.points = autoware::motion_utils::removeOverlapPoints(filtered_path.points); return filtered_path; } @@ -612,10 +615,11 @@ BehaviorModuleOutput getReferencePath( // clip backward length // NOTE: In order to keep backward_path_length at least, resampling interval is added to the // backward. - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - reference_path.points, no_shift_pose, p.ego_nearest_dist_threshold, - p.ego_nearest_yaw_threshold); - reference_path.points = motion_utils::cropPoints( + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + reference_path.points, no_shift_pose, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold); + reference_path.points = autoware::motion_utils::cropPoints( reference_path.points, no_shift_pose.position, current_seg_idx, p.forward_path_length, p.backward_path_length + p.input_path_interval); diff --git a/planning/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp similarity index 94% rename from planning/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp index babcbefecbdeb..40802558c0b38 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include namespace autoware::behavior_path_planner::utils::traffic_light { -using motion_utils::calcSignedArcLength; +using autoware::motion_utils::calcSignedArcLength; double getDistanceToNextTrafficLight( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) @@ -98,7 +98,8 @@ std::optional calcDistanceToRedTrafficLight( const auto y = 0.5 * (stop_line.front().y() + stop_line.back().y()); const auto z = 0.5 * (stop_line.front().z() + stop_line.back().z()); - return calcSignedArcLength(path.points, ego_pos, tier4_autoware_utils::createPoint(x, y, z)); + return calcSignedArcLength( + path.points, ego_pos, autoware::universe_utils::createPoint(x, y, z)); } } diff --git a/planning/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp similarity index 90% rename from planning/autoware_behavior_path_planner_common/src/utils/utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 4610f7584ea29..55a6c6ff39d30 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include #include @@ -43,10 +43,11 @@ double calcInterpolatedZ( const tier4_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Point target_pos, const size_t seg_idx) { - const double closest_to_target_dist = motion_utils::calcSignedArcLength( + const double closest_to_target_dist = autoware::motion_utils::calcSignedArcLength( input.points, input.points.at(seg_idx).point.pose.position, target_pos); // TODO(murooka) implement calcSignedArcLength(points, idx, point) - const double seg_dist = motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + const double seg_dist = + autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); const double closest_z = input.points.at(seg_idx).point.pose.position.z; const double next_z = input.points.at(seg_idx + 1).point.pose.position.z; @@ -60,7 +61,8 @@ double calcInterpolatedZ( double calcInterpolatedVelocity( const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) { - const double seg_dist = motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + const double seg_dist = + autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); const double closest_vel = input.points.at(seg_idx).point.longitudinal_velocity_mps; const double next_vel = input.points.at(seg_idx + 1).point.longitudinal_velocity_mps; @@ -71,11 +73,11 @@ double calcInterpolatedVelocity( namespace autoware::behavior_path_planner::utils { +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::PoseWithCovarianceStamped; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; std::optional getPolygonByPoint( const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, @@ -111,7 +113,7 @@ double getDistanceBetweenPredictedPaths( if (!ego_pose) { continue; } - double distance = tier4_autoware_utils::calcDistance3d(*object_pose, *ego_pose); + double distance = autoware::universe_utils::calcDistance3d(*object_pose, *ego_pose); if (distance < min_distance) { min_distance = distance; } @@ -128,7 +130,7 @@ double getDistanceBetweenPredictedPathAndObject( double min_distance = std::numeric_limits::max(); rclcpp::Time ros_start_time = clock.now() + rclcpp::Duration::from_seconds(start_time); rclcpp::Time ros_end_time = clock.now() + rclcpp::Duration::from_seconds(end_time); - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object); for (double t = start_time; t < end_time; t += resolution) { const auto ego_pose = object_recognition_utils::calcInterpolatedPose(ego_path, t); if (!ego_pose) { @@ -145,7 +147,7 @@ double getDistanceBetweenPredictedPathAndObject( } bool checkCollisionBetweenPathFootprintsAndObjects( - const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, + const autoware::universe_utils::LinearRing2d & local_vehicle_footprint, const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double margin) { for (const auto & p : ego_path.points) { @@ -158,14 +160,14 @@ bool checkCollisionBetweenPathFootprintsAndObjects( } bool checkCollisionBetweenFootprintAndObjects( - const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, + const autoware::universe_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, const PredictedObjects & dynamic_objects, const double margin) { const auto vehicle_footprint = - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose)); + transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(ego_pose)); for (const auto & object : dynamic_objects.objects) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object); const double distance = boost::geometry::distance(obj_polygon, vehicle_footprint); if (distance < margin) return true; } @@ -176,18 +178,18 @@ double calcLateralDistanceFromEgoToObject( const Pose & ego_pose, const double vehicle_width, const PredictedObject & dynamic_object) { double min_distance = std::numeric_limits::max(); - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(dynamic_object); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(dynamic_object); const auto vehicle_left_pose = - tier4_autoware_utils::calcOffsetPose(ego_pose, 0, vehicle_width / 2, 0); + autoware::universe_utils::calcOffsetPose(ego_pose, 0, vehicle_width / 2, 0); const auto vehicle_right_pose = - tier4_autoware_utils::calcOffsetPose(ego_pose, 0, -vehicle_width / 2, 0); + autoware::universe_utils::calcOffsetPose(ego_pose, 0, -vehicle_width / 2, 0); for (const auto & p : obj_polygon.outer()) { - const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware::universe_utils::createPoint(p.x(), p.y(), 0.0); const double signed_distance_from_left = - tier4_autoware_utils::calcLateralDeviation(vehicle_left_pose, point); + autoware::universe_utils::calcLateralDeviation(vehicle_left_pose, point); const double signed_distance_from_right = - tier4_autoware_utils::calcLateralDeviation(vehicle_right_pose, point); + autoware::universe_utils::calcLateralDeviation(vehicle_right_pose, point); if (signed_distance_from_left < 0.0 && signed_distance_from_right > 0.0) { // point is between left and right @@ -206,21 +208,21 @@ double calcLongitudinalDistanceFromEgoToObject( const PredictedObject & dynamic_object) { double min_distance = std::numeric_limits::max(); - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(dynamic_object); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(dynamic_object); const auto vehicle_front_pose = - tier4_autoware_utils::calcOffsetPose(ego_pose, base_link2front, 0, 0); + autoware::universe_utils::calcOffsetPose(ego_pose, base_link2front, 0, 0); const auto vehicle_rear_pose = - tier4_autoware_utils::calcOffsetPose(ego_pose, base_link2rear, 0, 0); + autoware::universe_utils::calcOffsetPose(ego_pose, base_link2rear, 0, 0); for (const auto & p : obj_polygon.outer()) { - const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware::universe_utils::createPoint(p.x(), p.y(), 0.0); // forward is positive const double signed_distance_from_front = - tier4_autoware_utils::calcLongitudinalDeviation(vehicle_front_pose, point); + autoware::universe_utils::calcLongitudinalDeviation(vehicle_front_pose, point); // backward is positive const double signed_distance_from_rear = - -tier4_autoware_utils::calcLongitudinalDeviation(vehicle_rear_pose, point); + -autoware::universe_utils::calcLongitudinalDeviation(vehicle_rear_pose, point); if (signed_distance_from_front < 0.0 && signed_distance_from_rear < 0.0) { // point is between front and rear @@ -252,7 +254,7 @@ std::vector calcObjectsDistanceToPath( { std::vector distance_array; for (const auto & obj : objects.objects) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(obj); LineString2d ego_path_line; ego_path_line.reserve(ego_path.points.size()); for (const auto & p : ego_path.points) { @@ -288,7 +290,7 @@ std::optional findIndexOutOfGoalSearchRange( const auto & lane_ids = points.at(i).lane_ids; const double dist_to_goal = - tier4_autoware_utils::calcDistance2d(points.at(i).point.pose, goal); + autoware::universe_utils::calcDistance2d(points.at(i).point.pose, goal); const bool is_goal_lane_id_in_point = std::find(lane_ids.begin(), lane_ids.end(), goal_lane_id) != lane_ids.end(); if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { @@ -305,7 +307,7 @@ std::optional findIndexOutOfGoalSearchRange( // find index out of goal search range size_t min_dist_out_of_range_index = min_dist_index; for (int i = min_dist_index; 0 <= i; --i) { - const double dist = tier4_autoware_utils::calcDistance2d(points.at(i).point, goal); + const double dist = autoware::universe_utils::calcDistance2d(points.at(i).point, goal); min_dist_out_of_range_index = i; if (max_dist < dist) { break; @@ -341,7 +343,7 @@ bool setGoal( PathPointWithLaneId pre_refined_goal{}; constexpr double goal_to_pre_goal_distance = -1.0; pre_refined_goal.point.pose = - tier4_autoware_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); + autoware::universe_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); const size_t closest_seg_idx_for_pre_goal = findNearestSegmentIndex(input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); pre_refined_goal.point.pose.position.z = @@ -439,7 +441,7 @@ PathWithLaneId refinePathForGoal( { PathWithLaneId filtered_path = input; PathWithLaneId path_with_goal; - filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); + filtered_path.points = autoware::motion_utils::removeOverlapPoints(filtered_path.points); // always set zero velocity at the end of path for safety if (!filtered_path.points.empty()) { @@ -482,14 +484,15 @@ bool isInLaneletWithYawThreshold( const double pose_yaw = tf2::getYaw(current_pose.orientation); const double lanelet_angle = lanelet::utils::getLaneletAngle(lanelet, current_pose.position); const double angle_diff = - std::abs(tier4_autoware_utils::normalizeRadian(lanelet_angle - pose_yaw)); + std::abs(autoware::universe_utils::normalizeRadian(lanelet_angle - pose_yaw)); return (angle_diff < std::abs(yaw_threshold)) && lanelet::utils::isInLanelet(current_pose, lanelet, radius); } bool isEgoOutOfRoute( - const Pose & self_pose, const std::optional & modified_goal, + const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane, + const std::optional & modified_goal, const std::shared_ptr & route_handler) { const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid()) @@ -508,13 +511,17 @@ bool isEgoOutOfRoute( } // If ego vehicle is over goal on goal lane, return true - const double yaw_threshold = tier4_autoware_utils::deg2rad(90); - if (isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) { + const double yaw_threshold = autoware::universe_utils::deg2rad(90); + if ( + closest_road_lane.id() == goal_lane.id() && + isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) { constexpr double buffer = 1.0; const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose); const auto goal_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, route_handler->getGoalPose()); if (ego_arc_coord.length > goal_arc_coord.length + buffer) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("util"), "ego pose is beyond goal"); return true; } else { return false; @@ -526,14 +533,6 @@ bool isEgoOutOfRoute( const bool is_in_shoulder_lane = !route_handler->getShoulderLaneletsAtPose(self_pose).empty(); // Check if ego vehicle is in road lane const bool is_in_road_lane = std::invoke([&]() { - lanelet::ConstLanelet closest_road_lane; - if (!route_handler->getClosestLaneletWithinRoute(self_pose, &closest_road_lane)) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("util"), - "cannot find closest road lanelet"); - return false; - } - if (lanelet::utils::isInLanelet(self_pose, closest_road_lane)) { return true; } @@ -548,6 +547,7 @@ bool isEgoOutOfRoute( return false; }); + if (!is_in_shoulder_lane && !is_in_road_lane) { return true; } @@ -564,8 +564,8 @@ bool isEgoWithinOriginalLane( const auto base_link2front = common_param.base_link2front; const auto base_link2rear = common_param.base_link2rear; const auto vehicle_width = common_param.vehicle_width; - const auto vehicle_poly = - tier4_autoware_utils::toFootprint(current_pose, base_link2front, base_link2rear, vehicle_width); + const auto vehicle_poly = autoware::universe_utils::toFootprint( + current_pose, base_link2front, base_link2rear, vehicle_width); // Check if the ego vehicle is entirely within the lane with a given outer margin. for (const auto & p : vehicle_poly.outer()) { @@ -782,7 +782,7 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path) const size_t original_size = path.points.size(); // insert stop point - const auto insert_idx = motion_utils::insertStopPoint(length, path.points); + const auto insert_idx = autoware::motion_utils::insertStopPoint(length, path.points); if (!insert_idx) { return PathPointWithLaneId(); } @@ -851,16 +851,16 @@ std::optional getSignedDistanceFromBoundary( rear_left.y = vehicle_width / 2; front_left.x = base_link2front; front_left.y = vehicle_width / 2; - rear_corner_point = tier4_autoware_utils::transformPoint(rear_left, vehicle_pose); - front_corner_point = tier4_autoware_utils::transformPoint(front_left, vehicle_pose); + rear_corner_point = autoware::universe_utils::transformPoint(rear_left, vehicle_pose); + front_corner_point = autoware::universe_utils::transformPoint(front_left, vehicle_pose); } else { Point front_right, rear_right; rear_right.x = -base_link2rear; rear_right.y = -vehicle_width / 2; front_right.x = base_link2front; front_right.y = -vehicle_width / 2; - rear_corner_point = tier4_autoware_utils::transformPoint(rear_right, vehicle_pose); - front_corner_point = tier4_autoware_utils::transformPoint(front_right, vehicle_pose); + rear_corner_point = autoware::universe_utils::transformPoint(rear_right, vehicle_pose); + front_corner_point = autoware::universe_utils::transformPoint(front_right, vehicle_pose); } const auto combined_lane = lanelet::utils::combineLaneletsShape(lanelets); @@ -884,16 +884,18 @@ std::optional getSignedDistanceFromBoundary( const Point p1 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); const Point p2 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i + 1]); - const Point inverse_p1 = tier4_autoware_utils::inverseTransformPoint(p1, vehicle_corner_pose); - const Point inverse_p2 = tier4_autoware_utils::inverseTransformPoint(p2, vehicle_corner_pose); + const Point inverse_p1 = + autoware::universe_utils::inverseTransformPoint(p1, vehicle_corner_pose); + const Point inverse_p2 = + autoware::universe_utils::inverseTransformPoint(p2, vehicle_corner_pose); const double dx_p1 = inverse_p1.x; const double dx_p2 = inverse_p2.x; const double dy_p1 = inverse_p1.y; const double dy_p2 = inverse_p2.y; // Calculate the Euclidean distances between vehicle's corner and the current and next points. - const double distance1 = tier4_autoware_utils::calcDistance2d(p1, vehicle_corner_point); - const double distance2 = tier4_autoware_utils::calcDistance2d(p2, vehicle_corner_point); + const double distance1 = autoware::universe_utils::calcDistance2d(p1, vehicle_corner_point); + const double distance2 = autoware::universe_utils::calcDistance2d(p2, vehicle_corner_point); // If one of the bound points is behind and the other is in front of the vehicle corner point // and any of these points is closer than the current minimum distance, @@ -946,9 +948,9 @@ std::optional getSignedDistanceFromBoundary( bound_pose.orientation = vehicle_pose.orientation; const Point inverse_rear_point = - tier4_autoware_utils::inverseTransformPoint(rear_corner_point, bound_pose); + autoware::universe_utils::inverseTransformPoint(rear_corner_point, bound_pose); const Point inverse_front_point = - tier4_autoware_utils::inverseTransformPoint(front_corner_point, bound_pose); + autoware::universe_utils::inverseTransformPoint(front_corner_point, bound_pose); const double dx_rear = inverse_rear_point.x; const double dx_front = inverse_front_point.x; const double dy_rear = inverse_rear_point.y; @@ -1006,9 +1008,9 @@ Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) } polygon.outer().push_back(polygon.outer().front()); - return tier4_autoware_utils::isClockwise(polygon) + return autoware::universe_utils::isClockwise(polygon) ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); + : autoware::universe_utils::inverseClockwise(polygon); } Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) @@ -1019,7 +1021,9 @@ Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) } ret.outer().push_back(ret.outer().front()); - return tier4_autoware_utils::isClockwise(ret) ? ret : tier4_autoware_utils::inverseClockwise(ret); + return autoware::universe_utils::isClockwise(ret) + ? ret + : autoware::universe_utils::inverseClockwise(ret); } std::vector getTargetLaneletPolygons( @@ -1147,13 +1151,14 @@ PathWithLaneId getCenterLinePath( const auto raw_path_with_lane_id = route_handler.getCenterLinePath(lanelet_sequence, s_backward, s_forward, true); - auto resampled_path_with_lane_id = motion_utils::resamplePath( + auto resampled_path_with_lane_id = autoware::motion_utils::resamplePath( raw_path_with_lane_id, parameter.input_path_interval, parameter.enable_akima_spline_first); // convert centerline, which we consider as CoG center, to rear wheel center if (parameter.enable_cog_on_centerline) { const double rear_to_cog = parameter.vehicle_length / 2 - parameter.rear_overhang; - return motion_utils::convertToRearWheelCenter(resampled_path_with_lane_id, rear_to_cog); + return autoware::motion_utils::convertToRearWheelCenter( + resampled_path_with_lane_id, rear_to_cog); } return resampled_path_with_lane_id; @@ -1192,7 +1197,7 @@ PathWithLaneId setDecelerationVelocity( for (auto & point : reference_path.points) { const auto arclength_to_target = std::max( - 0.0, motion_utils::calcSignedArcLength( + 0.0, autoware::motion_utils::calcSignedArcLength( reference_path.points, point.point.pose.position, target_pose.position) + buffer); if (arclength_to_target > deceleration_interval) continue; @@ -1205,7 +1210,8 @@ PathWithLaneId setDecelerationVelocity( } const auto stop_point_length = - motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + buffer; + autoware::motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + + buffer; constexpr double eps{0.01}; if (std::abs(target_velocity) < eps && stop_point_length > 0.0) { const auto stop_point = utils::insertStopPoint(stop_point_length, reference_path); @@ -1308,26 +1314,26 @@ lanelet::ConstLanelets extendNextLane( { if (lanes.empty()) return lanes; - auto extended_lanes = lanes; + const auto next_lanes = route_handler->getNextLanelets(lanes.back()); + if (next_lanes.empty()) return lanes; // Add next lane - const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back()); - if (!next_lanes.empty()) { - std::optional target_next_lane; - if (!only_in_route) { - target_next_lane = next_lanes.front(); - } - // use the next lane in route if it exists - for (const auto & next_lane : next_lanes) { - if (route_handler->isRouteLanelet(next_lane)) { - target_next_lane = next_lane; - } - } - if (target_next_lane) { - extended_lanes.push_back(*target_next_lane); + auto extended_lanes = lanes; + std::optional target_next_lane; + for (const auto & next_lane : next_lanes) { + // skip overlapping lanes + if (next_lane.id() == lanes.front().id()) continue; + + // if route lane, set target and break + if (route_handler->isRouteLanelet(next_lane)) { + target_next_lane = next_lane; + break; } + if (!only_in_route && !target_next_lane) target_next_lane = next_lane; + } + if (target_next_lane) { + extended_lanes.push_back(*target_next_lane); } - return extended_lanes; } @@ -1337,24 +1343,25 @@ lanelet::ConstLanelets extendPrevLane( { if (lanes.empty()) return lanes; - auto extended_lanes = lanes; + const auto prev_lanes = route_handler->getPreviousLanelets(lanes.front()); + if (prev_lanes.empty()) return lanes; // Add previous lane - const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front()); - if (!prev_lanes.empty()) { - std::optional target_prev_lane; - if (!only_in_route) { - target_prev_lane = prev_lanes.front(); - } - // use the previous lane in route if it exists - for (const auto & prev_lane : prev_lanes) { - if (route_handler->isRouteLanelet(prev_lane)) { - target_prev_lane = prev_lane; - } - } - if (target_prev_lane) { - extended_lanes.insert(extended_lanes.begin(), *target_prev_lane); + auto extended_lanes = lanes; + std::optional target_prev_lane; + for (const auto & prev_lane : prev_lanes) { + // skip overlapping lanes + if (prev_lane.id() == lanes.back().id()) continue; + + // if route lane, set target and break + if (route_handler->isRouteLanelet(prev_lane)) { + target_prev_lane = prev_lane; + break; } + if (!only_in_route && !target_prev_lane) target_prev_lane = prev_lane; + } + if (target_prev_lane) { + extended_lanes.insert(extended_lanes.begin(), *target_prev_lane); } return extended_lanes; } diff --git a/planning/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp similarity index 97% rename from planning/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp index 39363a97ad8c0..bf69647304833 100644 --- a/planning/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include diff --git a/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp similarity index 86% rename from planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp index 2a3bbff492dfb..85a80eac8f215 100644 --- a/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include +#include #include @@ -28,12 +28,12 @@ constexpr double epsilon = 1e-6; using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { @@ -47,8 +47,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); - ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + ego_pose.position = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); + ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -75,8 +75,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = tier4_autoware_utils::createPoint(3.0, 4.0, 0.0); - ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + ego_pose.position = autoware::universe_utils::createPoint(3.0, 4.0, 0.0); + ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -103,9 +103,9 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { Pose ego_pose; - ego_pose.position = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + ego_pose.position = autoware::universe_utils::createPoint(0.0, 0.0, 0.0); ego_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::deg2rad(60)); + autoware::universe_utils::createQuaternionFromYaw(autoware::universe_utils::deg2rad(60)); const double lon_length = 10.0; const double lat_margin = 2.0; @@ -134,13 +134,13 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) { using autoware::behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::createQuaternionFromYaw; + using autoware::universe_utils::createPoint; + using autoware::universe_utils::createQuaternionFromYaw; { Pose obj_pose; obj_pose.position = createPoint(0.0, 0.0, 0.0); - obj_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + obj_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); Shape shape; shape.type = autoware_perception_msgs::msg::Shape::POLYGON; diff --git a/planning/autoware_behavior_path_planner_common/test/test_turn_signal.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp similarity index 80% rename from planning/autoware_behavior_path_planner_common/test/test_turn_signal.cpp rename to planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp index 74c78e7024b72..b6149e3b5f99c 100644 --- a/planning/autoware_behavior_path_planner_common/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp @@ -11,10 +11,10 @@ // 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 "autoware_behavior_path_planner_common/turn_signal_decider.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include +#include #include "autoware_planning_msgs/msg/path_point.hpp" #include @@ -26,13 +26,13 @@ using autoware::behavior_path_planner::PathWithLaneId; using autoware::behavior_path_planner::Pose; using autoware::behavior_path_planner::TurnSignalDecider; using autoware::behavior_path_planner::TurnSignalInfo; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromYaw; using autoware_planning_msgs::msg::PathPoint; using autoware_vehicle_msgs::msg::HazardLightsCommand; using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Twist; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromYaw; using tier4_planning_msgs::msg::PathPointWithLaneId; constexpr double nearest_dist_threshold = 5.0; @@ -105,8 +105,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose on the behavior desired start { Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -116,8 +117,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is right before the intersection required start { Pose current_pose = generateEgoSamplePose(34.99f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -127,8 +129,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is within the intersection required section { Pose current_pose = generateEgoSamplePose(40.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -138,8 +141,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is within the intersection and behavior required section { Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -149,8 +153,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is on the intersection required end { Pose current_pose = generateEgoSamplePose(48.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -160,8 +165,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is right after the intersection required end { Pose current_pose = generateEgoSamplePose(48.1f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -171,8 +177,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is on the behavior required end { Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -182,8 +189,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is right after the behavior required end { Pose current_pose = generateEgoSamplePose(50.1f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -193,8 +201,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is right on the intersection desired end { Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -204,8 +213,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is right right after the intersection desired end { Pose current_pose = generateEgoSamplePose(65.1f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -244,8 +254,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose on the behavior desired start { Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -255,8 +266,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is right before the intersection required start { Pose current_pose = generateEgoSamplePose(34.99f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -266,8 +278,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is on the behavior required start { Pose current_pose = generateEgoSamplePose(40.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -277,8 +290,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is on the behavior required end { Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -288,8 +302,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is on the intersection required end { Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -299,8 +314,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is right after the intersection required end { Pose current_pose = generateEgoSamplePose(50.1f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -310,8 +326,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is on the intersection desired end { Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -338,7 +355,6 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) TurnSignalInfo behavior_signal_info; behavior_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - behavior_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; behavior_signal_info.desired_start_point.position = createPoint(5.0, 0.0, 0.0); behavior_signal_info.desired_start_point.orientation = createQuaternionFromYaw(0.0); behavior_signal_info.desired_end_point.position = createPoint(70.0, 0.0, 0.0); @@ -351,8 +367,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose on the behavior desired start { Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -362,8 +379,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right before the behavior required start { Pose current_pose = generateEgoSamplePose(29.9f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -373,8 +391,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right on the behavior required start { Pose current_pose = generateEgoSamplePose(30.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -384,8 +403,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right before the intersection required start { Pose current_pose = generateEgoSamplePose(33.9f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -395,8 +415,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right on the intersection required start { Pose current_pose = generateEgoSamplePose(35.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -406,8 +427,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right before the behavior required end { Pose current_pose = generateEgoSamplePose(44.9f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -417,8 +439,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right on the behavior required end { Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -428,8 +451,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right before the intersection required end { Pose current_pose = generateEgoSamplePose(49.9f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -439,8 +463,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right on the intersection required end { Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -450,8 +475,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right on the intersection desired end { Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); diff --git a/planning/autoware_behavior_path_sampling_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_path_sampling_planner_module/CMakeLists.txt rename to planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/CMakeLists.txt diff --git a/planning/autoware_behavior_path_sampling_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/README.md similarity index 100% rename from planning/autoware_behavior_path_sampling_planner_module/README.md rename to planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/README.md diff --git a/planning/autoware_behavior_path_sampling_planner_module/config/sampling_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/config/sampling_planner.param.yaml similarity index 100% rename from planning/autoware_behavior_path_sampling_planner_module/config/sampling_planner.param.yaml rename to planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/config/sampling_planner.param.yaml diff --git a/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp similarity index 78% rename from planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/manager.hpp rename to planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp index 180d1af6190d2..28c310ae20ec2 100644 --- a/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ -#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "autoware_behavior_path_sampling_planner_module/sampling_planner_module.hpp" -#include "autoware_behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp" +#include "autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" #include @@ -50,4 +50,4 @@ class SamplingPlannerModuleManager : public SceneModuleManagerInterface } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp similarity index 88% rename from planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_module.hpp rename to planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp index 9f518d102257b..2b6f52998cb23 100644 --- a/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -12,17 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ - -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "autoware_behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" -#include "autoware_behavior_path_sampling_planner_module/util.hpp" +#ifndef AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ + +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" +#include "autoware/behavior_path_sampling_planner_module/util.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include "autoware_bezier_sampler/bezier_sampling.hpp" #include "autoware_frenet_planner/frenet_planner.hpp" #include "autoware_sampler_common/constraints/footprint.hpp" @@ -33,13 +39,7 @@ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" #include "tier4_planning_msgs/msg/lateral_offset.hpp" #include "tier4_planning_msgs/msg/path_with_lane_id.hpp" @@ -75,8 +75,8 @@ struct SamplingPlannerDebugData { std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; - std::vector obstacles{}; - std::vector footprints{}; + std::vector obstacles{}; + std::vector footprints{}; }; class SamplingPlannerModule : public SceneModuleInterface { @@ -204,10 +204,10 @@ class SamplingPlannerModule : public SceneModuleInterface if (length_to_goal < epsilon) return isReferencePathSafe(); const auto nearest_index = - motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose); + autoware::motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose); if (!nearest_index) return false; auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double { - const auto rpy = tier4_autoware_utils::getRPY(quat); + const auto rpy = autoware::universe_utils::getRPY(quat); return rpy.z; }; const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation; @@ -263,4 +263,4 @@ class SamplingPlannerModule : public SceneModuleInterface } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ diff --git a/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp similarity index 81% rename from planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_parameters.hpp rename to planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp index 6344af7163577..f778eef981a92 100644 --- a/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ #include "autoware_bezier_sampler/bezier_sampling.hpp" #include "autoware_sampler_common/structures.hpp" @@ -21,11 +21,11 @@ #include namespace autoware::behavior_path_planner { -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::MultiPoint2d; -using tier4_autoware_utils::MultiPolygon2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPolygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; struct SamplingPlannerParameters { // constraints.hard @@ -88,4 +88,4 @@ struct SamplingPlannerInternalParameters Preprocessing preprocessing{}; }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ diff --git a/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp similarity index 84% rename from planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/util.hpp rename to planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp index 1f910812f5360..4408645b26095 100644 --- a/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/util.hpp @@ -12,15 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ #include "autoware_sampler_common/structures.hpp" #include "autoware_sampler_common/transform/spline_transform.hpp" -#include +#include +#include + +#include #include +#include +#include + #include #include #include @@ -29,6 +35,8 @@ namespace autoware::behavior_path_planner { using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathWithLaneId; +using PlanResult = PathWithLaneId::SharedPtr; struct SoftConstraintsInputs { @@ -93,7 +101,7 @@ inline autoware::sampler_common::State getInitialState( { autoware::sampler_common::State initial_state; Point2d initial_state_pose{pose.position.x, pose.position.y}; - const auto rpy = tier4_autoware_utils::getRPY(pose.orientation); + const auto rpy = autoware::universe_utils::getRPY(pose.orientation); initial_state.pose = initial_state_pose; initial_state.frenet = reference_spline.frenet({pose.position.x, pose.position.y}); initial_state.heading = rpy.z; @@ -102,4 +110,4 @@ inline autoware::sampler_common::State getInitialState( } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/autoware_behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml similarity index 95% rename from planning/autoware_behavior_path_sampling_planner_module/package.xml rename to planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml index 7934c2fb0ae91..aab393f4bdb5a 100644 --- a/planning/autoware_behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml @@ -16,17 +16,18 @@ autoware_behavior_path_planner_common autoware_bezier_sampler autoware_frenet_planner + autoware_motion_utils autoware_path_sampler autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs interpolation lanelet2_extension - motion_utils pluginlib rclcpp rclcpp_components @@ -36,7 +37,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_path_sampling_planner_module/plugins.xml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_path_sampling_planner_module/plugins.xml rename to planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/plugins.xml diff --git a/planning/autoware_behavior_path_sampling_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp similarity index 97% rename from planning/autoware_behavior_path_sampling_planner_module/src/manager.cpp rename to planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp index 89be13bc9dabd..af8a538f894cc 100644 --- a/planning/autoware_behavior_path_sampling_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_sampling_planner_module/manager.hpp" +#include "autoware/behavior_path_sampling_planner_module/manager.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -77,7 +77,7 @@ void SamplingPlannerModuleManager::init(rclcpp::Node * node) void SamplingPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; auto & p = parameters_; diff --git a/planning/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp similarity index 97% rename from planning/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp rename to planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 9a7eb970e521a..526a761e6957a 100644 --- a/planning/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_sampling_planner_module/sampling_planner_module.hpp" +#include "autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp" namespace autoware::behavior_path_planner { +using autoware::motion_utils::calcSignedArcLength; +using autoware::motion_utils::findNearestIndex; +using autoware::motion_utils::findNearestSegmentIndex; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::getPoint; +using autoware::universe_utils::Point2d; using geometry_msgs::msg::Point; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::getPoint; -using tier4_autoware_utils::Point2d; namespace bg = boost::geometry; namespace bgi = boost::geometry::index; @@ -103,7 +103,7 @@ SamplingPlannerModule::SamplingPlannerModule( // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { // if (path.points.empty()) return 0.0; // const auto & goal_pose_yaw = - // tier4_autoware_utils::getRPY(input_data.goal_pose.orientation).z; const auto & + // autoware::universe_utils::getRPY(input_data.goal_pose.orientation).z; const auto & // last_point_yaw = path.yaws.back(); const double angle_difference = std::abs(last_point_yaw // - goal_pose_yaw); return angle_difference / (3.141519 / 4.0); // }); @@ -190,7 +190,7 @@ bool SamplingPlannerModule::isExecutionRequested() const return false; } - if (!motion_utils::isDrivingForward(getPreviousModuleOutput().reference_path.points)) { + if (!autoware::motion_utils::isDrivingForward(getPreviousModuleOutput().reference_path.points)) { RCLCPP_WARN(getLogger(), "Backward path is NOT supported. Just converting path to trajectory"); return false; } @@ -343,7 +343,7 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID( point.lane_ids = path.points.at(i - 1).lane_ids; } if (reference_path_ptr) { - const auto idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( reference_path_ptr->points, point.point.pose); const auto & closest_point = reference_path_ptr->points[idx]; point.point.longitudinal_velocity_mps = closest_point.point.longitudinal_velocity_mps; @@ -365,9 +365,9 @@ void SamplingPlannerModule::prepareConstraints( size_t i = 0; for (const auto & o : predicted_objects->objects) { if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) { - const auto polygon = tier4_autoware_utils::toPolygon2d(o); + const auto polygon = autoware::universe_utils::toPolygon2d(o); constraints.obstacle_polygons.push_back(polygon); - const auto box = boost::geometry::return_envelope(polygon); + const auto box = boost::geometry::return_envelope(polygon); constraints.rtree.insert(std::make_pair(box, i)); } i++; diff --git a/planning/autoware_behavior_path_side_shift_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_path_side_shift_module/CMakeLists.txt rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/CMakeLists.txt diff --git a/planning/autoware_behavior_path_side_shift_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/README.md similarity index 100% rename from planning/autoware_behavior_path_side_shift_module/README.md rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/README.md diff --git a/planning/autoware_behavior_path_side_shift_module/config/side_shift.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/config/side_shift.param.yaml similarity index 100% rename from planning/autoware_behavior_path_side_shift_module/config/side_shift.param.yaml rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/config/side_shift.param.yaml diff --git a/planning/autoware_behavior_path_side_shift_module/images/side_shift_status.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/images/side_shift_status.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_side_shift_module/images/side_shift_status.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/images/side_shift_status.drawio.svg diff --git a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/data_structs.hpp similarity index 84% rename from planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/data_structs.hpp index 9140132de1335..92565197abf70 100644 --- a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/data_structs.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include @@ -51,4 +51,4 @@ struct SideShiftDebugData }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp similarity index 81% rename from planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.hpp rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp index 339d1e2679e31..b495e6619a1c7 100644 --- a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ -#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "autoware_behavior_path_side_shift_module/scene.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware/behavior_path_side_shift_module/scene.hpp" #include @@ -50,4 +50,4 @@ class SideShiftModuleManager : public SceneModuleManagerInterface } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp similarity index 91% rename from planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp index d2ef96056a9ce..543b17aca9352 100644 --- a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware_behavior_path_side_shift_module/data_structs.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_side_shift_module/data_structs.hpp" #include @@ -131,4 +131,4 @@ class SideShiftModule : public SceneModuleInterface } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ diff --git a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp similarity index 88% rename from planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp index bda216b812467..a36eaabcf928c 100644 --- a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ #include #include @@ -41,4 +41,4 @@ Point transformToGrid( } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ diff --git a/planning/autoware_behavior_path_side_shift_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml similarity index 94% rename from planning/autoware_behavior_path_side_shift_module/package.xml rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml index f3ac76d2bb1bb..96b305a63a454 100644 --- a/planning/autoware_behavior_path_side_shift_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml @@ -20,12 +20,12 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_planning_msgs + autoware_universe_utils geometry_msgs - motion_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs ament_cmake_ros diff --git a/planning/autoware_behavior_path_side_shift_module/plugins.xml b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_path_side_shift_module/plugins.xml rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/plugins.xml diff --git a/planning/autoware_behavior_path_side_shift_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp similarity index 93% rename from planning/autoware_behavior_path_side_shift_module/src/manager.cpp rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp index e1ac25726d6d0..2e8d7e98c11cf 100644 --- a/planning/autoware_behavior_path_side_shift_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/manager.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_side_shift_module/manager.hpp" +#include "autoware/behavior_path_side_shift_module/manager.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -48,7 +48,7 @@ void SideShiftModuleManager::init(rclcpp::Node * node) void SideShiftModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; [[maybe_unused]] auto p = parameters_; diff --git a/planning/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp similarity index 94% rename from planning/autoware_behavior_path_side_shift_module/src/scene.cpp rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index 2ddd0de4cddfc..bcb972bf5dc8b 100644 --- a/planning/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_side_shift_module/scene.hpp" +#include "autoware/behavior_path_side_shift_module/scene.hpp" -#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "autoware_behavior_path_side_shift_module/utils.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_side_shift_module/utils.hpp" #include @@ -28,12 +28,12 @@ namespace autoware::behavior_path_planner { +using autoware::motion_utils::calcSignedArcLength; +using autoware::motion_utils::findNearestIndex; +using autoware::motion_utils::findNearestSegmentIndex; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::getPoint; using geometry_msgs::msg::Point; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::getPoint; SideShiftModule::SideShiftModule( const std::string & name, rclcpp::Node & node, @@ -195,7 +195,8 @@ void SideShiftModule::updateData() const auto longest_dist_to_shift_line = [&]() { double max_dist = 0.0; for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max(max_dist, tier4_autoware_utils::calcDistance2d(getEgoPose(), pnt.start)); + max_dist = + std::max(max_dist, autoware::universe_utils::calcDistance2d(getEgoPose(), pnt.start)); } return max_dist; }(); @@ -372,7 +373,8 @@ double SideShiftModule::getClosestShiftLength() const } const auto ego_point = planner_data_->self_odometry->pose.pose.position; - const auto closest = motion_utils::findNearestIndex(prev_output_.path.points, ego_point); + const auto closest = + autoware::motion_utils::findNearestIndex(prev_output_.path.points, ego_point); return prev_output_.shift_length.at(closest); } @@ -395,7 +397,7 @@ BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & pat auto output_path = path.path; const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(output_path.points); const auto & current_pose = planner_data_->self_odometry->pose.pose; - output_path.points = motion_utils::cropPoints( + output_path.points = autoware::motion_utils::cropPoints( output_path.points, current_pose.position, current_seg_idx, p.forward_path_length, p.backward_path_length + p.input_path_interval); @@ -467,7 +469,7 @@ void SideShiftModule::setDebugMarkersVisualization() const debug_marker_.markers.clear(); const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + autoware::universe_utils::appendMarkerArray(added, &debug_marker_); }; const auto add_shift_line_marker = [this, add]( diff --git a/planning/autoware_behavior_path_side_shift_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp similarity index 95% rename from planning/autoware_behavior_path_side_shift_module/src/utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp index 783874720b17d..60fc5f74a3e1a 100644 --- a/planning/autoware_behavior_path_side_shift_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_side_shift_module/utils.hpp" +#include "autoware/behavior_path_side_shift_module/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" #include diff --git a/planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp similarity index 98% rename from planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index fecafe1a8680b..1d70f1388d80b 100644 --- a/planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" #include #include diff --git a/planning/autoware_behavior_path_start_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/CMakeLists.txt rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt diff --git a/planning/autoware_behavior_path_start_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/README.md rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md diff --git a/planning/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml similarity index 99% rename from planning/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index 5882480a596af..ce7ead87b54c7 100644 --- a/planning/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -35,6 +35,7 @@ # geometric pull out enable_geometric_pull_out: true geometric_collision_check_distance_from_end: 0.0 + arc_path_interval: 1.0 divide_pull_out_path: true geometric_pull_out_velocity: 1.0 lane_departure_margin: 0.2 diff --git a/planning/autoware_behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/images/freespace_pull_out.png b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/freespace_pull_out.png similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/freespace_pull_out.png rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/freespace_pull_out.png diff --git a/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/images/priority_order.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/priority_order.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/priority_order.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/priority_order.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/shift_pull_out.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/shift_pull_out.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/shift_pull_out.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/images/start_planner_example.png b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/start_planner_example.png similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/start_planner_example.png rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/start_planner_example.png diff --git a/planning/autoware_behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp similarity index 91% rename from planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 9d6a716ecc22c..6eea3e3576732 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -13,15 +13,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ -#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -168,4 +168,4 @@ struct StartPlannerParameters } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/debug.hpp similarity index 81% rename from planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/debug.hpp index 58b9f2d1aa428..81358f37132f5 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/debug.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ -#include "autoware_behavior_path_start_planner_module/data_structs.hpp" +#include "autoware/behavior_path_start_planner_module/data_structs.hpp" #include #include @@ -36,4 +36,4 @@ void updateSafetyCheckDebugData( } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp similarity index 75% rename from planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index 61165ef1956db..145ca5175762e 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ -#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include -#include -#include +#include +#include +#include #include @@ -50,4 +50,4 @@ class FreespacePullOut : public PullOutPlannerBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp similarity index 75% rename from planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index 5ec2d8823a1db..c63dd392a5463 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ -#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" -#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include +#include #include @@ -46,4 +46,4 @@ class GeometricPullOut : public PullOutPlannerBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp similarity index 82% rename from planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp index e2634c7feb34a..5d3d224673124 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ -#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "autoware_behavior_path_start_planner_module/start_planner_module.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware/behavior_path_start_planner_module/start_planner_module.hpp" #include @@ -53,4 +53,4 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_path.hpp similarity index 79% rename from planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_path.hpp index d0a764b7b3f1a..764499ea6dbcf 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_path.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include @@ -34,4 +34,4 @@ struct PullOutPath Pose end_pose{}; }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp similarity index 85% rename from planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index 77049ab1d9179..7b30745057743 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_start_planner_module/data_structs.hpp" -#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" -#include "autoware_behavior_path_start_planner_module/util.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_start_planner_module/data_structs.hpp" +#include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware/behavior_path_start_planner_module/util.hpp" #include #include @@ -30,8 +30,8 @@ namespace autoware::behavior_path_planner { +using autoware::universe_utils::LinearRing2d; using geometry_msgs::msg::Pose; -using tier4_autoware_utils::LinearRing2d; using tier4_planning_msgs::msg::PathWithLaneId; class PullOutPlannerBase @@ -97,4 +97,4 @@ class PullOutPlannerBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp similarity index 84% rename from planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index df9f461cb9220..92cad1c2eb8b8 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ -#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" -#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include +#include #include @@ -63,4 +63,4 @@ class ShiftPullOut : public PullOutPlannerBase }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp similarity index 92% rename from planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 1d09502fe1a64..7554098414442 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ - -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "autoware_behavior_path_start_planner_module/data_structs.hpp" -#include "autoware_behavior_path_start_planner_module/freespace_pull_out.hpp" -#include "autoware_behavior_path_start_planner_module/geometric_pull_out.hpp" -#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" -#include "autoware_behavior_path_start_planner_module/shift_pull_out.hpp" - -#include +#ifndef AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ + +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_start_planner_module/data_structs.hpp" +#include "autoware/behavior_path_start_planner_module/freespace_pull_out.hpp" +#include "autoware/behavior_path_start_planner_module/geometric_pull_out.hpp" +#include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware/behavior_path_start_planner_module/shift_pull_out.hpp" + +#include #include #include @@ -337,4 +337,4 @@ class StartPlannerModule : public SceneModuleInterface }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/util.hpp similarity index 78% rename from planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/util.hpp index 0cbb0fd504c4b..09485e68852a3 100644 --- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/util.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" -#include +#include #include #include @@ -55,4 +55,4 @@ std::optional extractCollisionCheckSection( const PullOutPath & path, const double collision_check_distance_from_end); } // namespace autoware::behavior_path_planner::start_planner_utils -#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml similarity index 95% rename from planning/autoware_behavior_path_start_planner_module/package.xml rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml index 5b2ad9f8dc9c1..77f83993f2ccc 100644 --- a/planning/autoware_behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml @@ -22,11 +22,11 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_rtc_interface - motion_utils + autoware_universe_utils pluginlib rclcpp - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_path_start_planner_module/plugins.xml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_path_start_planner_module/plugins.xml rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/plugins.xml diff --git a/planning/autoware_behavior_path_start_planner_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/debug.cpp similarity index 94% rename from planning/autoware_behavior_path_start_planner_module/src/debug.cpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/debug.cpp index 1c07d175a3d09..1b25d8eac8846 100644 --- a/planning/autoware_behavior_path_start_planner_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_start_planner_module/debug.hpp" +#include "autoware/behavior_path_start_planner_module/debug.hpp" namespace autoware::behavior_path_planner { diff --git a/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp similarity index 92% rename from planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index 901c9f6ff2263..137a2c53321fc 100644 --- a/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_start_planner_module/freespace_pull_out.hpp" +#include "autoware/behavior_path_start_planner_module/freespace_pull_out.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "autoware_behavior_path_start_planner_module/util.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_start_planner_module/util.hpp" #include @@ -81,7 +81,7 @@ std::optional FreespacePullOut::plan( const size_t index = std::distance(last_path.points.begin(), it); if (index == 0) continue; const double distance = - tier4_autoware_utils::calcDistance2d(end_pose.position, it->point.pose.position); + autoware::universe_utils::calcDistance2d(end_pose.position, it->point.pose.position); if (distance < th_end_distance) { last_path.points.erase(it, last_path.points.end()); break; diff --git a/planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp similarity index 86% rename from planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index b19abfc717ec0..f3239d41bc1b9 100644 --- a/planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_start_planner_module/geometric_pull_out.hpp" +#include "autoware/behavior_path_start_planner_module/geometric_pull_out.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "autoware_behavior_path_start_planner_module/util.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_start_planner_module/util.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include +using autoware::motion_utils::findNearestIndex; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcOffsetPose; using lanelet::utils::getArcCoordinates; -using motion_utils::findNearestIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; namespace autoware::behavior_path_planner { using start_planner_utils::getPullOutLanes; @@ -92,14 +92,14 @@ std::optional GeometricPullOut::plan( // insert stop velocity to first arc path end output.partial_paths.front().points.back().point.longitudinal_velocity_mps = 0.0; const double arc_length_on_first_arc_path = - motion_utils::calcArcLength(output.partial_paths.front().points); + autoware::motion_utils::calcArcLength(output.partial_paths.front().points); const double time_to_center = arc_length_on_first_arc_path / (2 * velocity); const double average_velocity = arc_length_on_first_arc_path / (time_to_center * 2); const double average_acceleration = average_velocity / (time_to_center * 2); output.pairs_terminal_velocity_and_accel.push_back( std::make_pair(average_velocity, average_acceleration)); const double arc_length_on_second_arc_path = - motion_utils::calcArcLength(planner_.getArcPaths().at(1).points); + autoware::motion_utils::calcArcLength(planner_.getArcPaths().at(1).points); output.pairs_terminal_velocity_and_accel.push_back( std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path))); } else { @@ -109,7 +109,7 @@ std::optional GeometricPullOut::plan( // Calculate the acceleration required to reach the forward parking velocity at the center of // the path, assuming constant acceleration and deceleration. - const double arc_length_on_path = motion_utils::calcArcLength(combined_path.points); + const double arc_length_on_path = autoware::motion_utils::calcArcLength(combined_path.points); output.pairs_terminal_velocity_and_accel.push_back( std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } diff --git a/planning/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp similarity index 98% rename from planning/autoware_behavior_path_start_planner_module/src/manager.cpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 5bc22a01c1e09..9451bb3870648 100644 --- a/planning/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_start_planner_module/manager.hpp" +#include "autoware/behavior_path_start_planner_module/manager.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -91,6 +91,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); p.parallel_parking_parameters.pull_out_velocity = node->declare_parameter(ns + "geometric_pull_out_velocity"); + p.parallel_parking_parameters.pull_out_arc_path_interval = + node->declare_parameter(ns + "arc_path_interval"); p.parallel_parking_parameters.pull_out_lane_departure_margin = node->declare_parameter(ns + "lane_departure_margin"); p.lane_departure_check_expansion_margin = @@ -349,7 +351,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) void StartPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { - using tier4_autoware_utils::updateParam; + using autoware::universe_utils::updateParam; auto & p = parameters_; @@ -416,6 +418,9 @@ void StartPlannerModuleManager::updateModuleParams( parameters, ns + "maximum_longitudinal_deviation", p->maximum_longitudinal_deviation); updateParam(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); updateParam(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path); + updateParam( + parameters, ns + "arc_path_interval", + p->parallel_parking_parameters.pull_out_arc_path_interval); updateParam( parameters, ns + "lane_departure_margin", p->parallel_parking_parameters.pull_out_lane_departure_margin); diff --git a/planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp similarity index 90% rename from planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 4e554adc9b931..971c9a48e8b40 100644 --- a/planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -12,25 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_start_planner_module/shift_pull_out.hpp" +#include "autoware/behavior_path_start_planner_module/shift_pull_out.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "autoware_behavior_path_start_planner_module/util.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_start_planner_module/util.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include #include #include +using autoware::motion_utils::findNearestIndex; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcOffsetPose; using lanelet::utils::getArcCoordinates; -using motion_utils::findNearestIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; namespace autoware::behavior_path_planner { using start_planner_utils::getPullOutLanes; @@ -107,9 +107,10 @@ std::optional ShiftPullOut::plan( // this ensures that the backward_path stays within the drivable area when starting from a // narrow place. - const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, - common_parameters.ego_nearest_yaw_threshold); + const size_t start_segment_idx = + autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes( lanelet_map_ptr, shift_path, start_segment_idx); @@ -124,15 +125,18 @@ std::optional ShiftPullOut::plan( if (cropped_path.points.size() < 2) return false; const double max_long_offset = parameters_.maximum_longitudinal_deviation; const size_t start_segment_idx_after_crop = - motion_utils::findFirstNearestIndexWithSoftConstraints(cropped_path.points, start_pose); + autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + cropped_path.points, start_pose); // if the start segment id after crop is not 0, then the cropping is not excessive if (start_segment_idx_after_crop != 0) return true; - const auto long_offset_to_closest_point = motion_utils::calcLongitudinalOffsetToSegment( - cropped_path.points, start_segment_idx_after_crop, start_pose.position); - const auto long_offset_to_next_point = motion_utils::calcLongitudinalOffsetToSegment( - cropped_path.points, start_segment_idx_after_crop + 1, start_pose.position); + const auto long_offset_to_closest_point = + autoware::motion_utils::calcLongitudinalOffsetToSegment( + cropped_path.points, start_segment_idx_after_crop, start_pose.position); + const auto long_offset_to_next_point = + autoware::motion_utils::calcLongitudinalOffsetToSegment( + cropped_path.points, start_segment_idx_after_crop + 1, start_pose.position); return std::abs(long_offset_to_closest_point - long_offset_to_next_point) < max_long_offset; }; @@ -171,7 +175,7 @@ bool ShiftPullOut::refineShiftedPathToStartPose( size_t iteration = 0; while (iteration < MAX_ITERATION) { const double lateral_offset = - motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position); + autoware::motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position); PathShifter path_shifter; path_shifter.setPath(shifted_path.path); @@ -193,7 +197,7 @@ bool ShiftPullOut::refineShiftedPathToStartPose( if (is_within_tolerance( lateral_offset, - motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position), + autoware::motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position), TOLERANCE)) { return true; } @@ -419,7 +423,8 @@ double ShiftPullOut::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; - for (const auto & [k, segment_length] : motion_utils::calcCurvatureAndArcLength(path.points)) { + for (const auto & [k, segment_length] : + autoware::motion_utils::calcCurvatureAndArcLength(path.points)) { // after shifted segment length const double after_segment_length = k < 0 ? segment_length * (1 - k * dr) : segment_length / (1 + k * dr); diff --git a/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp similarity index 95% rename from planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index cdf556b8363e0..9400f3ff40286 100644 --- a/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_start_planner_module/start_planner_module.hpp" +#include "autoware/behavior_path_start_planner_module/start_planner_module.hpp" -#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_start_planner_module/debug.hpp" -#include "autoware_behavior_path_start_planner_module/util.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_start_planner_module/debug.hpp" +#include "autoware/behavior_path_start_planner_module/util.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include #include @@ -41,9 +41,9 @@ using autoware::behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; -using motion_utils::calcLateralOffset; -using motion_utils::calcLongitudinalOffsetPose; -using tier4_autoware_utils::calcOffsetPose; +using autoware::motion_utils::calcLateralOffset; +using autoware::motion_utils::calcLongitudinalOffsetPose; +using autoware::universe_utils::calcOffsetPose; // set as macro so that calling function name will be printed. // debug print is heavy. turn on only when debugging. @@ -274,7 +274,7 @@ bool StartPlannerModule::hasFinishedBackwardDriving() const // check ego car is close enough to pull out start pose and stopped const auto current_pose = planner_data_->self_odometry->pose.pose; const auto distance = - tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); + autoware::universe_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); const bool is_near = distance < parameters_->th_arrived_distance; const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); @@ -383,7 +383,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { const double x = boundary_point.x(); const double y = boundary_point.y(); - boundary_path.push_back(tier4_autoware_utils::createPoint(x, y, 0.0)); + boundary_path.push_back(autoware::universe_utils::createPoint(x, y, 0.0)); }); return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); @@ -393,12 +393,12 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const const auto centerline_path = route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); const auto start_pose_nearest_segment_index = - motion_utils::findNearestSegmentIndex(centerline_path.points, start_pose); + autoware::motion_utils::findNearestSegmentIndex(centerline_path.points, start_pose); if (!start_pose_nearest_segment_index) return false; - const auto start_pose_point_msg = tier4_autoware_utils::createPoint( + const auto start_pose_point_msg = autoware::universe_utils::createPoint( start_pose.position.x, start_pose.position.y, start_pose.position.z); - const auto starting_pose_lateral_offset = motion_utils::calcLateralOffset( + const auto starting_pose_lateral_offset = autoware::motion_utils::calcLateralOffset( centerline_path.points, start_pose_point_msg, start_pose_nearest_segment_index.value()); if (std::isnan(starting_pose_lateral_offset)) return false; @@ -412,8 +412,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const geometry_msgs::msg::Pose & ego_overhang_point_as_pose, const bool ego_is_merging_from_the_left) -> std::optional> { const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - const auto vehicle_footprint = - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); + const auto vehicle_footprint = transformVector( + local_vehicle_footprint, autoware::universe_utils::pose2transform(current_pose)); double smallest_lateral_gap_between_ego_and_border = std::numeric_limits::max(); double corresponding_lateral_gap_with_other_lane_bound = std::numeric_limits::max(); @@ -505,7 +505,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const std::for_each( target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { - const auto arc_length = motion_utils::calcSignedArcLength( + const auto arc_length = autoware::motion_utils::calcSignedArcLength( centerline_path.points, current_pose.position, o.initial_pose.pose.position); if (arc_length > 0.0) return; if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; @@ -524,7 +524,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const bool StartPlannerModule::isCloseToOriginalStartPose() const { const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); - return tier4_autoware_utils::calcDistance2d( + return autoware::universe_utils::calcDistance2d( start_pose.position, planner_data_->self_odometry->pose.pose.position) > parameters_->th_arrived_distance; } @@ -532,7 +532,7 @@ bool StartPlannerModule::isCloseToOriginalStartPose() const bool StartPlannerModule::hasArrivedAtGoal() const { const Pose goal_pose = planner_data_->route_handler->getGoalPose(); - return tier4_autoware_utils::calcDistance2d( + return autoware::universe_utils::calcDistance2d( goal_pose.position, planner_data_->self_odometry->pose.pose.position) < parameters_->th_arrived_distance; } @@ -684,10 +684,10 @@ BehaviorModuleOutput StartPlannerModule::plan() const auto steering_factor_direction = getSteeringFactorDirection(output); if (status_.driving_forward) { - const double start_distance = motion_utils::calcSignedArcLength( + const double start_distance = autoware::motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); - const double finish_distance = motion_utils::calcSignedArcLength( + const double finish_distance = autoware::motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); @@ -698,7 +698,7 @@ BehaviorModuleOutput StartPlannerModule::plan() setDebugData(); return output; } - const double distance = motion_utils::calcSignedArcLength( + const double distance = autoware::motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); @@ -788,10 +788,10 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() const auto steering_factor_direction = getSteeringFactorDirection(output); if (status_.driving_forward) { - const double start_distance = motion_utils::calcSignedArcLength( + const double start_distance = autoware::motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); - const double finish_distance = motion_utils::calcSignedArcLength( + const double finish_distance = autoware::motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); @@ -803,7 +803,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() return output; } - const double distance = motion_utils::calcSignedArcLength( + const double distance = autoware::motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); @@ -888,20 +888,16 @@ PriorityOrder StartPlannerModule::determinePriorityOrder( order_priority.emplace_back(i, planner); } } - return order_priority; - } - - if (search_priority == "short_back_distance") { + } else if (search_priority == "short_back_distance") { for (size_t i = 0; i < start_pose_candidates_num; i++) { for (const auto & planner : start_planners_) { order_priority.emplace_back(i, planner); } } - return order_priority; + } else { + RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str()); + throw std::domain_error("[start_planner] invalid search_priority"); } - - RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str()); - throw std::domain_error("[start_planner] invalid search_priority"); return order_priority; } @@ -913,7 +909,7 @@ bool StartPlannerModule::findPullOutPath( // if start_pose_candidate is far from refined_start_pose, backward driving is necessary constexpr double epsilon = 0.01; const double backwards_distance = - tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose); + autoware::universe_utils::calcDistance2d(start_pose_candidate, refined_start_pose); const bool backward_is_unnecessary = backwards_distance < epsilon; planner->setCollisionCheckMargin(collision_check_margin); @@ -1240,7 +1236,7 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( bool StartPlannerModule::hasReachedFreespaceEnd() const { const auto & current_pose = planner_data_->self_odometry->pose.pose; - return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + return autoware::universe_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < parameters_->th_arrived_distance; } @@ -1282,8 +1278,8 @@ bool StartPlannerModule::hasFinishedCurrentPath() const auto current_path = getCurrentPath(); const auto current_path_end = current_path.points.back(); const auto self_pose = planner_data_->self_odometry->pose.pose; - const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < - parameters_->th_arrived_distance; + const bool is_near_target = autoware::universe_utils::calcDistance2d( + current_path_end, self_pose) < parameters_->th_arrived_distance; return is_near_target && isStopped(); } @@ -1294,10 +1290,10 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const auto shift_start_idx = - motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position); + const auto shift_start_idx = autoware::motion_utils::findNearestIndex( + path.points, status_.pull_out_path.start_pose.position); const auto shift_end_idx = - motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position); + autoware::motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position); const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); const auto is_ignore_signal = [this](const lanelet::Id & id) { @@ -1336,8 +1332,9 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() } constexpr double distance_threshold = 1.0; const auto stop_point = status_.pull_out_path.partial_paths.front().points.back(); - const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength( - path.points, stop_point.point.pose.position, current_pose.position)); + const double distance_from_ego_to_stop_point = + std::abs(autoware::motion_utils::calcSignedArcLength( + path.points, stop_point.point.pose.position, current_pose.position)); return distance_from_ego_to_stop_point < distance_threshold; }); @@ -1449,7 +1446,7 @@ bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const // Return true when the goal is located behind of ego. const auto ego_lane_path = rh->getCenterLinePath( lanelet::ConstLanelets{ego_lanelet}, 0.0, std::numeric_limits::max()); - const auto dist_ego_to_goal = motion_utils::calcSignedArcLength( + const auto dist_ego_to_goal = autoware::motion_utils::calcSignedArcLength( ego_lane_path.points, getEgoPosition(), rh->getGoalPose().position); const bool is_goal_behind_of_ego = (dist_ego_to_goal < 0.0); @@ -1562,6 +1559,9 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons void StartPlannerModule::setDebugData() { + using autoware::universe_utils::createDefaultMarker; + using autoware::universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerScale; using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::addFootprintMarker; using marker_utils::createFootprintMarkerArray; @@ -1572,9 +1572,6 @@ void StartPlannerModule::setDebugData() using marker_utils::showPolygon; using marker_utils::showPredictedPath; using marker_utils::showSafetyCheckInfo; - using tier4_autoware_utils::createDefaultMarker; - using tier4_autoware_utils::createMarkerColor; - using tier4_autoware_utils::createMarkerScale; using visualization_msgs::msg::Marker; const auto red_color = createMarkerColor(1.0, 0.0, 0.0, 0.999); @@ -1588,7 +1585,7 @@ void StartPlannerModule::setDebugData() for (auto & marker : added.markers) { marker.lifetime = life_time; } - tier4_autoware_utils::appendMarkerArray(added, &target_marker_array); + autoware::universe_utils::appendMarkerArray(added, &target_marker_array); }; debug_marker_.markers.clear(); @@ -1618,7 +1615,7 @@ void StartPlannerModule::setDebugData() {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}}; double collision_check_distance_from_end = collision_check_distances[status_.planner_type]; - const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + const auto collision_check_end_pose = autoware::motion_utils::calcLongitudinalOffsetPose( getFullPath().points, status_.pull_out_path.end_pose.position, collision_check_distance_from_end); if (collision_check_end_pose) { @@ -1672,10 +1669,10 @@ void StartPlannerModule::setDebugData() PathWithLaneId path_shift_start_to_end{}; const auto shift_path = status_.pull_out_path.partial_paths.front(); { - const size_t pull_out_start_idx = motion_utils::findNearestIndex( + const size_t pull_out_start_idx = autoware::motion_utils::findNearestIndex( shift_path.points, status_.pull_out_path.start_pose.position); - const size_t pull_out_end_idx = - motion_utils::findNearestIndex(shift_path.points, status_.pull_out_path.end_pose.position); + const size_t pull_out_end_idx = autoware::motion_utils::findNearestIndex( + shift_path.points, status_.pull_out_path.end_pose.position); path_shift_start_to_end.points.insert( path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, diff --git a/planning/autoware_behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp similarity index 86% rename from planning/autoware_behavior_path_start_planner_module/src/util.cpp rename to planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp index e8197fcf095cc..3fe9524e34a17 100644 --- a/planning/autoware_behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_start_planner_module/util.hpp" +#include "autoware/behavior_path_start_planner_module/util.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include #include -#include #include -#include #include @@ -65,7 +65,7 @@ PathWithLaneId getBackwardPath( const double lateral_distance_to_shoulder_center = current_pose_arc_coords.distance; for (size_t i = 0; i < backward_path.points.size(); ++i) { auto & p = backward_path.points.at(i).point.pose; - p = tier4_autoware_utils::calcOffsetPose(p, 0, lateral_distance_to_shoulder_center, 0); + p = autoware::universe_utils::calcOffsetPose(p, 0, lateral_distance_to_shoulder_center, 0); } } @@ -115,16 +115,16 @@ std::optional extractCollisionCheckSection( if (full_path.points.empty()) return std::nullopt; // Find the start index for collision check section based on the shift start pose const auto shift_start_idx = - motion_utils::findNearestIndex(full_path.points, path.start_pose.position); + autoware::motion_utils::findNearestIndex(full_path.points, path.start_pose.position); // Find the end index for collision check section based on the end pose and collision check // distance const auto collision_check_end_idx = [&]() -> size_t { - const auto end_pose_offset = motion_utils::calcLongitudinalOffsetPose( + const auto end_pose_offset = autoware::motion_utils::calcLongitudinalOffsetPose( full_path.points, path.end_pose.position, collision_check_distance_from_end); return end_pose_offset - ? motion_utils::findNearestIndex(full_path.points, end_pose_offset->position) + ? autoware::motion_utils::findNearestIndex(full_path.points, end_pose_offset->position) : full_path.points.size() - 1; // Use the last point if offset pose is not calculable }(); diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md similarity index 88% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index bc70c1fdd5c6a..33abc8eaf40ed 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -8,7 +8,7 @@ This is a rule-based avoidance module, which is running based on perception outp ![fig](./images/purpose/avoidance.png) -This module has [RTC interface](../autoware_rtc_interface/README.md), and user can select operation mode from MANUAL/AUTO depending on the vehicle sensor performance. If user selects MANUAL mode, this module outputs avoidance path as candidate and waits operator approval. In the case where the sensor/perception performance is not enough and false positive maybe occurs, we recommend to use this module with MANUAL mode in order to prevent unnecessary avoidance maneuver. +This module has [RTC interface](../../autoware_rtc_interface/README.md), and user can select operation mode from MANUAL/AUTO depending on the vehicle sensor performance. If user selects MANUAL mode, this module outputs avoidance path as candidate and waits operator approval. In the case where the sensor/perception performance is not enough and false positive maybe occurs, we recommend to use this module with MANUAL mode in order to prevent unnecessary avoidance maneuver. On the other hand, if user selects AUTO mode, this module modifies current following path without operator approval. If the sensor/perception performance is good enough, user can use this module with AUTO mode. @@ -83,7 +83,7 @@ partition fillEgoStatus() { note right This module has following status: - RUNNING: target object is still remaining. Or, the ego hasn't returned original lane. - - CANCEL: taget obejct has gone. And, the ego hasn't initiated avoidance maneuver. + - CANCEL: target object has gone. And, the ego hasn't initiated avoidance maneuver. - SUCCEEDED: the ego finishes avoiding all objects and returns original lane. end note @@ -122,7 +122,7 @@ if (Is there object that is potentially avoidable?) then (yes) :return true; note right Sometimes, we meet the situation where there is enough space to avoid - but ego speed is to high to avoid target obejct under lateral jerk constraints. + but ego speed is to high to avoid target object under lateral jerk constraints. This module keeps running in this case in order to decelerate ego speed. end note stop @@ -747,6 +747,59 @@ The longitudinal positions depends on envelope polygon, ego vehicle specificatio ![fig](./images/path_generation/margin.png) +### Lateral margin + +As mentioned above, user can adjust lateral margin by changing following two types parameter. The `soft_margin` is a soft constraint parameter for lateral margin. The `hard_margin` and `hard_margin_for_parked_vehicle` are hard constraint parameter. + +```yaml + car: + ... + lateral_margin: + soft_margin: 0.3 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] +``` + +Basically, this module tries to generate avoidance path in order to keep lateral distance, which is sum of `soft_margin` and `hard_margin`/`hard_margin_for_parked_vehicle`, from avoidance target object. + +![fig](./images/path_generation/soft_hard.png) + +But if there isn't enough space to keep `soft_margin` distance, this module shortens soft constraint lateral margin. The parameter `soft_margin` is a maximum value of soft constraint, and actual soft margin can be a value between 0.0 and `soft_margin`. On the other hand, this module definitely keeps `hard_margin` or `hard_margin_for_parked_vehicle` depending on the situation. Thus, the minimum value of total lateral margin is `hard_margin`/`hard_margin_for_parked_vehicle`, and the maximum value is the sum of `hard_margin`/`hard_margin_for_parked_vehicle` and `soft_margin`. + +Following figure shows the situation where this module shortens lateral soft constraint in order not to drive opposite direction lane when user set a parameter `use_opposite_lane` to `false`. + +![fig](./images/path_generation/adjust_margin.png) + +This module avoids not only parked vehicle but also non-parked vehicle which stops temporarily for some reason (e.g. waiting for traffic light to change red to green.). Additionally, this module has two types hard margin parameters, `hard_margin` and `hard_margin_for_parked_vehicle` and judges if it's a parked vehicle or not for each vehicles because it takes the risk of vehicle doors opening suddenly and people getting out from parked vehicle into consideration. + +Basically, user had better make `hard_margin_for_parked_vehicle` larger than `hard_margin` to prevent collision with doors or people who suddenly get out from vehicle. + +On the other hand, this module has only one parameter `soft_margin` for soft lateral margin constraint. + +![fig](./images/path_generation/hard_margin.png) + +As the hard margin parameters the distance which the user definitely want to keep, they are used in the logic to check whether the ego can pass side of the target object without avoidance maneuver as well. + +If the lateral distance is less than `hard_margin`/`hard_margin_for_parked_vehicle` when assuming that the ego follows current lane without avoidance maneuver, this module thinks the ego can not pass the side of the object safely and the ego must avoid it. In this case, this module inserts stop point until the avoidance maneuver is allowed to execute so that the ego can avoid the object after approval. (e.g. The ego keeps stopping in front of such a object until operator approves avoidance maneuver if user uses this module in MANUAL mode.) + +![fig](./images/path_generation/must_avoid.png) + +On the other hand, if the lateral distance is larger than `hard_margin`/`hard_margin_for_parked_vehicle`, this module doesn't insert stop point even when it's waiting approval because it thinks it's possible to pass the side of the object safely. + +![fig](./images/path_generation/pass_through.png) + +### When there is not enough space + +This module inserts stop point only when the ego can potentially avoid the object. So, if it is not able to keep distance more than `hard_margin`/`hard_margin_for_parked_vehicle`, this module does nothing. Following figure shows the situation where this module is not able to keep enough lateral distance when user set a parameter `use_opposite_lane` to `false`. + +![fig](./images/path_generation/do_nothing.png) + +!!! info + + In this situation, obstacle stop feature in [obstacle_cruise_planner](../../autoware_obstacle_cruise_planner/README.md) is responsible for ego vehicle safety. + +![fig](./images/path_generation/insufficient_drivable_space.png) + ### Shift length calculation The lateral shift length is sum of `overhang_distance`, lateral margin, whose value is set in config file, and the half of ego vehicle width defined in `vehicle_info.param.yaml`. On the other hand, the module limits the shift length depending on the space which the module can use for avoidance maneuver and the parameters `soft_drivable_bound_margin` `hard_drivable_bound_margin`. Basically, the shift length is limited so that the ego doesn't get closer than `soft_drivable_bound_margin` to drivable boundary. But it allows to relax the threshold `soft_drivable_bound_margin` to `hard_drivable_bound_margin` when the road is narrow. @@ -804,6 +857,34 @@ The `prepare_length` is calculated as the product of ego speed and `max_prepare_ ![fig](./images/path_generation/shift_line.png) +## Planning at RED traffic light + +This module takes traffic light information into account so that the ego can behave properly. Sometimes, the ego straddles lane boundary but we want to prevent the ego from stopping in front of red traffic signal in such a situation. This is because the ego will block adjacent lane and it's inconvenient for other vehicles. + +![fig](./images/traffic_light/traffic_light.png) + +So, this module controls shift length and shift start/end point in order to prevent above situation. + +### Control shift length + +At first, if the ego hasn't initiated avoidance maneuver yet, this module limits maximum shift length and uses **ONLY** current lane during red traffic signal. This prevents the ego from blocking other vehicles even if this module executes avoidance maneuver and the ego is caught by red traffic signal. + +![fig](./images/traffic_light/limit_shift_length.png) + +### Control avoidance shift start point + +Additionally, if the target object is farther than stop line for traffic light, this module set avoidance shift start point on the stop line in order to prevent the ego from stopping by red traffic signal in middle of avoidance maneuver. + +![fig](./images/traffic_light/shift_from_current_pos.png) +![fig](./images/traffic_light/shift_from_stop_line.png) + +### Control return shift end point + +If the ego has already initiated avoidance maneuver, this module tries to set return shift end point on the stop line. + +![fig](./images/traffic_light/return_after_stop_line.png) +![fig](./images/traffic_light/return_before_stop_line.png) + ## Safety check This feature can be enable by setting following parameter to `true`. @@ -915,15 +996,6 @@ use_freespace_areas: true ## Future extensions / Unimplemented parts -- **Planning on the intersection** - - If it is known that the ego vehicle is going to stop in the middle of avoidance execution (for example, at a red traffic light), sometimes the avoidance should not be executed until the vehicle is ready to move. This is because it is impossible to predict how the environment will change during the stop. This is especially important at intersections. - -![fig](./images/intersection_problem.drawio.svg) - -- **Safety Check** - - - In the current implementation, it is only the jerk limit that permits the avoidance execution. It is needed to consider the collision with other vehicles when change the path shape. - - **Consideration of the speed of the avoidance target** - In the current implementation, only stopped vehicle is targeted as an avoidance target. It is needed to support the overtaking function for low-speed vehicles, such as a bicycle. (It is actually possible to overtake the low-speed objects by changing the parameter, but the logic is not supported and thus the safety cannot be guaranteed.) @@ -1146,4 +1218,4 @@ The shift points are modified by a filtering process in order to get the expecte The avoidance specific parameter configuration file can be located at `src/autoware/launcher/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml`. -{{ json_to_markdown("planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json") }} +{{ json_to_markdown("planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json") }} diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml similarity index 92% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 884812d0db716..324be9a73439e 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -5,10 +5,6 @@ resample_interval_for_planning: 0.3 # [m] FOR DEVELOPER resample_interval_for_output: 4.0 # [m] FOR DEVELOPER - # avoidance module common setting - enable_bound_clipping: false - disable_path_update: false - # drivable area setting use_adjacent_lane: true use_opposite_lane: true @@ -284,6 +280,19 @@ max_acceleration: 0.5 # [m/ss] min_velocity_to_limit_max_acceleration: 2.78 # [m/ss] + # path generation method. select "shift_line_base" or "optimization_base" or "both". + # "shift_line_base" : Create avoidance path based on shift line. + # User can control avoidance maneuver execution via RTC. + # However, this method doesn't support complex avoidance scenario (e.g. S-shape maneuver). + # "optimization_base": This module selects avoidance target object + # and bpp module clips drivable area based on avoidance target object polygon shape. + # But this module doesn't modify the path shape. + # On the other hand, autoware_path_optimizer module optimizes path shape instead of this module + # so that the path can be within drivable area. This method is able to deal with complex avoidance scenario. + # However, user can't control avoidance manuever execution. + # "both" : Use both method. + path_generation_method: "shift_line_base" + shift_line_pipeline: trim: quantize_size: 0.1 diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_freespace.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_freespace.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_freespace.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_freespace.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_intersection.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_intersection.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_intersection.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_intersection.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_opposite_direction.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_opposite_direction.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_opposite_direction.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_opposite_direction.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_same_direction.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_same_direction.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_same_direction.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_same_direction.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_zebra.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_zebra.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_zebra.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_zebra.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_debug_message_array.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_debug_message_array.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_debug_message_array.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_debug_message_array.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/cancel/cancel.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/cancel/cancel.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/cancel/cancel.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/cancel/cancel.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/debug/debug_marker_rviz.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/debug/debug_marker_rviz.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/debug/debug_marker_rviz.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/debug/debug_marker_rviz.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/adjust_margin.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/adjust_margin.png new file mode 100644 index 0000000000000..ccb1f8c671234 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/adjust_margin.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/do_nothing.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/do_nothing.png new file mode 100644 index 0000000000000..9301382dcd093 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/do_nothing.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon_rviz.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon_rviz.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon_rviz.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon_rviz.png diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/hard_margin.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/hard_margin.png new file mode 100644 index 0000000000000..5578d8443044f Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/hard_margin.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/insufficient_drivable_space.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/insufficient_drivable_space.png new file mode 100644 index 0000000000000..18ac89f6dceef Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/insufficient_drivable_space.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/lateral.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/lateral.png new file mode 100644 index 0000000000000..4ca0627274ba6 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/lateral.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/margin.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/margin.png new file mode 100644 index 0000000000000..e5eba89ac7f8c Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/margin.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/must_avoid.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/must_avoid.png new file mode 100644 index 0000000000000..95ba285fb449a Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/must_avoid.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/opposite_direction.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/opposite_direction.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/opposite_direction.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/opposite_direction.png diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/pass_through.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/pass_through.png new file mode 100644 index 0000000000000..19b5cfaa07626 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/pass_through.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/polygon_update.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/polygon_update.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/polygon_update.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/polygon_update.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/same_direction.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/same_direction.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/same_direction.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/same_direction.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/shift_line.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/shift_line.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/shift_line.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/shift_line.png diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/soft_hard.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/soft_hard.png new file mode 100644 index 0000000000000..a92b819b94d1b Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/soft_hard.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/avoidance.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/avoidance.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/avoidance.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/avoidance.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/rviz.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/rviz.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/rviz.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/rviz.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/not_stop.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/not_stop.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/not_stop.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/not_stop.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/safety_check_flow.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/safety_check_flow.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/safety_check_flow.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/safety_check_flow.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/stop.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/stop.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/stop.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/stop.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_length_parameters.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_length_parameters.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_length_parameters.drawio.svg rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_length_parameters.drawio.svg diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_deviating.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_deviating.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_deviating.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_deviating.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_merging.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_merging.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_merging.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_merging.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_parallel.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_parallel.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_parallel.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_parallel.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_not_on_ego_lane.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_not_on_ego_lane.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_not_on_ego_lane.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_not_on_ego_lane.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_on_ego_lane.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_on_ego_lane.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_on_ego_lane.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_on_ego_lane.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/crosswalk.svg b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/crosswalk.svg similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/crosswalk.svg rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/crosswalk.svg diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area.svg b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area.svg similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area.svg rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area.svg diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area_rviz.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area_rviz.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area_rviz.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area_rviz.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/deviating.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/deviating.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/deviating.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/deviating.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/merging.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/merging.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/merging.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/merging.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_deviating.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_deviating.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_deviating.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_deviating.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_intersection.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_intersection.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_intersection.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_intersection.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_merging.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_merging.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_merging.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_merging.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_not_edge.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_not_edge.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_not_edge.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_not_edge.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_stop_factor.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_stop_factor.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_stop_factor.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_stop_factor.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/none.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/none.png similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/none.png rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/none.png diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/parked_vehicle.svg b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/parked_vehicle.svg similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/parked_vehicle.svg rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/parked_vehicle.svg diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/limit_shift_length.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/limit_shift_length.png new file mode 100644 index 0000000000000..0cb9eb893943c Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/limit_shift_length.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/return_after_stop_line.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/return_after_stop_line.png new file mode 100644 index 0000000000000..1e2b0ea95d3b9 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/return_after_stop_line.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/return_before_stop_line.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/return_before_stop_line.png new file mode 100644 index 0000000000000..7e0bdd704ce58 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/return_before_stop_line.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/shift_from_current_pos.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/shift_from_current_pos.png new file mode 100644 index 0000000000000..bcc1298670eba Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/shift_from_current_pos.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/shift_from_stop_line.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/shift_from_stop_line.png new file mode 100644 index 0000000000000..0830f516afa7f Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/shift_from_stop_line.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/traffic_light.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/traffic_light.png new file mode 100644 index 0000000000000..ae566bf833484 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/traffic_light/traffic_light.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp similarity index 96% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index abbdbfdc2f1e6..f05230f4ec19d 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include @@ -120,9 +120,6 @@ struct AvoidanceParameters // enable yield maneuver. bool enable_yield_maneuver_during_shifting{false}; - // disable path update - bool disable_path_update{false}; - // use hatched road markings for avoidance bool use_hatched_road_markings{false}; @@ -313,6 +310,9 @@ struct AvoidanceParameters // policy std::string policy_lateral_margin{"best_effort"}; + // path generation method. + std::string path_generation_method{"shift_line_base"}; + // target velocity matrix std::vector velocity_map; @@ -334,9 +334,6 @@ struct AvoidanceParameters // rss parameters utils::path_safety_checker::RSSparams rss_params{}; - // clip left and right bounds for objects - bool enable_bound_clipping{false}; - // debug bool enable_other_objects_marker{false}; bool enable_other_objects_info{false}; @@ -438,6 +435,9 @@ struct ObjectData // avoidance target // is ambiguous stopped vehicle. bool is_ambiguous{false}; + // is clip targe. + bool is_clip_target{false}; + // object direction. Direction direction{Direction::NONE}; @@ -673,4 +673,4 @@ struct DebugData } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp similarity index 86% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp index 043da5cbaf8cc..316501fbbd37f 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ -#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include #include @@ -60,4 +60,4 @@ std::string toStrInfo(const autoware::behavior_path_planner::ShiftLine & sl); std::string toStrInfo(const autoware::behavior_path_planner::AvoidLine & ap); -#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp similarity index 93% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp index 5d1dc168f6efe..8c46affbc64e3 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" #include #include @@ -177,14 +177,14 @@ class AvoidanceHelper double getShift(const Point & p) const { validate(); - const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p); + const auto idx = autoware::motion_utils::findNearestIndex(prev_reference_path_.points, p); return prev_spline_shift_path_.shift_length.at(idx); } double getLinearShift(const Point & p) const { validate(); - const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p); + const auto idx = autoware::motion_utils::findNearestIndex(prev_reference_path_.points, p); return prev_linear_shift_path_.shift_length.at(idx); } @@ -278,7 +278,7 @@ class AvoidanceHelper } const auto start_idx = data_->findEgoIndex(path.points); - const auto distance = motion_utils::calcSignedArcLength( + const auto distance = autoware::motion_utils::calcSignedArcLength( path.points, start_idx, max_v_point_.value().first.position); return std::make_pair(distance, max_v_point_.value().second); } @@ -290,7 +290,7 @@ class AvoidanceHelper const auto & a_now = data_->self_acceleration->accel.accel.linear.x; const auto & a_lim = use_hard_constraints ? p->max_deceleration : p->nominal_deceleration; const auto & j_lim = use_hard_constraints ? p->max_jerk : p->nominal_jerk; - const auto ret = motion_utils::calcDecelDistWithJerkAndAccConstraints( + const auto ret = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim); if (!!ret) { @@ -449,15 +449,15 @@ class AvoidanceHelper const auto x_max_accel = v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0; - const auto point = motion_utils::calcLongitudinalOffsetPose( + const auto point = autoware::motion_utils::calcLongitudinalOffsetPose( path.points, getEgoPosition(), x_neg_jerk + x_max_accel); if (point.has_value()) { max_v_point_ = std::make_pair(point.value(), v_max); return; } - const auto x_end = - motion_utils::calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1); + const auto x_end = autoware::motion_utils::calcSignedArcLength( + path.points, getEgoPosition(), path.points.size() - 1); const auto t_end = (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration; const auto v_end = v0 + p->max_acceleration * t_end; @@ -520,4 +520,4 @@ class AvoidanceHelper }; } // namespace autoware::behavior_path_planner::helper::static_obstacle_avoidance -#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp similarity index 77% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp index f3acf0c5f3abc..895390f91cc16 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp" #include #include @@ -52,4 +52,4 @@ class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp similarity index 96% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 351c686f761d7..b761939589968 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -11,12 +11,12 @@ // 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 AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ -#include "tier4_autoware_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" -#include +#include #include #include @@ -26,8 +26,8 @@ namespace autoware::behavior_path_planner { +using autoware::universe_utils::getOrDeclareParameter; using autoware_perception_msgs::msg::ObjectClassification; -using tier4_autoware_utils::getOrDeclareParameter; AvoidanceParameters getParameter(rclcpp::Node * node) { @@ -39,8 +39,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = getOrDeclareParameter(*node, ns + "resample_interval_for_output"); - p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); - p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); + p.path_generation_method = + getOrDeclareParameter(*node, ns + "path_generation_method"); } // drivable area @@ -403,4 +403,4 @@ AvoidanceParameters getParameter(rclcpp::Node * node) } } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp similarity index 93% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index ccd00078bdec1..635edb7c84f40 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ -#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include #include @@ -115,8 +115,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const auto ego_idx = planner_data_->findEgoIndex(path.points); for (const auto & left_shift : left_shift_array_) { - const double start_distance = - motion_utils::calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position); + const double start_distance = autoware::motion_utils::calcSignedArcLength( + path.points, ego_idx, left_shift.start_pose.position); const double finish_distance = start_distance + left_shift.relative_longitudinal; rtc_interface_ptr_map_.at("left")->updateCooperateStatus( left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); @@ -128,8 +128,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } for (const auto & right_shift : right_shift_array_) { - const double start_distance = - motion_utils::calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position); + const double start_distance = autoware::motion_utils::calcSignedArcLength( + path.points, ego_idx, right_shift.start_pose.position); const double finish_distance = start_distance + right_shift.relative_longitudinal; rtc_interface_ptr_map_.at("right")->updateCooperateStatus( right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); @@ -436,6 +436,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface UUID candidate_uuid_; + ObjectDataArray clip_objects_; + // TODO(Satoshi OTA) create detected object manager. ObjectDataArray registered_objects_; @@ -458,4 +460,4 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp similarity index 93% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp index 8ca24db8042e7..671f9d6333a0c 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ -#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" +#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include @@ -244,4 +244,4 @@ class ShiftLineGenerator } // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance -#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp similarity index 55% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp index 8e4cab52a7419..cfbd3f89308ac 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -53,22 +53,22 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_rtc_msgs::msg::State; // tier4 utils functions -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::calcYawDeviation; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::getPoint; -using tier4_autoware_utils::getPose; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; -using tier4_autoware_utils::pose2transform; -using tier4_autoware_utils::toHexString; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::calcDistance2d; +using autoware::universe_utils::calcLateralDeviation; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::calcYawDeviation; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::getPoint; +using autoware::universe_utils::getPose; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::pose2transform; +using autoware::universe_utils::toHexString; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp similarity index 93% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp index dc061457f7045..1f68ef7d49c47 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ -#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ -#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include #include @@ -121,6 +121,8 @@ void updateRegisteredObject( ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, const std::shared_ptr & parameters); +void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data); + void compensateDetectionLost( const ObjectDataArray & registered_objects, ObjectDataArray & now_objects, ObjectDataArray & other_objects); @@ -177,4 +179,4 @@ double calcDistanceToAvoidStartLine( } // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance -#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml similarity index 96% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index a6da4d5550562..ea6d405f80f41 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -23,15 +23,16 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_route_handler autoware_rtc_interface + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs lanelet2_extension magic_enum - motion_utils pluginlib rclcpp sensor_msgs @@ -40,7 +41,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json similarity index 99% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index cb150514ac372..3e6f0ef1f61a3 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -16,15 +16,10 @@ "description": "Path resample interval for output path. Too short interval increases computational cost for latter modules.", "default": "4.0" }, - "enable_bound_clipping": { - "type": "boolean", - "description": "Enable clipping left and right bound of drivable area when obstacles are in the drivable area.", - "default": "false" - }, - "disable_path_update": { - "type": "boolean", - "description": "Disable path update.", - "default": "false" + "path_generation_method": { + "enum": ["shift_line_base", "optimization_base", "both"], + "description": "Path generation method.", + "default": "shift_line_base" }, "use_adjacent_lane": { "type": "boolean", @@ -1445,7 +1440,7 @@ "required": [ "resample_interval_for_planning", "resample_interval_for_output", - "enable_bound_clipping", + "path_generation_method", "use_adjacent_lane", "use_opposite_lane", "use_hatched_road_markings", diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp similarity index 97% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index a495213626e22..bf5e3ac1714cf 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp" -#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" #include #include @@ -147,6 +147,7 @@ MarkerArray createObjectInfoMarkerArray( string_stream << std::fixed << std::setprecision(2) << std::boolalpha; string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" << "lateral:" << object.to_centerline << " [m]\n" + << "clip:" << object.is_clip_target << " [-]\n" << "necessity:" << object.avoid_required << " [-]\n" << "stoppable:" << object.is_stoppable << " [-]\n" << "stop_factor:" << object.to_stop_factor_distance << " [m]\n" @@ -573,55 +574,45 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE); } - // shift line pre-process if (parameters->enable_shift_line_marker) { + // shift line pre-process addAvoidLine(debug.step1_registered_shift_line, "step1_registered_shift_line", 0.2, 0.2, 1.0); addAvoidLine(debug.step1_current_shift_line, "step1_current_shift_line", 0.2, 0.4, 0.8, 0.3); addAvoidLine(debug.step1_merged_shift_line, "step1_merged_shift_line", 0.2, 0.6, 0.6, 0.3); addAvoidLine(debug.step1_filled_shift_line, "step1_filled_shift_line", 0.2, 0.8, 0.4, 0.3); addAvoidLine(debug.step1_return_shift_line, "step1_return_shift_line", 0.2, 1.0, 0.2, 0.3); - } - // merge process - if (parameters->enable_shift_line_marker) { + // merge process addAvoidLine(debug.step2_merged_shift_line, "step2_merged_shift_line", 0.2, 1.0, 0.0, 0.3); - } - // trimming process - if (parameters->enable_shift_line_marker) { + // trimming process addAvoidLine(debug.step3_grad_filtered_1st, "step3_grad_filtered_1st", 0.2, 0.8, 0.0, 0.3); addAvoidLine(debug.step3_grad_filtered_2nd, "step3_grad_filtered_2nd", 0.4, 0.6, 0.0, 0.3); addAvoidLine(debug.step3_grad_filtered_3rd, "step3_grad_filtered_3rd", 0.6, 0.4, 0.0, 0.3); - } - // registering process - if (parameters->enable_shift_line_marker) { + // registering process addShiftLine(shifter.getShiftLines(), "step4_old_shift_line", 1.0, 1.0, 0.0, 0.3); addAvoidLine(data.new_shift_line, "step4_new_shift_line", 1.0, 0.0, 0.0, 0.3); - } - - // safety check - if (parameters->enable_safety_check_marker) { - add(showSafetyCheckInfo(debug.collision_check, "object_debug_info")); - add(showPredictedPath(debug.collision_check, "ego_predicted_path")); - add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation")); - } - // shift length - if (parameters->enable_shift_line_marker) { + // shift length addShiftLength(debug.pos_shift, "merged_length_pos", 0.0, 0.7, 0.5); addShiftLength(debug.neg_shift, "merged_length_neg", 0.0, 0.5, 0.7); addShiftLength(debug.total_shift, "merged_length_total", 0.99, 0.4, 0.2); - } - // shift grad - if (parameters->enable_shift_line_marker) { + // shift grad addShiftGrad(debug.pos_shift_grad, debug.pos_shift, "merged_grad_pos", 0.0, 0.7, 0.5); addShiftGrad(debug.neg_shift_grad, debug.neg_shift, "merged_grad_neg", 0.0, 0.5, 0.7); addShiftGrad(debug.total_forward_grad, debug.total_shift, "grad_forward", 0.99, 0.4, 0.2); addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9); } + // safety check + if (parameters->enable_safety_check_marker) { + add(showSafetyCheckInfo(debug.collision_check, "object_debug_info")); + add(showPredictedPath(debug.collision_check, "ego_predicted_path")); + add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation")); + } + // detection area if (parameters->enable_detection_area_marker) { size_t i = 0; @@ -691,7 +682,7 @@ std::string toStrInfo(const autoware::behavior_path_planner::AvoidLineArray & ap } std::string toStrInfo(const autoware::behavior_path_planner::AvoidLine & ap) { - using tier4_autoware_utils::toHexString; + using autoware::universe_utils::toHexString; std::stringstream pids; for (const auto pid : ap.parent_ids) { diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp similarity index 97% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index b59af0fbbf0e5..2d02f33e19870 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" #include @@ -28,8 +28,8 @@ namespace autoware::behavior_path_planner { void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) { + using autoware::universe_utils::getOrDeclareParameter; using autoware_perception_msgs::msg::ObjectClassification; - using tier4_autoware_utils::getOrDeclareParameter; // init manager interface initInterface(node, {"left", "right"}); @@ -42,8 +42,8 @@ void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) void StaticObstacleAvoidanceModuleManager::updateModuleParams( const std::vector & parameters) { + using autoware::universe_utils::updateParam; using autoware_perception_msgs::msg::ObjectClassification; - using tier4_autoware_utils::updateParam; auto p = parameters_; diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp similarity index 96% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index b69508c9fdcef..b0fd1b5c1b8ab 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp" - -#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp" + +#include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" #include #include @@ -278,7 +278,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( // arclength from ego pose (used in many functions) data.arclength_from_ego = utils::calcPathArcLengthArray( data.reference_path, 0, data.reference_path.points.size(), - motion_utils::calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); + autoware::motion_utils::calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); data.to_return_point = utils::static_obstacle_avoidance::calcDistanceToReturnDeadLine( data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); @@ -294,6 +294,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( registered_objects_, data.target_objects, parameters_); utils::static_obstacle_avoidance::compensateDetectionLost( registered_objects_, data.target_objects, data.other_objects); + utils::static_obstacle_avoidance::updateClipObject(clip_objects_, data); // sort object order by longitudinal distance std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) { @@ -378,7 +379,7 @@ ObjectData StaticObstacleAvoidanceModule::createObjectData( const auto & path_points = data.reference_path.points; const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = - motion_utils::findNearestIndex(path_points, object_pose.position); + autoware::motion_utils::findNearestIndex(path_points, object_pose.position); const auto object_closest_pose = path_points.at(object_closest_index).point.pose; const auto object_type = utils::getHighestProbLabel(object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); @@ -424,7 +425,7 @@ bool StaticObstacleAvoidanceModule::canYieldManeuver(const AvoidancePlanningData const auto registered_lines = path_shifter_.getShiftLines(); if (!registered_lines.empty()) { const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); - const auto prepare_distance = motion_utils::calcSignedArcLength( + const auto prepare_distance = autoware::motion_utils::calcSignedArcLength( path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); if (!helper_->isEnoughPrepareDistance(prepare_distance)) { RCLCPP_DEBUG( @@ -642,7 +643,7 @@ void StaticObstacleAvoidanceModule::fillDebugData( const auto prepare_distance = helper_->getNominalPrepareDistance(); const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance; - dead_pose_ = motion_utils::calcLongitudinalOffsetPose( + dead_pose_ = autoware::motion_utils::calcLongitudinalOffsetPose( data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); if (!dead_pose_) { @@ -653,7 +654,7 @@ void StaticObstacleAvoidanceModule::fillDebugData( void StaticObstacleAvoidanceModule::updateEgoBehavior( const AvoidancePlanningData & data, ShiftedPath & path) { - if (parameters_->disable_path_update) { + if (parameters_->path_generation_method == "optimization_base") { return; } @@ -755,7 +756,7 @@ bool StaticObstacleAvoidanceModule::isSafePath( auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); const auto obj_polygon = - tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); + autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape); const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); @@ -812,7 +813,7 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( return a.start_idx < b.start_idx; }); return std::max( - max_dist, motion_utils::calcSignedArcLength( + max_dist, autoware::motion_utils::calcSignedArcLength( previous_path.points, lines.front().start.position, getEgoPosition())); }(); @@ -821,7 +822,7 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( planner_data_->parameters.backward_path_length, longest_dist_to_shift_point + extra_margin); const size_t orig_ego_idx = planner_data_->findEgoIndex(original_path.points); - const auto prev_ego_idx = motion_utils::findNearestSegmentIndex( + const auto prev_ego_idx = autoware::motion_utils::findNearestSegmentIndex( previous_path.points, getPose(original_path.points.at(orig_ego_idx)), std::numeric_limits::max(), planner_data_->parameters.ego_nearest_yaw_threshold); if (!prev_ego_idx) { @@ -832,7 +833,7 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( for (size_t i = 0; i < prev_ego_idx; ++i) { if ( backward_length > - motion_utils::calcSignedArcLength(previous_path.points, clip_idx, *prev_ego_idx)) { + autoware::motion_utils::calcSignedArcLength(previous_path.points, clip_idx, *prev_ego_idx)) { break; } clip_idx = i; @@ -925,7 +926,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() constexpr double threshold = 1.0; const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); const auto lateral_deviation = - motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx); + autoware::motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx); return std::abs(lateral_deviation) > threshold; }; @@ -998,19 +999,14 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() // expand freespace areas current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas; // generate obstacle polygons - if (parameters_->enable_bound_clipping) { - ObjectDataArray clip_objects; - // If avoidance is executed by both behavior and motion, only non-avoidable object will be - // extracted from the drivable area. - std::for_each( - data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { - if (!object.is_avoidable) clip_objects.push_back(object); - }); + current_drivable_area_info.obstacles.clear(); + + if ( + parameters_->path_generation_method == "optimization_base" || + parameters_->path_generation_method == "both") { current_drivable_area_info.obstacles = utils::static_obstacle_avoidance::generateObstaclePolygonsForDrivableArea( - clip_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); - } else { - current_drivable_area_info.obstacles.clear(); + clip_objects_, parameters_, planner_data_->parameters.vehicle_width / 2.0); } output.drivable_area_info = utils::combineDrivableAreaInfo( @@ -1077,7 +1073,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::planWaitingApproval() void StaticObstacleAvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) { - if (parameters_->disable_path_update) { + if (parameters_->path_generation_method == "optimization_base") { return; } @@ -1471,7 +1467,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( // Consider the difference in path length between the shifted path and original path (the path // that is shifted inward has a shorter distance to the end of the path than the other one.) const auto & to_reference_path_end = data.arclength_from_ego.back(); - const auto to_shifted_path_end = motion_utils::calcSignedArcLength( + const auto to_shifted_path_end = autoware::motion_utils::calcSignedArcLength( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); @@ -1509,7 +1505,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { const auto distance_from_ego = - motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); + autoware::motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); // slow down speed is inserted only in front of the object. const auto shift_longitudinal_distance = to_stop_line - distance_from_ego; @@ -1602,8 +1598,8 @@ void StaticObstacleAvoidanceModule::insertStopPoint( return shifted_path.path.points.size() - 1; }(); - const auto stop_distance = - motion_utils::calcSignedArcLength(shifted_path.path.points, getEgoPosition(), stop_idx); + const auto stop_distance = autoware::motion_utils::calcSignedArcLength( + shifted_path.path.points, getEgoPosition(), stop_idx); // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately @@ -1715,7 +1711,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { const auto distance_from_ego = - motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); + autoware::motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); // slow down speed is inserted only in front of the object. const auto shift_longitudinal_distance = distance_to_object - distance_from_ego; @@ -1732,7 +1728,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert); } - slow_pose_ = motion_utils::calcLongitudinalOffsetPose( + slow_pose_ = autoware::motion_utils::calcLongitudinalOffsetPose( shifted_path.path.points, start_idx, distance_to_object); } @@ -1759,7 +1755,7 @@ void StaticObstacleAvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifte const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { const auto distance_from_ego = - motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); + autoware::motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); // slow down speed is inserted only in front of the object. const auto accel_distance = distance_to_accel_end_point - distance_from_ego; @@ -1779,7 +1775,7 @@ void StaticObstacleAvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifte shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_target); } - slow_pose_ = motion_utils::calcLongitudinalOffsetPose( + slow_pose_ = autoware::motion_utils::calcLongitudinalOffsetPose( shifted_path.path.points, start_idx, distance_to_accel_end_point); } diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp similarity index 98% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index cdb89daad8adc..a171ae9161041 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" -#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance { @@ -355,8 +355,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( return false; } const auto goal_pose = data_->route_handler->getGoalPose(); - const double goal_longitudinal_distance = - motion_utils::calcSignedArcLength(data.reference_path.points, 0, goal_pose.position); + const double goal_longitudinal_distance = autoware::motion_utils::calcSignedArcLength( + data.reference_path.points, 0, goal_pose.position); const bool is_return_shift_to_goal = std::abs(al_return.end_longitudinal - goal_longitudinal_distance) < parameters_->object_check_return_pose_distance; @@ -365,7 +365,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position; const bool has_object_near_goal = - tier4_autoware_utils::calcDistance2d(goal_pose.position, object_pos) < + autoware::universe_utils::calcDistance2d(goal_pose.position, object_pos) < parameters_->object_check_goal_distance; return has_object_near_goal; }(); @@ -1026,7 +1026,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( if (utils::isAllowedGoalModification(data_->route_handler)) { const auto has_object_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { - return tier4_autoware_utils::calcDistance2d( + return autoware::universe_utils::calcDistance2d( data_->route_handler->getGoalPose().position, o.object.kinematics.initial_pose_with_covariance.pose.position) < parameters_->object_check_goal_distance; @@ -1097,7 +1097,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( if (utils::isAllowedGoalModification(data_->route_handler)) { const bool has_last_shift_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { - return tier4_autoware_utils::calcDistance2d( + return autoware::universe_utils::calcDistance2d( last_sl.end.position, o.object.kinematics.initial_pose_with_covariance.pose.position) < parameters_->object_check_goal_distance; diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp similarity index 94% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index d04a0e0281cd2..70de9e9499917 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" #include @@ -55,7 +55,8 @@ geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const return p; } -geometry_msgs::msg::Polygon toMsg(const tier4_autoware_utils::Polygon2d & polygon, const double z) +geometry_msgs::msg::Polygon toMsg( + const autoware::universe_utils::Polygon2d & polygon, const double z) { geometry_msgs::msg::Polygon ret; for (const auto & p : polygon.outer()) { @@ -67,14 +68,14 @@ geometry_msgs::msg::Polygon toMsg(const tier4_autoware_utils::Polygon2d & polygo template size_t findFirstNearestIndex(const T & points, const geometry_msgs::msg::Point & point) { - motion_utils::validateNonEmpty(points); + autoware::motion_utils::validateNonEmpty(points); double min_dist = std::numeric_limits::max(); size_t min_idx = 0; bool decreasing = false; for (size_t i = 0; i < points.size(); ++i) { - const auto dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), point); + const auto dist = autoware::universe_utils::calcSquaredDistance2d(points.at(i), point); if (dist < min_dist) { decreasing = true; min_dist = dist; @@ -103,7 +104,7 @@ size_t findFirstNearestSegmentIndex(const T & points, const geometry_msgs::msg:: } const double signed_length = - motion_utils::calcLongitudinalOffsetToSegment(points, nearest_idx, point); + autoware::motion_utils::calcLongitudinalOffsetToSegment(points, nearest_idx, point); if (signed_length <= 0) { return nearest_idx - 1; @@ -118,7 +119,7 @@ double calcSignedArcLengthToFirstNearestPoint( const geometry_msgs::msg::Point & dst_point) { try { - motion_utils::validateNonEmpty(points); + autoware::motion_utils::validateNonEmpty(points); } catch (const std::exception & e) { std::cerr << e.what() << std::endl; return 0.0; @@ -128,11 +129,11 @@ double calcSignedArcLengthToFirstNearestPoint( const size_t dst_seg_idx = findFirstNearestSegmentIndex(points, dst_point); const double signed_length_on_traj = - motion_utils::calcSignedArcLength(points, src_seg_idx, dst_seg_idx); + autoware::motion_utils::calcSignedArcLength(points, src_seg_idx, dst_seg_idx); const double signed_length_src_offset = - motion_utils::calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + autoware::motion_utils::calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); const double signed_length_dst_offset = - motion_utils::calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + autoware::motion_utils::calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } @@ -328,7 +329,7 @@ bool isWithinIntersection( return false; } - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object.object); const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); @@ -645,7 +646,7 @@ bool isNeverAvoidanceTarget( return true; } - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object.object); const auto is_disjoint_right_lane = boost::geometry::disjoint(object_polygon, right_lane.value().polygon2d().basicPolygon()); if (is_disjoint_right_lane) { @@ -668,7 +669,7 @@ bool isNeverAvoidanceTarget( return true; } - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object.object); const auto is_disjoint_left_lane = boost::geometry::disjoint(object_polygon, left_lane.value().polygon2d().basicPolygon()); if (is_disjoint_left_lane) { @@ -758,7 +759,7 @@ bool isSatisfiedWithCommonCondition( const auto ego_idx = planner_data->findEgoIndex(data.reference_path_rough.points); const auto to_goal_distance = rh->isInGoalRouteSection(data.current_lanelets.back()) - ? motion_utils::calcSignedArcLength( + ? autoware::motion_utils::calcSignedArcLength( data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1) : std::numeric_limits::max(); @@ -940,12 +941,12 @@ double getRoadShoulderDistance( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data) { + using autoware::universe_utils::Point2d; using lanelet::utils::to2D; - using tier4_autoware_utils::Point2d; const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = - motion_utils::findNearestIndex(data.reference_path.points, object_pose.position); + autoware::motion_utils::findNearestIndex(data.reference_path.points, object_pose.position); const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; const auto rh = planner_data->route_handler; @@ -968,7 +969,7 @@ double getRoadShoulderDistance( const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; const auto opt_intersect = - tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + autoware::universe_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); if (opt_intersect.has_value()) { intersects.emplace_back( @@ -986,7 +987,7 @@ double getRoadShoulderDistance( calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0) .position; const auto opt_intersect = - tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + autoware::universe_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); if (opt_intersect.has_value()) { intersects.emplace_back( @@ -1035,8 +1036,8 @@ bool isWithinLanes( { const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto transform = tier4_autoware_utils::pose2transform(ego_pose); - const auto footprint = tier4_autoware_utils::transformVector( + const auto transform = autoware::universe_utils::pose2transform(ego_pose); + const auto footprint = autoware::universe_utils::transformVector( planner_data->parameters.vehicle_info.createFootprint(), transform); lanelet::ConstLanelet closest_lanelet{}; @@ -1192,9 +1193,10 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( double min_distance = std::numeric_limits::max(); double max_distance = std::numeric_limits::lowest(); for (const auto & p : obj.envelope_poly.outer()) { - const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware::universe_utils::createPoint(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. - const double arc_length = motion_utils::calcSignedArcLength(path.points, ego_pos, point); + const double arc_length = + autoware::motion_utils::calcSignedArcLength(path.points, ego_pos, point); min_distance = std::min(min_distance, arc_length); max_distance = std::max(max_distance, arc_length); } @@ -1209,9 +1211,9 @@ std::vector> calcEnvelopeOverhangDistance( std::vector> overhang_points{}; for (const auto & p : object_data.envelope_poly.outer()) { - const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + const auto point = autoware::universe_utils::createPoint(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. - const auto idx = motion_utils::findNearestIndex(path.points, point); + const auto idx = autoware::motion_utils::findNearestIndex(path.points, point); const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); overhang_points.emplace_back(lateral, point); } @@ -1245,9 +1247,9 @@ Polygon2d createEnvelopePolygon( const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer) { namespace bg = boost::geometry; - using tier4_autoware_utils::expandPolygon; - using tier4_autoware_utils::Point2d; - using tier4_autoware_utils::Polygon2d; + using autoware::universe_utils::expandPolygon; + using autoware::universe_utils::Point2d; + using autoware::universe_utils::Polygon2d; using Box = bg::model::box; const auto toPolygon2d = [](const geometry_msgs::msg::Polygon & polygon) { @@ -1291,7 +1293,7 @@ Polygon2d createEnvelopePolygon( Polygon2d createEnvelopePolygon( const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object_data.object); return createEnvelopePolygon(object_polygon, closest_pose, envelope_buffer); } @@ -1323,7 +1325,7 @@ std::vector generateObstaclePolygonsForDrivableArea( const double diff_poly_buffer = object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = - tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); + autoware::universe_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); obstacles_for_drivable_area.push_back( {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)}); } @@ -1382,16 +1384,18 @@ void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, std::optional & p_out) { - const auto decel_point = motion_utils::calcLongitudinalOffsetPoint(path.points, p_src, offset); + const auto decel_point = + autoware::motion_utils::calcLongitudinalOffsetPoint(path.points, p_src, offset); if (!decel_point) { // TODO(Satoshi OTA) Think later the process in the case of no decel point found. return; } - const auto seg_idx = motion_utils::findNearestSegmentIndex(path.points, decel_point.value()); + const auto seg_idx = + autoware::motion_utils::findNearestSegmentIndex(path.points, decel_point.value()); const auto insert_idx = - motion_utils::insertTargetPoint(seg_idx, decel_point.value(), path.points); + autoware::motion_utils::insertTargetPoint(seg_idx, decel_point.value(), path.points); if (!insert_idx) { // TODO(Satoshi OTA) Think later the process in the case of no decel point found. @@ -1453,7 +1457,7 @@ void fillObjectEnvelopePolygon( const auto multi_step_envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object_data.object); const auto object_polygon_area = boost::geometry::area(object_polygon); const auto envelope_polygon_area = boost::geometry::area(multi_step_envelope_poly); @@ -1686,6 +1690,59 @@ void compensateDetectionLost( } } +void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data) +{ + std::for_each(data.target_objects.begin(), data.target_objects.end(), [](auto & o) { + if (o.is_avoidable) { + o.is_clip_target = true; + } + }); + + const auto itr = + std::remove_if(clip_objects.begin(), clip_objects.end(), [&data](const auto & clip_object) { + const auto id = clip_object.object.object_id; + + // update target objects + { + const auto same_id_obj = std::find_if( + data.target_objects.begin(), data.target_objects.end(), + [&id](const auto & o) { return o.object.object_id == id; }); + if (same_id_obj != data.target_objects.end()) { + same_id_obj->is_clip_target = true; + return false; + } + } + + // update other objects + { + const auto same_id_obj = std::find_if( + data.other_objects.begin(), data.other_objects.end(), + [&id](const auto & o) { return o.object.object_id == id; }); + if (same_id_obj != data.other_objects.end()) { + same_id_obj->is_clip_target = true; + return false; + } + } + + return true; + }); + + clip_objects.erase(itr, clip_objects.end()); + + for (const auto & object : data.target_objects) { + const auto id = object.object.object_id; + const auto same_id_obj = std::find_if( + clip_objects.begin(), clip_objects.end(), + [&id](const auto & o) { return o.object.object_id == id; }); + + if (same_id_obj != clip_objects.end()) { + continue; + } + + clip_objects.push_back(object); + } +} + void updateRoadShoulderDistance( AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) @@ -1813,9 +1870,9 @@ void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineAr // calc longitudinal for (auto & sl : lines) { - sl.start_idx = motion_utils::findNearestIndex(path.points, sl.start.position); + sl.start_idx = autoware::motion_utils::findNearestIndex(path.points, sl.start.position); sl.start_longitudinal = arc.at(sl.start_idx); - sl.end_idx = motion_utils::findNearestIndex(path.points, sl.end.position); + sl.end_idx = autoware::motion_utils::findNearestIndex(path.points, sl.end.position); sl.end_longitudinal = arc.at(sl.end_idx); } } @@ -1864,7 +1921,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( { // TODO(Horibe) parametrize const auto isSimilar = [](const AvoidLine & a, const AvoidLine & b) { - using tier4_autoware_utils::calcDistance2d; + using autoware::universe_utils::calcDistance2d; if (calcDistance2d(a.start, b.start) > 1.0) { return false; } @@ -2113,7 +2170,7 @@ std::pair separateObjectsByPath( double next_longitudinal_distance = parameters->resample_interval_for_output; for (size_t i = 0; i < points_size; ++i) { const auto distance_from_ego = - motion_utils::calcSignedArcLength(reference_path.points, ego_idx, i); + autoware::motion_utils::calcSignedArcLength(reference_path.points, ego_idx, i); if (distance_from_ego > object_check_forward_distance) { break; } @@ -2151,7 +2208,7 @@ std::pair separateObjectsByPath( const auto objects = planner_data->dynamic_object->objects; std::for_each(objects.begin(), objects.end(), [&](const auto & object) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object); if (!within_detection_area(obj_polygon)) { other_objects.objects.push_back(object); } else { @@ -2361,7 +2418,7 @@ double calcDistanceToReturnDeadLine( if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) { const auto & ego_pos = planner_data->self_odometry->pose.pose.position; const auto to_goal_distance = - motion_utils::calcSignedArcLength(path.points, ego_pos, path.points.size() - 1); + autoware::motion_utils::calcSignedArcLength(path.points, ego_pos, path.points.size() - 1); distance_to_return_dead_line = std::min( distance_to_return_dead_line, to_goal_distance - parameters->dead_line_buffer_for_goal); } diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp similarity index 98% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 7aa5b67b23d34..8cf5696a89372 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" #include #include diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp similarity index 94% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp rename to planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp index 110a5d39a8100..bafe50b12edc3 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" -#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" #include #include diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml deleted file mode 100644 index ba2d2ba61ad50..0000000000000 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp deleted file mode 100644 index f594432c9e452..0000000000000 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ /dev/null @@ -1,76 +0,0 @@ -// 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 "manager.hpp" - -#include "scene_dynamic_obstacle_stop.hpp" - -#include - -#include - -namespace autoware::behavior_velocity_planner -{ -using tier4_autoware_utils::getOrDeclareParameter; - -DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterface(node, getModuleName()), module_id_(0UL) -{ - const std::string ns(getModuleName()); - auto & pp = planner_param_; - - pp.extra_object_width = getOrDeclareParameter(node, ns + ".extra_object_width"); - pp.minimum_object_velocity = getOrDeclareParameter(node, ns + ".minimum_object_velocity"); - pp.stop_distance_buffer = getOrDeclareParameter(node, ns + ".stop_distance_buffer"); - pp.time_horizon = getOrDeclareParameter(node, ns + ".time_horizon"); - pp.hysteresis = getOrDeclareParameter(node, ns + ".hysteresis"); - pp.add_duration_buffer = getOrDeclareParameter(node, ns + ".add_stop_duration_buffer"); - pp.remove_duration_buffer = - getOrDeclareParameter(node, ns + ".remove_stop_duration_buffer"); - pp.minimum_object_distance_from_ego_path = - getOrDeclareParameter(node, ns + ".minimum_object_distance_from_ego_path"); - pp.ignore_unavoidable_collisions = - getOrDeclareParameter(node, ns + ".ignore_unavoidable_collisions"); - - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - pp.ego_lateral_offset = - std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m); - pp.ego_longitudinal_offset = vehicle_info.max_longitudinal_offset_m; -} - -void DynamicObstacleStopModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - if (path.points.empty()) return; - // general - if (!isModuleRegistered(module_id_)) { - registerModule(std::make_shared( - module_id_, planner_param_, logger_.get_child("dynamic_obstacle_stop_module"), clock_)); - } -} - -std::function &)> -DynamicObstacleStopModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { - return false; - }; -} -} // namespace autoware::behavior_velocity_planner - -#include -PLUGINLIB_EXPORT_CLASS( - autoware::behavior_velocity_planner::DynamicObstacleStopModulePlugin, - autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp deleted file mode 100644 index d5c403e26792c..0000000000000 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// 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 MANAGER_HPP_ -#define MANAGER_HPP_ - -#include "scene_dynamic_obstacle_stop.hpp" - -#include -#include -#include -#include - -#include - -#include -#include -#include - -namespace autoware::behavior_velocity_planner -{ -class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface -{ -public: - explicit DynamicObstacleStopModuleManager(rclcpp::Node & node); - - const char * getModuleName() override { return "dynamic_obstacle_stop"; } - -private: - using PlannerParam = dynamic_obstacle_stop::PlannerParam; - - PlannerParam planner_param_; - int64_t module_id_; - - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; -}; - -class DynamicObstacleStopModulePlugin : public PluginWrapper -{ -}; - -} // namespace autoware::behavior_velocity_planner - -#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp deleted file mode 100644 index 9c5eced187aa8..0000000000000 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp +++ /dev/null @@ -1,166 +0,0 @@ -// Copyright 2023-2024 TIER IV, Inc. All rights reserved. -// -// 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 "scene_dynamic_obstacle_stop.hpp" - -#include "collision.hpp" -#include "debug.hpp" -#include "footprint.hpp" -#include "object_filtering.hpp" -#include "object_stop_decision.hpp" -#include "types.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop -{ - -using visualization_msgs::msg::Marker; -using visualization_msgs::msg::MarkerArray; - -DynamicObstacleStopModule::DynamicObstacleStopModule( - const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param)) -{ - velocity_factor_.init(PlanningBehavior::ROUTE_OBSTACLE); -} - -bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) -{ - debug_data_.reset_data(); - *stop_reason = planning_utils::initializeStopReason(StopReason::OBSTACLE_STOP); - if (!path || path->points.size() < 2) return true; - - tier4_autoware_utils::StopWatch stopwatch; - stopwatch.tic(); - stopwatch.tic("preprocessing"); - EgoData ego_data; - ego_data.pose = planner_data_->current_odometry->pose; - ego_data.path.points = path->points; - motion_utils::removeOverlapPoints(ego_data.path.points); - ego_data.first_path_idx = - motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); - ego_data.longitudinal_offset_to_first_path_idx = motion_utils::calcLongitudinalOffsetToSegment( - ego_data.path.points, ego_data.first_path_idx, ego_data.pose.position); - const auto min_stop_distance = - motion_utils::calcDecelDistWithJerkAndAccConstraints( - planner_data_->current_velocity->twist.linear.x, 0.0, - planner_data_->current_acceleration->accel.accel.linear.x, - planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold, - planner_data_->max_stop_jerk_threshold) - .value_or(0.0); - ego_data.earliest_stop_pose = motion_utils::calcLongitudinalOffsetPose( - ego_data.path.points, ego_data.pose.position, min_stop_distance); - - make_ego_footprint_rtree(ego_data, params_); - double hysteresis = - std::find_if( - object_map_.begin(), object_map_.end(), - [](const auto & pair) { return pair.second.should_be_avoided(); }) == object_map_.end() - ? 0.0 - : params_.hysteresis; - const auto dynamic_obstacles = - filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_, hysteresis); - - const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); - - stopwatch.tic("footprints"); - const auto obstacle_forward_footprints = - make_forward_footprints(dynamic_obstacles, params_, hysteresis); - const auto footprints_duration_us = stopwatch.toc("footprints"); - stopwatch.tic("collisions"); - auto collisions = find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints); - update_object_map(object_map_, collisions, clock_->now(), ego_data.path.points, params_); - std::optional earliest_collision = - find_earliest_collision(object_map_, ego_data); - const auto collisions_duration_us = stopwatch.toc("collisions"); - if (earliest_collision) { - const auto arc_length_diff = motion_utils::calcSignedArcLength( - ego_data.path.points, *earliest_collision, ego_data.pose.position); - const auto can_stop_before_limit = arc_length_diff < min_stop_distance - - params_.ego_longitudinal_offset - - params_.stop_distance_buffer; - const auto stop_pose = can_stop_before_limit - ? motion_utils::calcLongitudinalOffsetPose( - ego_data.path.points, *earliest_collision, - -params_.stop_distance_buffer - params_.ego_longitudinal_offset) - : ego_data.earliest_stop_pose; - debug_data_.stop_pose = stop_pose; - if (stop_pose) { - motion_utils::insertStopPoint(*stop_pose, 0.0, path->points); - const auto stop_pose_reached = - planner_data_->current_velocity->twist.linear.x < 1e-3 && - tier4_autoware_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; - velocity_factor_.set( - path->points, ego_data.pose, *stop_pose, - stop_pose_reached ? VelocityFactor::STOPPED : VelocityFactor::APPROACHING, - "dynamic_obstacle_stop"); - } - } - - const auto total_time_us = stopwatch.toc(); - RCLCPP_DEBUG( - logger_, - "Total time = %2.2fus\n\tpreprocessing = %2.2fus\n\tfootprints = " - "%2.2fus\n\tcollisions = %2.2fus\n", - total_time_us, preprocessing_duration_us, footprints_duration_us, collisions_duration_us); - debug_data_.ego_footprints = ego_data.path_footprints; - debug_data_.obstacle_footprints = obstacle_forward_footprints; - debug_data_.z = ego_data.pose.position.z; - return true; -} - -MarkerArray DynamicObstacleStopModule::createDebugMarkerArray() -{ - const auto z = debug_data_.z; - MarkerArray array; - std::string ns = "collisions"; - const auto collision_markers = debug::make_collision_markers(object_map_, ns, z, clock_->now()); - debug::add_markers(array, debug_data_.prev_collisions_nb, collision_markers, ns); - ns = "dynamic_obstacles_footprints"; - const auto obstacle_footprint_markers = - debug::make_polygon_markers(debug_data_.obstacle_footprints, ns, z); - debug::add_markers(array, debug_data_.prev_dynamic_obstacles_nb, obstacle_footprint_markers, ns); - ns = "ego_footprints"; - const auto ego_footprint_markers = debug::make_polygon_markers(debug_data_.ego_footprints, ns, z); - debug::add_markers(array, debug_data_.prev_ego_footprints_nb, ego_footprint_markers, ns); - return array; -} - -motion_utils::VirtualWalls DynamicObstacleStopModule::createVirtualWalls() -{ - motion_utils::VirtualWalls virtual_walls{}; - if (debug_data_.stop_pose) { - motion_utils::VirtualWall virtual_wall; - virtual_wall.text = "dynamic_obstacle_stop"; - virtual_wall.longitudinal_offset = params_.ego_longitudinal_offset; - virtual_wall.style = motion_utils::VirtualWallType::stop; - virtual_wall.pose = *debug_data_.stop_pose; - virtual_walls.push_back(virtual_wall); - } - return virtual_walls; -} - -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp deleted file mode 100644 index 8018412ba2b4b..0000000000000 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright 2023-2024 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 SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ -#define SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ - -#include "object_stop_decision.hpp" -#include "types.hpp" - -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop -{ -class DynamicObstacleStopModule : public SceneModuleInterface -{ -public: - DynamicObstacleStopModule( - const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); - - /// @brief insert stop or slow down points to prevent dangerously entering another lane - /// @param [inout] path the path to update - /// @param [inout] stop_reason reason for stopping - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; - - visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; - -private: - PlannerParam params_; - ObjectStopDecisionMap object_map_; - -protected: - int64_t module_id_{}; - - // Debug - mutable DebugData debug_data_; -}; -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop - -#endif // SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/plugins.xml b/planning/behavior_velocity_occlusion_spot_module/plugins.xml deleted file mode 100644 index 53a2c460c2e44..0000000000000 --- a/planning/behavior_velocity_occlusion_spot_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt diff --git a/planning/autoware_behavior_velocity_blind_spot_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/README.md similarity index 100% rename from planning/autoware_behavior_velocity_blind_spot_module/README.md rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/README.md diff --git a/planning/autoware_behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/config/blind_spot.param.yaml similarity index 100% rename from planning/autoware_behavior_velocity_blind_spot_module/config/blind_spot.param.yaml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/config/blind_spot.param.yaml diff --git a/planning/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.drawio.svg diff --git a/planning/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.svg similarity index 100% rename from planning/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.svg diff --git a/planning/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml similarity index 94% rename from planning/autoware_behavior_velocity_blind_spot_module/package.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index 8a740727445ca..dd4858dabe293 100644 --- a/planning/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -17,16 +17,16 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils geometry_msgs lanelet2_extension - motion_utils pluginlib rclcpp tf2 - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_velocity_blind_spot_module/plugins.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_velocity_blind_spot_module/plugins.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/plugins.xml diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp similarity index 82% rename from planning/autoware_behavior_velocity_blind_spot_module/src/debug.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp index c357df481bca8..714280daad38e 100644 --- a/planning/autoware_behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp @@ -14,10 +14,10 @@ #include "scene.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -27,10 +27,10 @@ namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; namespace { @@ -73,12 +73,12 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( } // namespace -motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWalls virtual_walls; if (debug_data_.virtual_wall_pose) { - motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWall wall; wall.text = "blind_spot"; wall.pose = debug_data_.virtual_wall_pose.value(); wall.ns = std::to_string(module_id_) + "_"; diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp similarity index 93% rename from planning/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index 6b963d86c9bbf..da4582dd1643f 100644 --- a/planning/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include namespace autoware::behavior_velocity_planner { @@ -88,8 +88,8 @@ void BlindSpotModule::setRTCStatusByDecision( { setSafe(false); const auto & current_pose = planner_data_->current_odometry->pose; - setDistance( - motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + setDistance(autoware::motion_utils::calcSignedArcLength( + path.points, current_pose.position, decision.stop_line_idx)); return; } @@ -124,8 +124,8 @@ void BlindSpotModule::setRTCStatusByDecision( { setSafe(true); const auto & current_pose = planner_data_->current_odometry->pose; - setDistance( - motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + setDistance(autoware::motion_utils::calcSignedArcLength( + path.points, current_pose.position, decision.stop_line_idx)); return; } diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp similarity index 96% rename from planning/autoware_behavior_velocity_blind_spot_module/src/manager.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index 7f2f7ebd89baa..39c82573de0a4 100644 --- a/planning/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "manager.hpp" -#include -#include +#include +#include #include diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.hpp similarity index 88% rename from planning/autoware_behavior_velocity_blind_spot_module/src/manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.hpp index 756aec415c4ec..edc4ea502b396 100644 --- a/planning/autoware_behavior_velocity_blind_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp similarity index 95% rename from planning/autoware_behavior_velocity_blind_spot_module/src/scene.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index eed767fb891f9..f7e859a9085c7 100644 --- a/planning/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -14,13 +14,13 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include +#include #include #include #include -#include #include #include @@ -91,7 +91,7 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( /* calc closest index */ const auto & current_pose = planner_data_->current_odometry->pose; - const auto closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto closest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); const auto stop_line_idx = @@ -256,7 +256,7 @@ std::optional BlindSpotModule::getSiblingStraightLanelet( static std::optional getFirstPointIntersectsLineByFootprint( const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -266,8 +266,8 @@ static std::optional getFirstPointIntersectsLineByFootprint( const auto line2d = line.basicLineString(); for (auto i = start; i <= lane_end; ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - footprint, tier4_autoware_utils::pose2transform(base_pose)); + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, line2d)) { return std::make_optional(i); } @@ -282,7 +282,7 @@ static std::optional getDuplicatedPointIdx( const auto & p = path.points.at(i).point.pose.position; constexpr double min_dist = 0.001; - if (tier4_autoware_utils::calcDistance2d(p, point) < min_dist) { + if (autoware::universe_utils::calcDistance2d(p, point) < min_dist) { return i; } } @@ -299,7 +299,7 @@ static std::optional insertPointIndex( return duplicate_idx_opt.value(); } - const size_t closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t closest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( inout_path->points, in_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) @@ -361,12 +361,12 @@ std::optional> BlindSpotModule::generateStopLine( const geometry_msgs::msg::Point mid_point = geometry_msgs::build().x(mid_pt.x()).y(mid_pt.y()).z(mid_pt.z()); stop_idx_default_ip = stop_idx_critical_ip = - motion_utils::findNearestSegmentIndex(path_ip.points, mid_point); + autoware::motion_utils::findNearestSegmentIndex(path_ip.points, mid_point); /* // NOTE: it is not ambiguous when the curve starts on the left/right lanelet, so this module inserts stopline at the beginning of the lanelet for baselink stop_idx_default_ip = stop_idx_critical_ip = static_cast(std::max(0, - static_cast(motion_utils::findNearestSegmentIndex(path_ip.points, mid_point)) - + static_cast(autoware::motion_utils::findNearestSegmentIndex(path_ip.points, mid_point)) - baselink2front_dist)); */ } @@ -419,12 +419,13 @@ std::optional BlindSpotModule::isOverPassJudge( const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithAccLimit( planner_data_->current_velocity->twist.linear.x, planner_data_->max_stop_acceleration_threshold, planner_data_->delay_response_time); - const auto ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); + const auto ego_segment_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); const size_t stop_point_segment_idx = - motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); - const auto distance_until_stop = motion_utils::calcSignedArcLength( + autoware::motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); + const auto distance_until_stop = autoware::motion_utils::calcSignedArcLength( input_path.points, current_pose.position, ego_segment_idx, stop_point_pose.position, stop_point_segment_idx); diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp similarity index 96% rename from planning/autoware_behavior_velocity_blind_spot_module/src/scene.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp index 721576a9fac8a..7faf78cb2d979 100644 --- a/planning/autoware_behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp @@ -15,9 +15,9 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -117,7 +117,7 @@ class BlindSpotModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - std::vector createVirtualWalls() override; + std::vector createVirtualWalls() override; private: // (semi) const variables diff --git a/planning/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt diff --git a/planning/autoware_behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/README.md rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md diff --git a/planning/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_module.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_module.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_module.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_module.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/debug_markers.png b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/debug_markers.png similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/debug_markers.png rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/debug_markers.png diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/example.png b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/example.png similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/example.png rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/example.png diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/ttc-ttv.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/ttc-ttv.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/ttc-ttv.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/ttc-ttv.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/with_occlusion.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/with_occlusion.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/with_occlusion.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/with_occlusion.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/with_traffic_light.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/with_traffic_light.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/with_traffic_light.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/with_traffic_light.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/docs/without_traffic_light.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/without_traffic_light.svg similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/docs/without_traffic_light.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/without_traffic_light.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/include/autoware_behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp similarity index 93% rename from planning/autoware_behavior_velocity_crosswalk_module/include/autoware_behavior_velocity_crosswalk_module/util.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp index ef7a8217fc6e6..bcf31eadcc35f 100644 --- a/planning/autoware_behavior_velocity_crosswalk_module/include/autoware_behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ #include #include @@ -33,7 +33,7 @@ #define EIGEN_MPL2_ONLY #include #include -#include +#include #include #include @@ -110,4 +110,4 @@ std::optional getStopLineFromMap( const std::string & attribute_name); } // namespace autoware::behavior_velocity_planner -#endif // AUTOWARE_BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ diff --git a/planning/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml similarity index 94% rename from planning/autoware_behavior_velocity_crosswalk_module/package.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index c3cf1339bbe6a..85a19411405a7 100644 --- a/planning/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -22,26 +22,26 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_grid_map_utils + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils eigen geometry_msgs grid_map_core grid_map_ros - grid_map_utils interpolation lanelet2_extension libboost-dev - motion_utils nav_msgs pcl_conversions pluginlib rclcpp sensor_msgs tier4_api_msgs - tier4_autoware_utils tier4_debug_msgs visualization_msgs diff --git a/planning/autoware_behavior_velocity_crosswalk_module/plugins.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/plugins.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/plugins.xml diff --git a/planning/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp similarity index 88% rename from planning/autoware_behavior_velocity_crosswalk_module/src/debug.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp index 1418d46081fcd..d96256d2753ce 100644 --- a/planning/autoware_behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp @@ -14,24 +14,24 @@ #include "scene_crosswalk.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include namespace autoware::behavior_velocity_planner { -using motion_utils::createSlowDownVirtualWallMarker; -using motion_utils::createStopVirtualWallMarker; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; +using autoware::motion_utils::createSlowDownVirtualWallMarker; +using autoware::motion_utils::createStopVirtualWallMarker; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; using visualization_msgs::msg::Marker; namespace @@ -177,19 +177,19 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( } } // namespace -motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWall wall; wall.text = "crosswalk"; wall.ns = std::to_string(module_id_) + "_"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware::motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } - wall.style = motion_utils::VirtualWallType::slowdown; + wall.style = autoware::motion_utils::VirtualWallType::slowdown; wall.text += debug_data_.virtual_wall_suffix; for (const auto & p : debug_data_.slow_poses) { wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp similarity index 98% rename from planning/autoware_behavior_velocity_crosswalk_module/src/manager.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index 428ec2510c7f4..82cddc2c28f1e 100644 --- a/planning/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "manager.hpp" -#include -#include +#include +#include #include #include @@ -27,8 +27,8 @@ namespace autoware::behavior_velocity_planner { +using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::Crosswalk; -using tier4_autoware_utils::getOrDeclareParameter; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp similarity index 90% rename from planning/autoware_behavior_velocity_crosswalk_module/src/manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp index 520dab49b347f..8ce2adea56a4c 100644 --- a/planning/autoware_behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_crosswalk.hpp" -#include -#include -#include +#include +#include +#include #include #include diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp similarity index 94% rename from planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 9b66802c1e5ed..55aa4daed9f87 100644 --- a/planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -14,10 +14,10 @@ #include "occluded_crosswalk.hpp" -#include "autoware_behavior_velocity_crosswalk_module/util.hpp" +#include "autoware/behavior_velocity_crosswalk_module/util.hpp" +#include #include -#include #include #include @@ -118,7 +118,7 @@ void clear_occlusions_behind_objects( object.kinematics.initial_pose_with_covariance.pose.position.y}; if (lanelet::geometry::distance2d(grid_map_position, object_position) < range) { lanelet::BasicPoints2d edge_points; - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object); for (const auto & edge_point : object_polygon.outer()) edge_points.push_back(edge_point); std::sort(edge_points.begin(), edge_points.end(), angle_cmp); // points.push_back(interpolate_point({object_position, edge_point}, 10.0 * range)); @@ -129,7 +129,8 @@ void clear_occlusions_behind_objects( polygon_to_clear.addVertex( interpolate_point({grid_map_position, edge_points.back()}, 10.0 * range)); polygon_to_clear.addVertex(edge_points.back()); - for (grid_map_utils::PolygonIterator it(grid_map, polygon_to_clear); !it.isPastEnd(); ++it) + for (autoware::grid_map_utils::PolygonIterator it(grid_map, polygon_to_clear); + !it.isPastEnd(); ++it) grid_map.at("layer", *it) = 0; } } @@ -156,7 +157,7 @@ bool is_crosswalk_occluded( crosswalk_lanelet, {path_intersection.x, path_intersection.y}, detection_range)) { grid_map::Polygon poly; for (const auto & p : detection_area) poly.addVertex(grid_map::Position(p.x(), p.y())); - for (grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter) + for (autoware::grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter) if (is_occluded(grid_map, min_nb_of_cells, *iter, params)) return true; } return false; diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp similarity index 100% rename from planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp similarity index 95% rename from planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 0db87508975a0..786828d72dcb3 100644 --- a/planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -16,15 +16,15 @@ #include "occluded_crosswalk.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include -#include -#include #include #include @@ -41,22 +41,22 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using motion_utils::calcArcLength; -using motion_utils::calcDecelDistWithJerkAndAccConstraints; -using motion_utils::calcLateralOffset; -using motion_utils::calcLongitudinalOffsetPoint; -using motion_utils::calcLongitudinalOffsetPose; -using motion_utils::calcSignedArcLength; -using motion_utils::calcSignedArcLengthPartialSum; -using motion_utils::findNearestSegmentIndex; -using motion_utils::insertTargetPoint; -using motion_utils::resamplePath; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::getPose; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; -using tier4_autoware_utils::pose2transform; -using tier4_autoware_utils::toHexString; +using autoware::motion_utils::calcArcLength; +using autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints; +using autoware::motion_utils::calcLateralOffset; +using autoware::motion_utils::calcLongitudinalOffsetPoint; +using autoware::motion_utils::calcLongitudinalOffsetPose; +using autoware::motion_utils::calcSignedArcLength; +using autoware::motion_utils::calcSignedArcLengthPartialSum; +using autoware::motion_utils::findNearestSegmentIndex; +using autoware::motion_utils::insertTargetPoint; +using autoware::motion_utils::resamplePath; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::getPose; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::pose2transform; +using autoware::universe_utils::toHexString; namespace { @@ -85,7 +85,7 @@ void offsetPolygon2d( { for (const auto & polygon_point : polygon.points) { const auto offset_pos = - tier4_autoware_utils::calcOffsetPose(origin_point, polygon_point.x, polygon_point.y, 0.0) + autoware::universe_utils::calcOffsetPose(origin_point, polygon_point.x, polygon_point.y, 0.0) .position; offset_polygon.outer().push_back(Point2d(offset_pos.x, offset_pos.y)); } @@ -99,7 +99,7 @@ Polygon2d createMultiStepPolygon( Polygon2d multi_step_polygon{}; for (size_t i = start_idx; i <= end_idx; ++i) { offsetPolygon2d( - tier4_autoware_utils::getPose(obj_path_points.at(i)), polygon, multi_step_polygon); + autoware::universe_utils::getPose(obj_path_points.at(i)), polygon, multi_step_polygon); } Polygon2d hull_multi_step_polygon{}; @@ -385,9 +385,10 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const double dist_ego2crosswalk = calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()); - const auto braking_distance_opt = motion_utils::calcDecelDistWithJerkAndAccConstraints( - ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, - p.min_jerk_for_no_stop_decision); + const auto braking_distance_opt = + autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( + ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, + p.min_jerk_for_no_stop_decision); const double braking_distance = braking_distance_opt ? *braking_distance_opt : 0.0; if ( dist_ego2crosswalk - base_link2front - braking_distance < @@ -414,7 +415,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( if ( isVehicleType(object.classification) && ego_crosswalk_passage_direction && collision_point.crosswalk_passage_direction) { - const double direction_diff = tier4_autoware_utils::normalizeRadian( + const double direction_diff = autoware::universe_utils::normalizeRadian( collision_point.crosswalk_passage_direction.value() - ego_crosswalk_passage_direction.value()); if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) { @@ -648,13 +649,14 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) -> std::optional> { const auto line_start = - tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + autoware::universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); const auto line_end = - tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + autoware::universe_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); for (unsigned i = 0; i < path.points.size() - 1; ++i) { const auto & start = path.points.at(i).point.pose.position; const auto & end = path.points.at(i + 1).point.pose.position; - if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + if (const auto intersect = + autoware::universe_utils::intersect(line_start, line_end, start, end); intersect.has_value()) { return std::make_optional(std::make_pair(i, intersect.value())); } @@ -676,18 +678,19 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( const autoware_perception_msgs::msg::PredictedPath & path) const { - using tier4_autoware_utils::Segment2d; + using autoware::universe_utils::Segment2d; auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) -> std::optional> { const auto line_start = - tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + autoware::universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); const auto line_end = - tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + autoware::universe_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); for (unsigned i = 0; i < path.path.size() - 1; ++i) { const auto & start = path.path.at(i).position; const auto & end = path.path.at(i + 1).position; - if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + if (const auto intersect = + autoware::universe_utils::intersect(line_start, line_end, start, end); intersect.has_value()) { return std::make_optional(std::make_pair(i, intersect.value())); } diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp similarity index 96% rename from planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index fe353b24273e6..491e0dadd4d80 100644 --- a/planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -15,13 +15,13 @@ #ifndef SCENE_CROSSWALK_HPP_ #define SCENE_CROSSWALK_HPP_ -#include "autoware_behavior_velocity_crosswalk_module/util.hpp" +#include "autoware/behavior_velocity_crosswalk_module/util.hpp" -#include +#include +#include +#include #include #include -#include -#include #include #include @@ -49,14 +49,14 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; +using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::StopWatch; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficLightElement; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; -using tier4_autoware_utils::Polygon2d; -using tier4_autoware_utils::StopWatch; using tier4_planning_msgs::msg::PathWithLaneId; namespace @@ -263,8 +263,8 @@ class CrosswalkModule : public SceneModuleInterface const bool is_object_away_from_path = !attention_area.outer().empty() && - boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) > - 0.5; + boost::geometry::distance( + autoware::universe_utils::fromMsg(position).to_2d(), attention_area) > 0.5; // add new object if (objects.count(uuid) == 0) { @@ -327,7 +327,7 @@ class CrosswalkModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware::motion_utils::VirtualWalls createVirtualWalls() override; private: // main functions diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp similarity index 91% rename from planning/autoware_behavior_velocity_crosswalk_module/src/util.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp index 1e40429e3a704..93b9780445479 100644 --- a/planning/autoware_behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_behavior_velocity_crosswalk_module/util.hpp" +#include "autoware/behavior_velocity_crosswalk_module/util.hpp" -#include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include #include @@ -48,10 +48,10 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using motion_utils::calcSignedArcLength; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::Line2d; -using tier4_autoware_utils::Point2d; +using autoware::motion_utils::calcSignedArcLength; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::Line2d; +using autoware::universe_utils::Point2d; std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, @@ -152,7 +152,7 @@ std::vector getPolygonIntersects( std::sort(intersects.begin(), intersects.end(), compare); - // convert tier4_autoware_utils::Point2d to geometry::msg::Point + // convert autoware::universe_utils::Point2d to geometry::msg::Point std::vector geometry_points; for (const auto & p : intersects) { geometry_points.push_back(createPoint(p.x(), p.y(), ego_pos.z)); @@ -201,7 +201,7 @@ std::vector getLinestringIntersects( std::sort(intersects.begin(), intersects.end(), compare); - // convert tier4_autoware_utils::Point2d to geometry::msg::Point + // convert autoware::universe_utils::Point2d to geometry::msg::Point std::vector geometry_points; for (const auto & p : intersects) { geometry_points.push_back(createPoint(p.x(), p.y(), ego_pos.z)); diff --git a/planning/behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt similarity index 84% rename from planning/behavior_velocity_detection_area_module/CMakeLists.txt rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt index e6990d2d28642..57eb826927663 100644 --- a/planning/behavior_velocity_detection_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_detection_area_module) +project(autoware_behavior_velocity_detection_area_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_detection_area_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/README.md similarity index 100% rename from planning/behavior_velocity_detection_area_module/README.md rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/README.md diff --git a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml similarity index 100% rename from planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml diff --git a/planning/behavior_velocity_detection_area_module/docs/detection_area.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/docs/detection_area.svg similarity index 100% rename from planning/behavior_velocity_detection_area_module/docs/detection_area.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/docs/detection_area.svg diff --git a/planning/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/docs/keep_stopping.svg similarity index 100% rename from planning/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/docs/keep_stopping.svg diff --git a/planning/autoware_behavior_velocity_stop_line_module/docs/restart.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/docs/restart.svg similarity index 100% rename from planning/autoware_behavior_velocity_stop_line_module/docs/restart.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/docs/restart.svg diff --git a/planning/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/docs/restart_prevention.svg similarity index 100% rename from planning/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/docs/restart_prevention.svg diff --git a/planning/behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml similarity index 84% rename from planning/behavior_velocity_detection_area_module/package.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml index 363251b9b5ef8..5a15c78c9dee2 100644 --- a/planning/behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_detection_area_module + autoware_behavior_velocity_detection_area_module 0.1.0 - The behavior_velocity_detection_area_module package + The autoware_behavior_velocity_detection_area_module package Kyoichi Sugahara Tomoya Kimura @@ -18,19 +18,19 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_planning_msgs autoware_route_handler + autoware_universe_utils eigen geometry_msgs lanelet2_extension libboost-dev - motion_utils pluginlib rclcpp tf2 tf2_eigen tf2_geometry_msgs - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_detection_area_module/plugins.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/plugins.xml similarity index 71% rename from planning/behavior_velocity_detection_area_module/plugins.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/plugins.xml index 9356c863ec26e..b499ad3f293e8 100644 --- a/planning/behavior_velocity_detection_area_module/plugins.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp similarity index 84% rename from planning/behavior_velocity_detection_area_module/src/debug.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp index 1ce982cd8df52..87df2a620f717 100644 --- a/planning/behavior_velocity_detection_area_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp @@ -14,11 +14,11 @@ #include "scene.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -31,11 +31,11 @@ namespace autoware::behavior_velocity_planner { +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; using std_msgs::msg::ColorRGBA; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; namespace { @@ -171,21 +171,21 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray return wall_marker; } -motion_utils::VirtualWalls DetectionAreaModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls DetectionAreaModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWall wall; wall.text = "detection_area"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware::motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { - wall.pose = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } - wall.style = motion_utils::VirtualWallType::deadline; + wall.style = autoware::motion_utils::VirtualWallType::deadline; for (const auto & p : debug_data_.dead_line_poses) { - wall.pose = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp similarity index 95% rename from planning/behavior_velocity_detection_area_module/src/manager.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index 336a1710ed593..4887fd7423817 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -14,9 +14,9 @@ #include "manager.hpp" -#include +#include +#include #include -#include #include @@ -30,8 +30,8 @@ namespace autoware::behavior_velocity_planner { +using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::DetectionArea; -using tier4_autoware_utils::getOrDeclareParameter; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( diff --git a/planning/behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp similarity index 89% rename from planning/behavior_velocity_detection_area_module/src/manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp index 64a76171814b9..7aa60cbbaa18b 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include diff --git a/planning/behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp similarity index 96% rename from planning/behavior_velocity_detection_area_module/src/scene.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index acd4c3650d34a..7abd88d59f398 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -14,9 +14,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -33,8 +33,8 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using motion_utils::calcLongitudinalOffsetPose; -using motion_utils::calcSignedArcLength; +using autoware::motion_utils::calcLongitudinalOffsetPose; +using autoware::motion_utils::calcSignedArcLength; DetectionAreaModule::DetectionAreaModule( const int64_t module_id, const int64_t lane_id, @@ -288,7 +288,7 @@ std::vector DetectionAreaModule::getObstaclePoints() (circle.first.y() - p.y) * (circle.first.y() - p.y); if (squared_dist <= circle.second) { if (bg::within(Point2d{p.x, p.y}, poly.basicPolygon())) { - obstacle_points.push_back(tier4_autoware_utils::createPoint(p.x, p.y, p.z)); + obstacle_points.push_back(autoware::universe_utils::createPoint(p.x, p.y, p.z)); // get all obstacle point becomes high computation cost so skip if any point is found break; } diff --git a/planning/behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp similarity index 93% rename from planning/behavior_velocity_detection_area_module/src/scene.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index f577cd92dea6e..5b15cc92f57b1 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -23,8 +23,8 @@ #define EIGEN_MPL2_ONLY #include -#include -#include +#include +#include #include #include @@ -73,7 +73,7 @@ class DetectionAreaModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware::motion_utils::VirtualWalls createVirtualWalls() override; private: LineString2d getStopLineGeometry2d() const; diff --git a/planning/autoware_behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/CMakeLists.txt rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/CMakeLists.txt diff --git a/planning/autoware_behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/README.md rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md diff --git a/planning/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/data-structure.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/data-structure.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/data-structure.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/data-structure.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/merge_from_private.png b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/merge_from_private.png similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/merge_from_private.png rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/merge_from_private.png diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/static-occlusion-timeout.png b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/static-occlusion-timeout.png similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/static-occlusion-timeout.png rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/static-occlusion-timeout.png diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/ttc.gif b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/ttc.gif similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/ttc.gif rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/ttc.gif diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml similarity index 95% rename from planning/autoware_behavior_velocity_intersection_module/package.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index 4e79139ba2fe7..e240722e66a5b 100644 --- a/planning/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -19,10 +19,12 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler autoware_rtc_interface + autoware_universe_utils autoware_vehicle_info_utils fmt geometry_msgs @@ -30,12 +32,10 @@ lanelet2_extension libopencv-dev magic_enum - motion_utils nav_msgs pluginlib rclcpp tf2_geometry_msgs - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_velocity_intersection_module/plugins.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/plugins.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/plugins.xml diff --git a/planning/autoware_behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/scripts/ttc.py rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py diff --git a/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp similarity index 92% rename from planning/autoware_behavior_velocity_intersection_module/src/debug.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp index ab09ca2e6e746..a53b6e7cd7f92 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp @@ -15,10 +15,10 @@ #include "scene_intersection.hpp" #include "scene_merge_from_private_road.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -33,10 +33,10 @@ namespace { -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, @@ -226,10 +226,10 @@ constexpr std::tuple light_blue() namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() { @@ -424,27 +424,27 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( return debug_marker_array; } -motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWall wall; if (debug_data_.collision_stop_wall_pose) { - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware::motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.collision_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_first_stop_wall_pose) { - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware::motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection_occlusion_first_stop" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_first_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_stop_wall_pose) { - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware::motion_utils::VirtualWallType::stop; wall.text = "intersection_occlusion"; if (debug_data_.static_occlusion_with_traffic_light_timeout) { std::stringstream timeout; @@ -457,7 +457,7 @@ motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() virtual_walls.push_back(wall); } if (debug_data_.absence_traffic_light_creep_wall) { - wall.style = motion_utils::VirtualWallType::slowdown; + wall.style = autoware::motion_utils::VirtualWallType::slowdown; wall.text = "intersection_occlusion"; wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.absence_traffic_light_creep_wall.value(); @@ -483,13 +483,13 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark return debug_marker_array; } -motion_utils::VirtualWalls MergeFromPrivateRoadModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls MergeFromPrivateRoadModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWalls virtual_walls; const auto state = state_machine_.getState(); if (state == StateMachine::State::STOP) { - motion_utils::VirtualWall wall; - wall.style = motion_utils::VirtualWallType::stop; + autoware::motion_utils::VirtualWall wall; + wall.style = autoware::motion_utils::VirtualWallType::stop; wall.pose = debug_data_.virtual_wall_pose; wall.text = "merge_from_private_road"; virtual_walls.push_back(wall); diff --git a/planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/decision_result.cpp similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/decision_result.cpp diff --git a/planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/decision_result.hpp similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/decision_result.hpp diff --git a/planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp diff --git a/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp similarity index 97% rename from planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp index be7e9e25cc8bc..648fc611ac6b1 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -26,7 +26,7 @@ namespace autoware::behavior_velocity_planner void IntersectionLanelets::update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length, lanelet::routing::RoutingGraphPtr routing_graph_ptr) { is_prioritized_ = is_prioritized; diff --git a/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp similarity index 97% rename from planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp index a7ca5eb3b0bc1..f8110475e2782 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -17,7 +17,7 @@ #include "interpolated_path_info.hpp" -#include +#include #include #include @@ -41,7 +41,7 @@ struct IntersectionLanelets */ void update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length, lanelet::routing::RoutingGraphPtr routing_graph_ptr); const lanelet::ConstLanelets & attention() const diff --git a/planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp diff --git a/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp similarity index 99% rename from planning/autoware_behavior_velocity_intersection_module/src/manager.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 8f540603791cb..3fa2dc3efb5b1 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -14,10 +14,10 @@ #include "manager.hpp" -#include -#include +#include +#include +#include #include -#include #include @@ -30,7 +30,7 @@ namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( diff --git a/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp similarity index 94% rename from planning/autoware_behavior_velocity_intersection_module/src/manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp index ecf6bb810f313..8189cbe7bc5d7 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp @@ -18,9 +18,9 @@ #include "scene_intersection.hpp" #include "scene_merge_from_private_road.hpp" -#include -#include -#include +#include +#include +#include #include #include diff --git a/planning/autoware_behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp similarity index 92% rename from planning/autoware_behavior_velocity_intersection_module/src/object_manager.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp index 7473cefa94cac..650363c602b90 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp @@ -14,9 +14,9 @@ #include "object_manager.hpp" +#include +#include // for toPolygon2d #include -#include -#include // for toPolygon2d #include #include @@ -38,15 +38,15 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) return ss.str(); } -tier4_autoware_utils::Polygon2d createOneStepPolygon( +autoware::universe_utils::Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, const autoware_perception_msgs::msg::Shape & shape) { namespace bg = boost::geometry; - const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + const auto prev_poly = autoware::universe_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = autoware::universe_utils::toPolygon2d(next_pose, shape); - tier4_autoware_utils::Polygon2d one_step_poly; + autoware::universe_utils::Polygon2d one_step_poly; for (const auto & point : prev_poly.outer()) { one_step_poly.outer().push_back(point); } @@ -56,7 +56,7 @@ tier4_autoware_utils::Polygon2d createOneStepPolygon( bg::correct(one_step_poly); - tier4_autoware_utils::Polygon2d convex_one_step_poly; + autoware::universe_utils::Polygon2d convex_one_step_poly; bg::convex_hull(one_step_poly, convex_one_step_poly); return convex_one_step_poly; @@ -164,13 +164,13 @@ bool ObjectInfo::can_stop_before_ego_lane( const auto stopline = stopline_opt.value(); const auto stopline_p1 = stopline.front(); const auto stopline_p2 = stopline.back(); - const tier4_autoware_utils::Point2d stopline_mid{ + const autoware::universe_utils::Point2d stopline_mid{ (stopline_p1.x() + stopline_p2.x()) / 2.0, (stopline_p1.y() + stopline_p2.y()) / 2.0}; const auto attention_lane_end = attention_lanelet.centerline().back(); - const tier4_autoware_utils::LineString2d attention_lane_later_part( - {tier4_autoware_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, - tier4_autoware_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); - std::vector ego_collision_points; + const autoware::universe_utils::LineString2d attention_lane_later_part( + {autoware::universe_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, + autoware::universe_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); + std::vector ego_collision_points; bg::intersection( attention_lane_later_part, ego_lane.centerline2d().basicLineString(), ego_collision_points); if (ego_collision_points.empty()) { diff --git a/planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp diff --git a/planning/autoware_behavior_velocity_intersection_module/src/result.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/result.hpp similarity index 100% rename from planning/autoware_behavior_velocity_intersection_module/src/result.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/result.hpp diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp similarity index 96% rename from planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index beed0aab8e382..274bbe5627b22 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -16,15 +16,15 @@ #include "util.hpp" -#include // for toGeomPoly -#include +#include // for toGeomPoly +#include +#include +#include +#include // for toPolygon2d +#include +#include #include #include -#include -#include -#include // for toPolygon2d -#include -#include #include @@ -42,7 +42,7 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using motion_utils::VelocityFactorInterface; +using autoware::motion_utils::VelocityFactorInterface; IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, @@ -56,7 +56,7 @@ IntersectionModule::IntersectionModule( associative_ids_(associative_ids), turn_direction_(turn_direction), has_traffic_light_(has_traffic_light), - occlusion_uuid_(tier4_autoware_utils::generateUUID()) + occlusion_uuid_(autoware::universe_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); @@ -342,7 +342,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail( // Occluded // utility functions auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + return autoware::motion_utils::calcSignedArcLength(path->points, closest_idx, index); }; auto stoppedForDuration = [&](const size_t pos, const double duration, StateMachine & state_machine) { @@ -527,12 +527,13 @@ void prepareRTCByDecisionResult( const auto closest_idx = result.closest_idx; const auto stopline_idx = result.stuck_stopline_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); + *default_distance = + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); *occlusion_safety = true; if (result.occlusion_stopline_idx) { const auto occlusion_stopline_idx = result.occlusion_stopline_idx.value(); *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); } return; } @@ -547,7 +548,8 @@ void prepareRTCByDecisionResult( const auto closest_idx = result.closest_idx; const auto stopline_idx = result.stuck_stopline_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); + *default_distance = + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); *occlusion_safety = true; return; } @@ -563,11 +565,11 @@ void prepareRTCByDecisionResult( const auto collision_stopline_idx = result.collision_stopline_idx; *default_safety = false; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); const auto occlusion_stopline = result.occlusion_stopline_idx; *occlusion_safety = true; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline); return; } @@ -583,10 +585,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = false; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, first_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, first_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -602,10 +604,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -620,7 +622,7 @@ void prepareRTCByDecisionResult( const auto collision_stopline_idx = result.closest_idx; *default_safety = !result.collision_detected; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = 0; return; @@ -638,10 +640,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = false; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -656,10 +658,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = true; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -675,10 +677,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = !result.collision_detected; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = true; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp similarity index 99% rename from planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index e25f41a18028a..ea2e1a7ae16d7 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -22,9 +22,9 @@ #include "object_manager.hpp" #include "result.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -325,7 +325,7 @@ class IntersectionModule : public SceneModuleInterface /** @}*/ visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware::motion_utils::VirtualWalls createVirtualWalls() override; const std::set & getAssociativeIds() const { return associative_ids_; } diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp similarity index 95% rename from planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index fb06ed9d811c8..906d39a9b127f 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -15,13 +15,13 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for toGeomPoly -#include // for smoothPath +#include // for toGeomPoly +#include // for smoothPath +#include +#include // for toPolygon2d +#include #include #include -#include -#include // for toPolygon2d -#include #include #include @@ -111,7 +111,7 @@ void IntersectionModule::updateObjectInfoManagerArea() return false; } return bg::within( - tier4_autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); + autoware::universe_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); }(); std::optional attention_lanelet{std::nullopt}; std::optional stopline{std::nullopt}; @@ -265,7 +265,9 @@ void IntersectionModule::updateObjectInfoManagerCollision( continue; } const auto & object_passage_interval = object_passage_interval_opt.value(); - const auto [object_enter_time, object_exit_time] = object_passage_interval.interval_time; + const auto object_enter_exit_time = object_passage_interval.interval_time; + const auto object_enter_time = std::get<0>(object_enter_exit_time); + const auto object_exit_time = std::get<1>(object_enter_exit_time); const auto ego_start_itr = std::lower_bound( time_distance_array.begin(), time_distance_array.end(), object_enter_time - collision_start_margin_time, @@ -311,7 +313,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( for (auto i = begin; i <= end; ++i) { if (bg::intersects( polygon, - tier4_autoware_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { + autoware::universe_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { collision_detected = true; break; } @@ -490,13 +492,13 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " "ego was at x = {4}, y = {5} when it passed the 1st pass judge line so it is the fault " "of detection side that failed to detect around {6}[m] range at that time.\n", - past_position.x, // 0 - past_position.y, // 1 - time_diff, // 2 - object_info->observed_velocity(), // 3 - passed_1st_judge_line_pose.position.x, // 4 - passed_1st_judge_line_pose.position.y, // 5 - tier4_autoware_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_1st_judge_line_pose.position.x, // 4 + passed_1st_judge_line_pose.position.y, // 5 + autoware::universe_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 ); } } @@ -528,13 +530,13 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " "ego was at x = {4}, y = {5} when it passed the 2nd pass judge line so it is the fault " "of detection side that failed to detect around {6}[m] range at that time.\n", - past_position.x, // 0 - past_position.y, // 1 - time_diff, // 2 - object_info->observed_velocity(), // 3 - passed_2nd_judge_line_pose.position.x, // 4 - passed_2nd_judge_line_pose.position.y, // 5 - tier4_autoware_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_2nd_judge_line_pose.position.x, // 4 + passed_2nd_judge_line_pose.position.y, // 5 + autoware::universe_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 ); } } @@ -614,10 +616,10 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( const auto [begin, end] = unsafe_interval.interval_position; const auto &p1 = unsafe_interval.path.at(begin).position, p2 = unsafe_interval.path.at(end).position; - const auto collision_pos = - tier4_autoware_utils::createPoint((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); + const auto collision_pos = autoware::universe_utils::createPoint( + (p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); const auto object_dist_to_margin_point = - tier4_autoware_utils::calcDistance2d( + autoware::universe_utils::calcDistance2d( object_info->predicted_object().kinematics.initial_pose_with_covariance.pose.position, collision_pos) - planner_param_.collision_detection.avoid_collision_by_acceleration @@ -630,7 +632,7 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( object_dist_to_margin_point / std::max(min_vel, object_info->observed_velocity()); // ego side const double ego_dist_to_collision_pos = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_pos); + autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, collision_pos); const auto ego_eta_to_collision_pos_it = std::lower_bound( ego_time_distance_array.begin(), ego_time_distance_array.end(), ego_dist_to_collision_pos, [](const auto & a, const double b) { return a.second < b; }); @@ -779,7 +781,8 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( } const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); + const double angle_diff = + autoware::universe_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { return std::make_optional(i); @@ -896,15 +899,16 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so // `last_intersection_stopline_candidate_idx` makes no sense - const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, path.points.at(closest_idx).point.pose, - planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + const auto smoothed_path_closest_idx = + autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, path.points.at(closest_idx).point.pose, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { if (upstream_stopline) { const auto upstream_stopline_point = reference_path.points.at(upstream_stopline.value()).point.pose; - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( smoothed_reference_path.points, upstream_stopline_point, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); } else { @@ -916,7 +920,7 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin const auto & p1 = smoothed_reference_path.points.at(i); const auto & p2 = smoothed_reference_path.points.at(i + 1); - const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); + const double dist = autoware::universe_utils::calcDistance2d(p1, p2); dist_sum += dist; // use average velocity between p1 and p2 diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp similarity index 96% rename from planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index c43f8617897b9..24e9a3b4a1793 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "util.hpp" +#include // for toPolygon2d #include -#include // for toPolygon2d #include #include @@ -151,7 +151,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( auto findCommonCvPolygons = [&](const auto & area2d, std::vector> & cv_polygons) -> void { - tier4_autoware_utils::Polygon2d area2d_poly; + autoware::universe_utils::Polygon2d area2d_poly; for (const auto & p : area2d) { area2d_poly.outer().emplace_back(p.x(), p.y()); } @@ -240,7 +240,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( std::vector> blocking_polygons; for (const auto & blocking_attention_object_info : blocking_attention_objects) { const Polygon2d obj_poly = - tier4_autoware_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); + autoware::universe_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { @@ -321,7 +321,8 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { const auto path_linestring_point = path_ip.points.at(i).point.pose.position; if ( - tier4_autoware_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < + autoware::universe_utils::calcDistance2d( + prev_path_linestring_point, path_linestring_point) < 1.0 /* rough tick for computational cost */) { continue; } @@ -405,9 +406,9 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( division_index, std::distance(division.begin(), point_it), acc_dist, - tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z), - tier4_autoware_utils::createPoint( + autoware::universe_utils::createPoint(point_it->x(), point_it->y(), origin.z), + autoware::universe_utils::createPoint(projection_it->x(), projection_it->y(), origin.z), + autoware::universe_utils::createPoint( projection_it->x(), projection_it->y(), origin.z) /* initialize with projection point at first*/}; found_min_dist_for_this_division = true; @@ -416,7 +417,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( // division previously in this iteration, and the iterated cells are still OCCLUDED since // then nearest_occlusion_point.visible_end = - tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z); + autoware::universe_utils::createPoint(point_it->x(), point_it->y(), origin.z); } } is_prev_occluded = (pixel == OCCLUDED); @@ -440,7 +441,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( bg::correct(ego_occlusion_triangle); for (const auto & attention_object_info : object_info_manager_.allObjects()) { const auto obj_poly = - tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); + autoware::universe_utils::toPolygon2d(attention_object_info->predicted_object()); if (bg::intersects(obj_poly, ego_occlusion_triangle)) { debug_data_.static_occlusion = false; return DynamicallyOccluded{min_dist}; diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp similarity index 97% rename from planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 223e8eca84fe8..1390104d98175 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -15,13 +15,13 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for to_bg2d -#include // for planning_utils:: +#include // for to_bg2d +#include // for planning_utils:: +#include #include #include // for lanelet::autoware::RoadMarking #include #include -#include #include #include @@ -31,7 +31,7 @@ #include #include -namespace tier4_autoware_utils +namespace autoware::universe_utils { template <> @@ -44,7 +44,7 @@ inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) return point; } -} // namespace tier4_autoware_utils +} // namespace autoware::universe_utils namespace { @@ -74,7 +74,7 @@ lanelet::ConstLanelet generatePathLanelet( for (size_t i = start_idx; i <= end_idx; ++i) { const auto & p = path.points.at(i).point.pose; const auto & p_prev = path.points.at(prev_idx).point.pose; - if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + if (i != start_idx && autoware::universe_utils::calcDistance2d(p_prev, p) < interval) { continue; } prev_idx = i; @@ -198,7 +198,7 @@ Result IntersectionModule::prepare const auto & path_ip = interpolated_path_info.path; const auto & path_ip_intersection_end = interpolated_path_info.lane_id_interval.value().second; - internal_debug_data_.distance = motion_utils::calcSignedArcLength( + internal_debug_data_.distance = autoware::motion_utils::calcSignedArcLength( path->points, current_pose.position, path_ip.points.at(path_ip_intersection_end).point.pose.position); @@ -275,9 +275,9 @@ Result IntersectionModule::prepare if (is_green_solid_on && !initial_green_light_observed_time_) { const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); const bool approached_assigned_lane = - motion_utils::calcSignedArcLength( + autoware::motion_utils::calcSignedArcLength( path->points, closest_idx, - tier4_autoware_utils::createPoint( + autoware::universe_utils::createPoint( assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), assigned_lane_begin_point.z())) < planner_param_.collision_detection.yield_on_green_traffic_light @@ -338,7 +338,7 @@ std::optional IntersectionModule::getStopLineIndexFromMap( stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); } @@ -381,8 +381,8 @@ std::optional IntersectionModule::generateIntersectionSto std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + const auto path_footprint = autoware::universe_utils::transformVector( + local_footprint, autoware::universe_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { // NOTE: maybe consideration of braking dist is necessary first_footprint_attention_centerline_ip_opt = i; @@ -413,7 +413,7 @@ std::optional IntersectionModule::generateIntersectionSto // (2) ego front stop line position on interpolated path const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; - const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto closest_idx_ip = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); @@ -423,8 +423,8 @@ std::optional IntersectionModule::generateIntersectionSto // NOTE: if footprints[0] is already inside the attention area, invalid { const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; - const auto path_footprint0 = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + const auto path_footprint0 = autoware::universe_utils::transformVector( + local_footprint, autoware::universe_utils::pose2transform(base_pose0)); if (bg::intersects( path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { occlusion_peeking_line_valid = false; diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp similarity index 97% rename from planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index eecb02d07c386..14baf6faa581a 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -15,9 +15,9 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for toGeomPoly +#include // for toGeomPoly +#include #include -#include #include #include @@ -124,7 +124,7 @@ std::optional IntersectionModule::isStuckStatus( { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path.points, closest_idx, index); + return autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, index); }; const auto & intersection_lanelets = intersection_lanelets_.value(); // this is OK @@ -290,7 +290,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection(const PathLanelets & pa } // check if the footprint is in the stuck detect area - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const auto obj_footprint = autoware::universe_utils::toPolygon2d(object); // NOTE: in order not to stop too much const bool is_in_stuck_area = bg::within( to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), @@ -310,7 +310,7 @@ std::optional IntersectionModule::isYieldStuckStatus( { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path.points, closest_idx, index); + return autoware::motion_utils::calcSignedArcLength(path.points, closest_idx, index); }; const auto & intersection_lanelets = intersection_lanelets_.value(); const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp similarity index 94% rename from planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 1aaed779e1b79..ddfd3b7d4cacd 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -16,11 +16,11 @@ #include "util.hpp" -#include -#include +#include +#include +#include #include #include -#include #include #include @@ -53,7 +53,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( static std::optional getFirstConflictingLanelet( const lanelet::ConstLanelets & conflicting_lanelets, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, end] = interpolated_path_info.lane_id_interval.value(); @@ -63,8 +63,8 @@ static std::optional getFirstConflictingLanelet( for (size_t i = start; i <= end; ++i) { const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = - tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(pose)); for (const auto & conflicting_lanelet : conflicting_lanelets) { const auto polygon_2d = conflicting_lanelet.polygon2d().basicPolygon(); const bool intersects = bg::intersects(polygon_2d, path_footprint); @@ -159,7 +159,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); - const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( + const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength( path->points, current_pose.position, path->points.at(stopline_idx).point.pose.position); if ( diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp similarity index 93% rename from planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index 19e9ea44869ea..a69297adea881 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -15,8 +15,8 @@ #ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ #define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#include -#include +#include +#include #include #include @@ -70,7 +70,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware::motion_utils::VirtualWalls createVirtualWalls() override; const std::set & getAssociativeIds() const { return associative_ids_; } lanelet::ConstLanelets getAttentionLanelets() const; diff --git a/planning/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp similarity index 93% rename from planning/autoware_behavior_velocity_intersection_module/src/util.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp index 09f16bcada3c1..893ec646dba73 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -16,11 +16,11 @@ #include "interpolated_path_info.hpp" -#include -#include -#include +#include +#include +#include +#include #include -#include #include #include @@ -51,7 +51,7 @@ static std::optional getDuplicatedPointIdx( const auto & p = path.points.at(i).point.pose.position; constexpr double min_dist = 0.001; - if (tier4_autoware_utils::calcDistance2d(p, point) < min_dist) { + if (autoware::universe_utils::calcDistance2d(p, point) < min_dist) { return i; } } @@ -68,7 +68,7 @@ std::optional insertPointIndex( return duplicate_idx_opt.value(); } - const size_t closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t closest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( inout_path->points, in_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) @@ -130,7 +130,7 @@ std::optional> findLaneIdsInterval( std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -140,8 +140,8 @@ std::optional getFirstPointInsidePolygonByFootprint( const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); for (auto i = start; i <= lane_end; ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - footprint, tier4_autoware_utils::pose2transform(base_pose)); + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(base_pose)); if (bg::intersects(path_footprint, area_2d)) { return std::make_optional(i); } @@ -154,7 +154,7 @@ std::optional & polygons, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); @@ -164,8 +164,8 @@ getFirstPointInsidePolygonsByFootprint( for (size_t i = start; i <= lane_end; ++i) { const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = - tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(pose)); for (size_t j = 0; j < polygons.size(); ++j) { const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); const bool is_in_polygon = bg::intersects(area_2d, path_footprint); @@ -332,7 +332,7 @@ bool isBeforeTargetIndex( return static_cast(target_idx > closest_idx); } -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); diff --git a/planning/autoware_behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp similarity index 94% rename from planning/autoware_behavior_velocity_intersection_module/src/util.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp index ef826380afa69..f89d6b0ea67ae 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp @@ -17,8 +17,8 @@ #include "interpolated_path_info.hpp" +#include #include -#include #include @@ -87,7 +87,7 @@ bool isBeforeTargetIndex( const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); /** @@ -125,7 +125,7 @@ mergeLaneletsByTopologicalSort( */ std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); /** * @brief find the index of the first point where vehicle footprint intersects with the given @@ -136,7 +136,7 @@ std::optional & polygons, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); std::vector getPolygon3dFromLanelets( const lanelet::ConstLanelets & ll_vec); diff --git a/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/CMakeLists.txt similarity index 85% rename from planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/CMakeLists.txt index 265b5b637e7ce..cee9e8ca039c9 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_no_drivable_lane_module) +project(autoware_behavior_velocity_no_drivable_lane_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_no_drivable_lane_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/README.md similarity index 100% rename from planning/behavior_velocity_no_drivable_lane_module/README.md rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/README.md diff --git a/planning/behavior_velocity_no_drivable_lane_module/config/no_drivable_lane.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/config/no_drivable_lane.param.yaml similarity index 100% rename from planning/behavior_velocity_no_drivable_lane_module/config/no_drivable_lane.param.yaml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/config/no_drivable_lane.param.yaml diff --git a/planning/behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_design.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_design.svg similarity index 100% rename from planning/behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_design.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_design.svg diff --git a/planning/behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_scenarios.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_scenarios.svg similarity index 100% rename from planning/behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_scenarios.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_scenarios.svg diff --git a/planning/behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/package.xml similarity index 86% rename from planning/behavior_velocity_no_drivable_lane_module/package.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/package.xml index f728167f09f23..81e3e54539bb3 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_no_drivable_lane_module + autoware_behavior_velocity_no_drivable_lane_module 0.1.0 - The behavior_velocity_no_drivable_lane_module package + The autoware_behavior_velocity_no_drivable_lane_module package Ahmed Ebrahim Tomoya Kimura @@ -20,18 +20,18 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_planning_msgs autoware_route_handler + autoware_universe_utils eigen geometry_msgs lanelet2_extension libboost-dev - motion_utils pluginlib rclcpp sensor_msgs tf2 - tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/plugins.xml similarity index 70% rename from planning/behavior_velocity_no_drivable_lane_module/plugins.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/plugins.xml index e3e9efb254fba..26635e8e24bc1 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/debug.cpp similarity index 80% rename from planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/debug.cpp index 0908fe712dbf5..c0c04a0949ce8 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/debug.cpp @@ -14,20 +14,20 @@ #include "scene.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using tier4_autoware_utils::createPoint; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; +using autoware::universe_utils::createPoint; using visualization_msgs::msg::Marker; namespace @@ -71,18 +71,18 @@ visualization_msgs::msg::MarkerArray createNoDrivableLaneMarkers( } } // namespace -motion_utils::VirtualWalls NoDrivableLaneModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls NoDrivableLaneModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWalls virtual_walls; const auto now = this->clock_->now(); if ( (state_ == State::APPROACHING) || (state_ == State::INSIDE_NO_DRIVABLE_LANE) || (state_ == State::STOPPED)) { - motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWall wall; wall.text = "no_drivable_lane"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware::motion_utils::VirtualWallType::stop; wall.ns = std::to_string(module_id_) + "_"; wall.pose = debug_data_.stop_pose; virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp similarity index 93% rename from planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp index 8c3fc5ef0f3ce..9c6f2f50925c6 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -14,14 +14,14 @@ #include "manager.hpp" -#include -#include +#include +#include #include namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::getOrDeclareParameter; +using autoware::universe_utils::getOrDeclareParameter; NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp similarity index 89% rename from planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp index 4afa498a5679b..545af1576c6a8 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp similarity index 87% rename from planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index ddc5d08d60cb8..42d896fec8ab3 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -14,16 +14,16 @@ #include "scene.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include "util.hpp" -#include +#include #include namespace autoware::behavior_velocity_planner { -using tier4_autoware_utils::createPoint; +using autoware::universe_utils::createPoint; NoDrivableLaneModule::NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, @@ -58,7 +58,7 @@ bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason if (path_no_drivable_lane_polygon_intersection.first_intersection_point) { first_intersection_point = path_no_drivable_lane_polygon_intersection.first_intersection_point.value(); - distance_ego_first_intersection = motion_utils::calcSignedArcLength( + distance_ego_first_intersection = autoware::motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, first_intersection_point); distance_ego_first_intersection -= planner_data_->vehicle_info_.max_longitudinal_offset_m; } @@ -135,7 +135,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR const double longitudinal_offset = -1.0 * (planner_param_.stop_margin + planner_data_->vehicle_info_.max_longitudinal_offset_m); - const auto op_target_point = motion_utils::calcLongitudinalOffsetPoint( + const auto op_target_point = autoware::motion_utils::calcLongitudinalOffsetPoint( path->points, first_intersection_point, longitudinal_offset); geometry_msgs::msg::Point target_point; @@ -144,17 +144,18 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR target_point = op_target_point.value(); } - const auto target_segment_idx = motion_utils::findNearestSegmentIndex(path->points, target_point); + const auto target_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(path->points, target_point); const auto & op_target_point_idx = - motion_utils::insertTargetPoint(target_segment_idx, target_point, path->points, 5e-2); + autoware::motion_utils::insertTargetPoint(target_segment_idx, target_point, path->points, 5e-2); size_t target_point_idx; if (op_target_point_idx) { target_point_idx = op_target_point_idx.value(); } geometry_msgs::msg::Point stop_point = - tier4_autoware_utils::getPoint(path->points.at(target_point_idx).point); + autoware::universe_utils::getPoint(path->points.at(target_point_idx).point); const auto & op_stop_pose = planning_utils::insertStopPoint(stop_point, target_segment_idx, *path); @@ -169,7 +170,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); - const auto virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( + const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); debug_data_.stop_pose = virtual_wall_pose.value(); @@ -177,9 +178,9 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR const size_t current_seg_idx = findEgoSegmentIndex(path->points); const auto intersection_segment_idx = - motion_utils::findNearestSegmentIndex(path->points, first_intersection_point); + autoware::motion_utils::findNearestSegmentIndex(path->points, first_intersection_point); const double signed_arc_dist_to_intersection_point = - motion_utils::calcSignedArcLength( + autoware::motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, current_seg_idx, first_intersection_point, intersection_segment_idx) - planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -215,14 +216,14 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( // Get stop point and stop factor { tier4_planning_msgs::msg::StopFactor stop_factor; - const auto & stop_pose = tier4_autoware_utils::getPose(path->points.at(0)); + const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0)); stop_factor.stop_pose = stop_pose; stop_factor.stop_factor_points.push_back(current_point); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); - const auto & virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( + const auto & virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); debug_data_.stop_pose = virtual_wall_pose.value(); @@ -239,7 +240,7 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path, StopReason * stop_reason) { - const auto & stopped_pose = motion_utils::calcLongitudinalOffsetPose( + const auto & stopped_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, planner_data_->current_odometry->pose.position, 0.0); if (!stopped_pose) { @@ -264,7 +265,7 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path, StopReaso velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); - const auto virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( + const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); debug_data_.stop_pose = virtual_wall_pose.value(); diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp similarity index 95% rename from planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index 6910e972a8685..1e8e93834f89e 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -17,7 +17,7 @@ #include "util.hpp" -#include +#include #include #include @@ -60,7 +60,7 @@ class NoDrivableLaneModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware::motion_utils::VirtualWalls createVirtualWalls() override; private: const int64_t lane_id_; diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp similarity index 91% rename from planning/behavior_velocity_no_drivable_lane_module/src/util.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp index c9a6ce1ac1510..1339fee207416 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -14,11 +14,11 @@ #include "util.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include -#include +#include +#include #include @@ -32,8 +32,8 @@ using Point = bg::model::d2::point_xy; using Polygon = bg::model::polygon; using Line = bg::model::linestring; -using motion_utils::calcSignedArcLength; -using tier4_autoware_utils::createPoint; +using autoware::motion_utils::calcSignedArcLength; +using autoware::universe_utils::createPoint; PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLanePolygon( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.hpp similarity index 100% rename from planning/behavior_velocity_no_drivable_lane_module/src/util.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/util.hpp diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/README.md similarity index 100% rename from planning/autoware_behavior_velocity_no_stopping_area_module/README.md rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/README.md diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml similarity index 100% rename from planning/autoware_behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/docs/no-stopping-area.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/docs/no-stopping-area.svg similarity index 100% rename from planning/autoware_behavior_velocity_no_stopping_area_module/docs/no-stopping-area.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/docs/no-stopping-area.svg diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml similarity index 95% rename from planning/autoware_behavior_velocity_no_stopping_area_module/package.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 1a2d435fb6a4a..eab1a13d7ebf1 100644 --- a/planning/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -18,22 +18,22 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils eigen geometry_msgs interpolation lanelet2_extension libboost-dev - motion_utils pluginlib rclcpp tf2 tf2_eigen tf2_geometry_msgs - tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/plugins.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_velocity_no_stopping_area_module/plugins.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/plugins.xml diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp similarity index 86% rename from planning/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp index e90273d4ac5e4..09f1908b107d5 100644 --- a/planning/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -14,11 +14,11 @@ #include "scene_no_stopping_area.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -34,10 +34,10 @@ namespace { const double marker_lifetime = 0.2; using DebugData = NoStoppingAreaModule::DebugData; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerScale; lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) { @@ -162,15 +162,15 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createDebugMarkerArra return debug_marker_array; } -motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWall wall; wall.ns = std::to_string(module_id_) + "_"; wall.text = "no_stopping_area"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware::motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { - wall.pose = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + wall.pose = autoware::universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } return virtual_walls; diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp similarity index 95% rename from planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 31682fe0419d2..4ee711297b494 100644 --- a/planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -14,9 +14,9 @@ #include "manager.hpp" -#include +#include +#include #include -#include #include @@ -30,8 +30,8 @@ namespace autoware::behavior_velocity_planner { +using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::NoStoppingArea; -using tier4_autoware_utils::getOrDeclareParameter; NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp similarity index 89% rename from planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp index 696115d4bda6d..1abd36d585d51 100644 --- a/planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_no_stopping_area.hpp" -#include -#include -#include +#include +#include +#include #include #include diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp similarity index 90% rename from planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index a89161de431ae..d007416dccb47 100644 --- a/planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -14,13 +14,13 @@ #include "scene_no_stopping_area.hpp" -#include -#include -#include +#include +#include +#include +#include +#include +#include #include -#include -#include -#include #include #include @@ -87,7 +87,7 @@ boost::optional NoStoppingAreaModule::getStopLineGeometry2d( std::vector collision_points; bg::intersection(area_poly, line, collision_points); if (!collision_points.empty()) { - const double yaw = tier4_autoware_utils::calcAzimuthAngle(p0, p1); + const double yaw = autoware::universe_utils::calcAzimuthAngle(p0, p1); const double w = planner_data_->vehicle_info_.vehicle_width_m; const double l = stop_line_margin; stop_line.emplace_back( @@ -132,7 +132,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason return true; } const auto & stop_pose = stop_point->second; - setDistance(motion_utils::calcSignedArcLength( + setDistance(autoware::motion_utils::calcSignedArcLength( original_path.points, current_pose->pose.position, stop_pose.position)); if (planning_utils::isOverLine( original_path, current_pose->pose, stop_pose, planner_param_.dead_line_margin)) { @@ -229,7 +229,7 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( continue; // not stop vehicle } // check if the footprint is in the stuck detect area - const Polygon2d obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const Polygon2d obj_footprint = autoware::universe_utils::toPolygon2d(object); const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly); if (is_in_stuck_area) { RCLCPP_DEBUG(logger_, "stuck vehicle found."); @@ -251,6 +251,10 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly) { const double stop_vel = std::numeric_limits::min(); + + // if the detected stop point is near goal, it's ignored. + static constexpr double close_to_goal_distance = 1.0; + // stuck points by stop line for (size_t i = 0; i < path.points.size() - 1; ++i) { const auto p0 = path.points.at(i).point.pose.position; @@ -260,6 +264,14 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( if (v0 > stop_vel && v1 > stop_vel) { continue; } + // judge if stop point p0 is near goal, by its distance to the path end. + const double dist_to_path_end = + autoware::motion_utils::calcSignedArcLength(path.points, i, path.points.size() - 1); + if (dist_to_path_end < close_to_goal_distance) { + // exit with false, cause position is near goal. + return false; + } + const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; std::vector collision_points; bg::intersection(poly, line, collision_points); @@ -290,10 +302,10 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( auto & pp = interpolated_path.points; /* calc closest index */ const auto closest_idx_opt = - motion_utils::findNearestIndex(interpolated_path.points, ego_pose, 3.0, M_PI_4); + autoware::motion_utils::findNearestIndex(interpolated_path.points, ego_pose, 3.0, M_PI_4); if (!closest_idx_opt) { RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, *clock_, 1000 /* ms */, "motion_utils::findNearestIndex fail"); + logger_, *clock_, 1000 /* ms */, "autoware::motion_utils::findNearestIndex fail"); return ego_area; } const size_t closest_idx = closest_idx_opt.value(); @@ -306,7 +318,7 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( } const auto no_stopping_area = no_stopping_area_reg_elem_.noStoppingAreas().front(); for (size_t i = closest_idx + num_ignore_nearest; i < pp.size() - 1; ++i) { - dist_from_start_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); const auto & p = pp.at(i).point.pose.position; if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { is_in_area = true; @@ -317,7 +329,9 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( } ++ego_area_start_idx; } - + if (ego_area_start_idx > num_ignore_nearest) { + ego_area_start_idx--; + } if (!is_in_area) { return ego_area; } @@ -325,15 +339,10 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( // decide end idx with extract distance size_t ego_area_end_idx = ego_area_start_idx; for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { - dist_from_start_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); const auto & p = pp.at(i).point.pose.position; if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { - dist_from_area_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); - - // do not take extra distance and exit as soon as p is outside no stopping area - // just a temporary fix - ego_area_end_idx = i - 1; - break; + dist_from_area_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); } if (dist_from_start_sum > extra_dist || dist_from_area_sum > margin) { break; diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp similarity index 96% rename from planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index e7e66775f368e..ae297d96c5afd 100644 --- a/planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -18,9 +18,9 @@ #define EIGEN_MPL2_ONLY #include -#include -#include -#include +#include +#include +#include #include #include @@ -86,7 +86,7 @@ class NoStoppingAreaModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware::motion_utils::VirtualWalls createVirtualWalls() override; private: const int64_t lane_id_; diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/README.md similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/README.md rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/README.md diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/docs/da.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/da.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/docs/da.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/da.drawio.svg diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml similarity index 92% rename from planning/autoware_behavior_velocity_occlusion_spot_module/package.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml index e229a69032c27..34fd712d2229c 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -17,24 +17,24 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_grid_map_utils + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_vehicle_info_utils geometry_msgs grid_map_ros - grid_map_utils interpolation lanelet2_extension libboost-dev libopencv-dev - motion_utils nav_msgs pluginlib rclcpp tf2 tf2_geometry_msgs - tier4_autoware_utils visualization_msgs ament_cmake_ros diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/plugins.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/plugins.xml similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/plugins.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/plugins.xml diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp similarity index 90% rename from planning/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp index 40e5b8f91e6b9..a32cb4018cda9 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -15,10 +15,10 @@ #include "occlusion_spot_utils.hpp" #include "scene_occlusion_spot.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -30,13 +30,13 @@ namespace { using builtin_interfaces::msg::Time; using BasicPolygons = std::vector; +using autoware::universe_utils::appendMarkerArray; +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createMarkerColor; +using autoware::universe_utils::createMarkerOrientation; +using autoware::universe_utils::createMarkerPosition; +using autoware::universe_utils::createMarkerScale; using occlusion_spot_utils::PossibleCollisionInfo; -using tier4_autoware_utils::appendMarkerArray; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; -using tier4_autoware_utils::createMarkerPosition; -using tier4_autoware_utils::createMarkerScale; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -214,12 +214,12 @@ MarkerArray OcclusionSpotModule::createDebugMarkerArray() return debug_marker_array; } -motion_utils::VirtualWalls OcclusionSpotModule::createVirtualWalls() +autoware::motion_utils::VirtualWalls OcclusionSpotModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWall wall; wall.text = "occlusion_spot"; - wall.style = motion_utils::VirtualWallType::slowdown; + wall.style = autoware::motion_utils::VirtualWallType::slowdown; for (size_t id = 0; id < debug_data_.debug_poses.size(); id++) { wall.pose = calcOffsetPose(debug_data_.debug_poses.at(id), debug_data_.baselink_to_front, 0.0, 0.0); diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp similarity index 96% rename from planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index 6ab88baf77b20..783970c701262 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -52,8 +52,8 @@ void findOcclusionSpots( for (const auto & point : polygon.outer()) { grid_polygon.addVertex({point.x(), point.y()}); } - for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); - ++iterator) { + for (autoware::grid_map_utils::PolygonIterator iterator(grid, grid_polygon); + !iterator.isPastEnd(); ++iterator) { const grid_map::Index & index = *iterator; if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::UNKNOWN) { grid_map::Position occlusion_spot_position; @@ -77,8 +77,8 @@ bool isCollisionFree( for (const auto & point : polygon.outer()) { grid_polygon.addVertex({point.x(), point.y()}); } - for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); - ++iterator) { + for (autoware::grid_map_utils::PolygonIterator iterator(grid, grid_polygon); + !iterator.isPastEnd(); ++iterator) { const grid_map::Index & index = *iterator; if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) { return false; @@ -95,7 +95,7 @@ std::optional generateOcclusionPolygon( const Polygon2d & occupancy_poly, const Point2d & origin, const Point2d & min_theta_pos, const Point2d & max_theta_pos, const double ray_max_length = 100.0) { - using tier4_autoware_utils::normalizeRadian; + using autoware::universe_utils::normalizeRadian; const double origin_x = origin.x(); const double origin_y = origin.y(); const double min_theta = @@ -134,7 +134,7 @@ std::optional generateOcclusionPolygon( Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, const double r = 100) { - using tier4_autoware_utils::calcOffsetPose; + using autoware::universe_utils::calcOffsetPose; // generate occupancy polygon from grid origin Polygon2d poly; // create counter clockwise poly poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, 0, 0).position)); @@ -155,7 +155,7 @@ std::pair calcEdgePoint(const Polygon2d & foot_print, const Poin for (size_t i = 0; i < foot_print.outer().size(); i++) { const auto & f = foot_print.outer().at(i); PolarCoordinates polar = toPolarCoordinates(origin, f); - const double theta_norm = tier4_autoware_utils::normalizeRadian(polar.theta, 0.0); + const double theta_norm = autoware::universe_utils::normalizeRadian(polar.theta, 0.0); if (theta_norm < min_theta) { min_theta = theta_norm; min_idx = i; @@ -183,7 +183,7 @@ std::optional generateOccupiedPolygon( Point transformFromMap2Grid(const TransformStamped & geom_tf_map2grid, const Point2d & p) { - Point geom_pt = tier4_autoware_utils::createPoint(p.x(), p.y(), 0); + Point geom_pt = autoware::universe_utils::createPoint(p.x(), p.y(), 0); Point transformed_geom_pt; // from map coordinate to occupancy grid coordinate tf2::doTransform(geom_pt, transformed_geom_pt, geom_tf_map2grid); diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp similarity index 91% rename from planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 8dc59d07173dc..d716f17751dc0 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -15,15 +15,15 @@ #ifndef GRID_UTILS_HPP_ #define GRID_UTILS_HPP_ -#include -#include +#include +#include +#include +#include +#include #include #include #include -#include #include -#include -#include #include #include @@ -55,15 +55,15 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; namespace grid_utils { +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::MapMetaData; using nav_msgs::msg::OccupancyGrid; -using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; namespace occlusion_cost_value { diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp similarity index 97% rename from planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index c1d345cbc9aac..5e686cf648c50 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -16,8 +16,8 @@ #include "scene_occlusion_spot.hpp" -#include -#include +#include +#include #include @@ -27,9 +27,9 @@ namespace autoware::behavior_velocity_planner { +using autoware::universe_utils::getOrDeclareParameter; using occlusion_spot_utils::DETECTION_METHOD; using occlusion_spot_utils::PASS_JUDGE; -using tier4_autoware_utils::getOrDeclareParameter; OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp similarity index 91% rename from planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp index 123d0ef32afc1..32b1818214952 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -18,9 +18,9 @@ #include "occlusion_spot_utils.hpp" #include "scene_occlusion_spot.hpp" -#include -#include -#include +#include +#include +#include #include #include diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp similarity index 95% rename from planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index ed195023ce67a..911842ddd57ed 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -16,13 +16,13 @@ #include "risk_predictive_braking.hpp" -#include -#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include #include #include @@ -41,7 +41,7 @@ namespace occlusion_spot_utils Polygon2d toFootprintPolygon(const PredictedObject & object, const double scale = 1.0) { const Pose & obj_pose = object.kinematics.initial_pose_with_covariance.pose; - Polygon2d obj_footprint = tier4_autoware_utils::toPolygon2d(object); + Polygon2d obj_footprint = autoware::universe_utils::toPolygon2d(object); // upscale foot print for noise obj_footprint = upScalePolygon(obj_pose.position, obj_footprint, scale); return obj_footprint; @@ -126,7 +126,7 @@ void calcSlowDownPointsForPossibleCollision( const double dist_to_col = possible_collisions.at(collision_index).arc_lane_dist_at_collision.length; dist_along_next_path_point += - tier4_autoware_utils::calcDistance2d(p_prev.pose.position, p_next.pose.position); + autoware::universe_utils::calcDistance2d(p_prev.pose.position, p_next.pose.position); // process if nearest possible collision is between current and next path point if (dist_along_path_point < dist_to_col) { for (; collision_index < possible_collisions.size(); collision_index++) { @@ -173,7 +173,8 @@ void clipPathByLength( double length_sum = 0; clipped.points.emplace_back(path.points.front()); for (int i = 1; i < static_cast(path.points.size()); i++) { - length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i)); + length_sum += + autoware::universe_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i)); if (length_sum > max_length) return; clipped.points.emplace_back(path.points.at(i)); } @@ -248,7 +249,7 @@ void categorizeVehicles( lanelet::ArcCoordinates getOcclusionPoint( const PredictedObject & obj, const lanelet::ConstLineString2d & ll_string) { - const auto poly = tier4_autoware_utils::toPolygon2d(obj); + const auto poly = autoware::universe_utils::toPolygon2d(obj); std::deque arcs; for (const auto & p : poly.outer()) { lanelet::BasicPoint2d obj_p = {p.x(), p.y()}; @@ -374,7 +375,7 @@ std::vector filterVehiclesByDetectionArea( // stuck points by predicted objects for (const auto & object : objs) { // check if the footprint is in the stuck detect area - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const auto obj_footprint = autoware::universe_utils::toPolygon2d(object); for (const auto & p : polys) { if (!bg::disjoint(obj_footprint, p)) { filtered_obj.emplace_back(object); @@ -402,8 +403,8 @@ bool generatePossibleCollisionsFromGridMap( param.detection_area.min_occlusion_spot_size); if (param.is_show_occlusion) { for (const auto & op : occlusion_spot_positions) { - Point p = - tier4_autoware_utils::createPoint(op[0], op[1], path.points.at(0).point.pose.position.z); + Point p = autoware::universe_utils::createPoint( + op[0], op[1], path.points.at(0).point.pose.position.z); debug_data.occlusion_points.emplace_back(p); } } diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp similarity index 98% rename from planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 542c869919339..189a5be5abe95 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -17,11 +17,11 @@ #include "grid_utils.hpp" -#include +#include +#include #include #include #include -#include #include #include diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp similarity index 98% rename from planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp index 9fe06273ea617..59434fd2f4cef 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp @@ -16,7 +16,7 @@ #include "occlusion_spot_utils.hpp" -#include +#include #include #include diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp similarity index 94% rename from planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index e5c5eaec346ff..b55869be8c902 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -17,10 +17,10 @@ #include "occlusion_spot_utils.hpp" #include "risk_predictive_braking.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -110,12 +110,12 @@ bool OcclusionSpotModule::modifyPathVelocity( //! never change this interpolation interval(will affect module accuracy) splineInterpolate(clipped_path, 1.0, path_interpolated, logger_); const geometry_msgs::msg::Point start_point = path_interpolated.points.at(0).point.pose.position; - const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( + const auto ego_segment_idx = autoware::motion_utils::findNearestSegmentIndex( path_interpolated.points, ego_pose, param_.dist_thr, param_.angle_thr); if (!ego_segment_idx) return true; const size_t start_point_segment_idx = - motion_utils::findNearestSegmentIndex(path_interpolated.points, start_point); - const auto offset = motion_utils::calcSignedArcLength( + autoware::motion_utils::findNearestSegmentIndex(path_interpolated.points, start_point); + const auto offset = autoware::motion_utils::calcSignedArcLength( path_interpolated.points, ego_pose.position, *ego_segment_idx, start_point, start_point_segment_idx); const double offset_from_start_to_ego = -offset; diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp similarity index 86% rename from planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 00126a2ebc39a..b83051fb6b6ec 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -17,10 +17,10 @@ #include "occlusion_spot_utils.hpp" -#include -#include +#include +#include +#include #include -#include #include #include @@ -56,12 +56,12 @@ class OcclusionSpotModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware::motion_utils::VirtualWalls createVirtualWalls() override; private: // Parameter PlannerParam param_; - tier4_autoware_utils::StopWatch stop_watch_; + autoware::universe_utils::StopWatch stop_watch_; std::vector partition_lanelets_; protected: diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp similarity index 94% rename from planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index a21e0333082a9..6bc9423b6dc96 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -15,8 +15,8 @@ #include "grid_utils.hpp" #include "utils.hpp" -#include -#include +#include +#include #include @@ -85,7 +85,7 @@ TEST(compareTime, polygon_vs_line_iterator) } } const grid_map::Matrix & grid_data = grid["layer"]; - tier4_autoware_utils::StopWatch stop_watch; + autoware::universe_utils::StopWatch stop_watch; stop_watch.tic("processing_time"); size_t count = 0; [[maybe_unused]] double time = 0; @@ -102,8 +102,8 @@ TEST(compareTime, polygon_vs_line_iterator) for (const auto & point : polygon.outer()) { grid_polygon.addVertex({point.x(), point.y()}); } - for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); - ++iterator) { + for (autoware::grid_map_utils::PolygonIterator iterator(grid, grid_polygon); + !iterator.isPastEnd(); ++iterator) { const grid_map::Index & index = *iterator; if (grid_data(index.x(), index.y()) == OCCUPIED) { std::cout << "occupied" << std::endl; diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp similarity index 97% rename from planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 60ef5d62a5b7b..0322bda0a88c9 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -16,8 +16,8 @@ #include "occlusion_spot_utils.hpp" #include "utils.hpp" -#include -#include +#include +#include #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/vector3.hpp" diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp similarity index 100% rename from planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp similarity index 98% rename from planning/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp index 4b8a7ae3b2f3f..39a887ab16476 100644 --- a/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -86,7 +86,7 @@ inline void generatePossibleCollisions( // intersection geometry_msgs::msg::Pose intersection_pose{}; intersection_pose.position.x = x0 + x_step * i + lon; - intersection_pose.position.x = y0 + y_step * i; + intersection_pose.position.y = y0 + y_step * i; // collision path point autoware_planning_msgs::msg::PathPoint collision_with_margin{}; diff --git a/planning/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_velocity_planner/CMakeLists.txt rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt diff --git a/planning/autoware_behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md similarity index 96% rename from planning/autoware_behavior_velocity_planner/README.md rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md index 5b6ec1f6fa0e5..f688318ed051e 100644 --- a/planning/autoware_behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md @@ -10,17 +10,16 @@ It loads modules as plugins. Please refer to the links listed below for detail o - [Blind Spot](../autoware_behavior_velocity_blind_spot_module/README.md) - [Crosswalk](../autoware_behavior_velocity_crosswalk_module/README.md) - [Walkway](../autoware_behavior_velocity_walkway_module/README.md) -- [Detection Area](../behavior_velocity_detection_area_module/README.md) +- [Detection Area](../autoware_behavior_velocity_detection_area_module/README.md) - [Intersection](../autoware_behavior_velocity_intersection_module/README.md) -- [MergeFromPrivate](../behavior_velocity_intersection_module/README.md#merge-from-private) +- [MergeFromPrivate](../autoware_behavior_velocity_intersection_module/README.md#merge-from-private) - [Stop Line](../autoware_behavior_velocity_stop_line_module/README.md) - [Virtual Traffic Light](../autoware_behavior_velocity_virtual_traffic_light_module/README.md) - [Traffic Light](../autoware_behavior_velocity_traffic_light_module/README.md) - [Occlusion Spot](../autoware_behavior_velocity_occlusion_spot_module/README.md) -- [No Stopping Area](../behavior_velocity_no_stopping_area_module/README.md) +- [No Stopping Area](../autoware_behavior_velocity_no_stopping_area_module/README.md) - [Run Out](../autoware_behavior_velocity_run_out_module/README.md) - [Speed Bump](../behavior_velocity_speed_bump_module/README.md) -- [Out of Lane](../behavior_velocity_out_of_lane_module/README.md) When each module plans velocity, it considers based on `base_link`(center of rear-wheel axis) pose. So for example, in order to stop at a stop line with the vehicles' front on the stop line, it calculates `base_link` position from the distance between `base_link` to front and modifies path velocity from the `base_link` position. diff --git a/planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml similarity index 100% rename from planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml diff --git a/planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg diff --git a/planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg diff --git a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml similarity index 96% rename from planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index f52d9637c2134..3c4f4c3fdd64c 100644 --- a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -25,7 +25,6 @@ - @@ -68,7 +67,6 @@ - diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml similarity index 94% rename from planning/autoware_behavior_velocity_planner/package.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index dde7ab91d70b2..24d11eb9d08d5 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -36,16 +36,17 @@ autoware_behavior_velocity_planner_common autoware_map_msgs + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_universe_utils autoware_velocity_smoother diagnostic_msgs eigen geometry_msgs lanelet2_extension libboost-dev - motion_utils pcl_conversions pluginlib rclcpp @@ -56,7 +57,6 @@ tf2_geometry_msgs tf2_ros tier4_api_msgs - tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs visualization_msgs @@ -68,7 +68,9 @@ autoware_behavior_velocity_blind_spot_module autoware_behavior_velocity_blind_spot_module autoware_behavior_velocity_crosswalk_module + autoware_behavior_velocity_detection_area_module autoware_behavior_velocity_intersection_module + autoware_behavior_velocity_no_drivable_lane_module autoware_behavior_velocity_no_stopping_area_module autoware_behavior_velocity_occlusion_spot_module autoware_behavior_velocity_run_out_module @@ -77,8 +79,6 @@ autoware_behavior_velocity_virtual_traffic_light_module autoware_behavior_velocity_walkway_module autoware_lint_common - behavior_velocity_detection_area_module - behavior_velocity_no_drivable_lane_module behavior_velocity_speed_bump_module diff --git a/planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json similarity index 100% rename from planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp similarity index 95% rename from planning/autoware_behavior_velocity_planner/src/node.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index 40457ce4b30f3..cbacbe625238e 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -14,13 +14,13 @@ #include "node.hpp" -#include -#include +#include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include #include #include @@ -130,8 +130,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_manager_.launchScenePlugin(*this, name); } - logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); } void BehaviorVelocityPlannerNode::onLoadPlugin( @@ -193,7 +194,7 @@ void BehaviorVelocityPlannerNode::processNoGroundPointCloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); if (!pc.empty()) { - tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); + autoware::universe_utils::transformPointCloud(pc, *pc_transformed, affine); } planner_data_.no_ground_pointcloud = pc_transformed; @@ -378,7 +379,7 @@ autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( autoware_planning_msgs::msg::Path output_path_msg; // TODO(someone): support backward path - const auto is_driving_forward = motion_utils::isDrivingForward(input_path_msg->points); + const auto is_driving_forward = autoware::motion_utils::isDrivingForward(input_path_msg->points); is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; if (!is_driving_forward_) { RCLCPP_WARN_THROTTLE( diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp similarity index 82% rename from planning/autoware_behavior_velocity_planner/src/node.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp index 73dfdb15e2d8d..fab047029e29c 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp @@ -15,15 +15,15 @@ #ifndef NODE_HPP_ #define NODE_HPP_ +#include "autoware/universe_utils/ros/logger_level_configure.hpp" +#include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "planner_manager.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include +#include #include #include -#include #include -#include #include #include @@ -69,29 +69,30 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; // polling subscribers - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::PredictedObjects> sub_predicted_objects_{this, "~/input/dynamic_objects"}; - tier4_autoware_utils::InterProcessPollingSubscriber - sub_no_ground_pointcloud_{this, "~/input/no_ground_pointcloud"}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_no_ground_pointcloud_{ + this, "~/input/no_ground_pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< geometry_msgs::msg::AccelWithCovarianceStamped> sub_acceleration_{this, "~/input/accel"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - tier4_autoware_utils::InterProcessPollingSubscriber< + autoware::universe_utils::InterProcessPollingSubscriber< tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; - tier4_autoware_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_occupancy_grid_{this, "~/input/occupancy_grid"}; void onTrigger(const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); @@ -142,9 +143,9 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); - std::unique_ptr logger_configure_; + std::unique_ptr logger_configure_; - std::unique_ptr published_time_publisher_; + std::unique_ptr published_time_publisher_; static constexpr int logger_throttle_interval = 3000; }; diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp similarity index 100% rename from planning/autoware_behavior_velocity_planner/src/planner_manager.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp similarity index 94% rename from planning/autoware_behavior_velocity_planner/src/planner_manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp index 73193a002918d..abeab16dd71cb 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -15,8 +15,8 @@ #ifndef PLANNER_MANAGER_HPP_ #define PLANNER_MANAGER_HPP_ -#include -#include +#include +#include #include #include diff --git a/planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv similarity index 100% rename from planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv diff --git a/planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv similarity index 100% rename from planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp similarity index 98% rename from planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 113edb6fcf7b4..29cd7a215907f 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -98,7 +98,7 @@ std::shared_ptr generateNode() get_behavior_velocity_module_config("blind_spot"), get_behavior_velocity_module_config("crosswalk"), get_behavior_velocity_module_config("walkway"), - get_behavior_velocity_module_config_no_prefix("detection_area"), + get_behavior_velocity_module_config("detection_area"), get_behavior_velocity_module_config("intersection"), get_behavior_velocity_module_config("no_stopping_area"), get_behavior_velocity_module_config("occlusion_spot"), @@ -107,7 +107,7 @@ std::shared_ptr generateNode() get_behavior_velocity_module_config("stop_line"), get_behavior_velocity_module_config("traffic_light"), get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config_no_prefix("no_drivable_lane")}); + get_behavior_velocity_module_config("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light diff --git a/planning/autoware_behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt similarity index 100% rename from planning/autoware_behavior_velocity_planner_common/CMakeLists.txt rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt diff --git a/planning/autoware_behavior_velocity_planner_common/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md similarity index 100% rename from planning/autoware_behavior_velocity_planner_common/README.md rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp similarity index 93% rename from planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index 068a10b7e16f3..6e7f8b679d32a 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ -#include "autoware_route_handler/route_handler.hpp" +#include "autoware/route_handler/route_handler.hpp" -#include +#include +#include #include -#include #include #include @@ -150,4 +150,4 @@ struct PlannerData }; } // namespace autoware::behavior_velocity_planner -#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp similarity index 81% rename from planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp index 86579f06790b2..b163cd31e3030 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ -#include +#include #include #include @@ -40,4 +40,4 @@ class PluginInterface } // namespace autoware::behavior_velocity_planner -#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp similarity index 84% rename from planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp index e82211937e55b..0e32ff958c0a5 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ -#include +#include #include #include @@ -50,4 +50,4 @@ class PluginWrapper : public PluginInterface } // namespace autoware::behavior_velocity_planner -#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp similarity index 89% rename from planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index bc929991199ac..b09f00ce367dc 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ - -#include -#include -#include +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ + +#include +#include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include #include #include @@ -48,14 +48,14 @@ namespace autoware::behavior_velocity_planner { +using autoware::motion_utils::PlanningBehavior; +using autoware::motion_utils::VelocityFactor; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::rtc_interface::RTCInterface; +using autoware::universe_utils::DebugPublisher; +using autoware::universe_utils::getOrDeclareParameter; using builtin_interfaces::msg::Time; -using motion_utils::PlanningBehavior; -using motion_utils::VelocityFactor; -using tier4_autoware_utils::DebugPublisher; -using tier4_autoware_utils::getOrDeclareParameter; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; @@ -87,7 +87,7 @@ class SceneModuleInterface virtual bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) = 0; virtual visualization_msgs::msg::MarkerArray createDebugMarkerArray() = 0; - virtual std::vector createVirtualWalls() = 0; + virtual std::vector createVirtualWalls() = 0; int64_t getModuleId() const { return module_id_; } void setPlannerData(const std::shared_ptr & planner_data) @@ -130,7 +130,7 @@ class SceneModuleInterface std::shared_ptr planner_data_; std::optional infrastructure_command_; std::optional first_stop_path_point_index_; - motion_utils::VelocityFactorInterface velocity_factor_; + autoware::motion_utils::VelocityFactorInterface velocity_factor_; std::vector objects_of_interest_; void setSafe(const bool safe) @@ -197,7 +197,7 @@ class SceneModuleManagerInterface std::set registered_module_id_set_; std::shared_ptr planner_data_; - motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_; + autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_; std::optional first_stop_path_point_index_; rclcpp::Node & node_; @@ -278,4 +278,4 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface } // namespace autoware::behavior_velocity_planner -#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp similarity index 82% rename from planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 5ea1ae9fffcc1..77b7d25f9f46e 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ -#include -#include +#include +#include #include @@ -31,7 +31,7 @@ namespace autoware::behavior_velocity_planner { namespace { -geometry_msgs::msg::Point convertToGeomPoint(const tier4_autoware_utils::Point2d & p) +geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); @@ -46,7 +46,7 @@ namespace arc_lane_utils { using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = - std::pair; // front index, point2d + std::pair; // front index, point2d using PathIndexWithPoint = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset @@ -76,9 +76,9 @@ std::optional findCollisionSegment( } const auto & p1 = - tier4_autoware_utils::getPoint(path.points.at(i)); // Point before collision point + autoware::universe_utils::getPoint(path.points.at(i)); // Point before collision point const auto & p2 = - tier4_autoware_utils::getPoint(path.points.at(i + 1)); // Point after collision point + autoware::universe_utils::getPoint(path.points.at(i + 1)); // Point after collision point const auto collision_point = checkCollision(p1, p2, stop_line_p1, stop_line_p2); @@ -106,7 +106,8 @@ std::optional findForwardOffsetSegment( { double sum_length = 0.0; for (size_t i = base_idx; i < path.points.size() - 1; ++i) { - sum_length += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + sum_length += + autoware::universe_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length if (sum_length >= offset_length) { @@ -125,7 +126,8 @@ std::optional findBackwardOffsetSegment( double sum_length = 0.0; const auto start = static_cast(base_idx) - 1; for (std::int32_t i = start; i >= 0; --i) { - sum_length += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + sum_length += + autoware::universe_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length if (sum_length >= offset_length) { @@ -149,13 +151,13 @@ std::optional findOffsetSegment( return findForwardOffsetSegment( path, collision_idx, offset_length + - tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx), collision_point)); + autoware::universe_utils::calcDistance2d(path.points.at(collision_idx), collision_point)); } return findBackwardOffsetSegment( path, collision_idx + 1, -offset_length + - tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); + autoware::universe_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); } std::optional findOffsetSegment( @@ -185,8 +187,8 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse target_pose.position.x = target_point_2d.x(); target_pose.position.y = target_point_2d.y(); target_pose.position.z = interpolated_z; - const double yaw = tier4_autoware_utils::calcAzimuthAngle(p_front, p_back); - target_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + const double yaw = autoware::universe_utils::calcAzimuthAngle(p_front, p_back); + target_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); return target_pose; } @@ -197,4 +199,4 @@ std::optional createTargetPoint( } // namespace arc_lane_utils } // namespace autoware::behavior_velocity_planner -#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp similarity index 83% rename from planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 22bba2b90dcc2..696d4a41b7673 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ -#include +#include #include #include @@ -50,9 +50,9 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using Point2d = tier4_autoware_utils::Point2d; -using LineString2d = tier4_autoware_utils::LineString2d; -using Polygon2d = tier4_autoware_utils::Polygon2d; +using Point2d = autoware::universe_utils::Point2d; +using LineString2d = autoware::universe_utils::LineString2d; +using Polygon2d = autoware::universe_utils::Polygon2d; template Point2d to_bg2d(const T & p) @@ -79,4 +79,4 @@ geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon); } // namespace autoware::behavior_velocity_planner -#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp similarity index 90% rename from planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp index c6e5d45ec4eb6..3a1c8fdeb7a0d 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ #include @@ -48,4 +48,4 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( const double r, const double g, const double b); } // namespace debug } // namespace autoware::behavior_velocity_planner -#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp similarity index 84% rename from planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp index e9bccc42e1dc4..1e45e5655f12f 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ #include @@ -35,4 +35,4 @@ autoware_planning_msgs::msg::Path filterStopPathPoint( const autoware_planning_msgs::msg::Path & path); } // namespace autoware::behavior_velocity_planner -#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp similarity index 91% rename from planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp index 12dd4db930745..4d88f2bfecce8 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ #include @@ -93,4 +93,4 @@ class StateMachine }; } // namespace autoware::behavior_velocity_planner -#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp similarity index 82% rename from planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index c5040f055c243..4dc355f0e6b1d 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ -#include +#include #include #include @@ -42,4 +42,4 @@ bool smoothPath( } // namespace autoware::behavior_velocity_planner -#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp similarity index 92% rename from planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp index 2fe728847b8f2..5f1c17ea1b815 100644 --- a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ -#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ -#include +#include #include #include @@ -57,9 +57,9 @@ struct TrafficSignalStamped }; using Pose = geometry_msgs::msg::Pose; -using Point2d = tier4_autoware_utils::Point2d; -using LineString2d = tier4_autoware_utils::LineString2d; -using Polygon2d = tier4_autoware_utils::Polygon2d; +using Point2d = autoware::universe_utils::Point2d; +using LineString2d = autoware::universe_utils::LineString2d; +using Polygon2d = autoware::universe_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; using autoware_perception_msgs::msg::PredictedObjects; @@ -228,19 +228,10 @@ std::set getAssociativeIntersectionLanelets( lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map, const lanelet::routing::RoutingGraphPtr routing_graph); -template