diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions new file mode 100644 index 0000000000000..5b140152e7f08 --- /dev/null +++ b/.cppcheck_suppressions @@ -0,0 +1,59 @@ +arrayIndexThenCheck +assignBoolToFloat +checkersReport +constParameterPointer +constParameterReference +constStatement +constVariable +constVariablePointer +constVariableReference +containerOutOfBounds +cstyleCast +ctuOneDefinitionRuleViolation +current_deleted_index +duplicateAssignExpression +duplicateBranch +duplicateBreak +duplicateCondition +duplicateExpression +funcArgNamesDifferent +functionConst +functionStatic +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 dc745760cb7d0..df8673cd68724 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,11 +1,9 @@ ### 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_auto_geometry/** satoshi.ota@tier4.jp -common/autoware_auto_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_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@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_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp @@ -18,70 +16,62 @@ 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/autoware_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/mission_planner_rviz_plugin/** isamu.takagi@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/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/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 common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp -common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp 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_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp -common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp +common/autoware_universe_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_control_rviz_plugin/** taiki.tanaka@tier4.jp common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp -common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp -common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp -common/tier4_screen_capture_rviz_plugin/** kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp -common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp -common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp +common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp khalil@leodrive.ai common/tier4_system_rviz_plugin/** koji.minoda@tier4.jp -common/tier4_target_object_type_rviz_plugin/** takamasa.horibe@tier4.jp common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autonomous_emergency_braking/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_external_cmd_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 +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_pid_longitudinal_controller/** mamoru.sobue@tier4.jp 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_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/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/external_cmd_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 -control/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/lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp -control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@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 tomoya.kimura@tier4.jp -control/pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@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/pure_pursuit/** takamasa.horibe@tier4.jp -control/shift_decider/** takamasa.horibe@tier4.jp -control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp -evaluator/control_evaluator/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp +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/autoware_control_evaluator/** daniel.sanchez@tier4.jp takayuki.murooka@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 +evaluator/autoware_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 -launch/tier4_map_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +launch/tier4_map_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 launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp zulfaqar.azmi@tier4.jp launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp @@ -110,14 +100,15 @@ localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tie localization/yabloc/yabloc_particle_filter/** 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_pose_initializer/** 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 map/map_height_fitter/** 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 -map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_loader/** 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 map/map_projection_loader/** 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 map/map_tf_generator/** 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 -map/util/lanelet2_map_preprocessor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/util/lanelet2_map_preprocessor/** 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 +perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp +perception/autoware_map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@tier4.jp perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp -perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp perception/detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -130,7 +121,7 @@ perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@ perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp perception/lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp -perception/map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp +perception/lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp perception/multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -155,67 +146,81 @@ 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/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_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_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 +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/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_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/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_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_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp -planning/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_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_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_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/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_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp -planning/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_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/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/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_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_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/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_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_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp -planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp -planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/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/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp -planning/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp -planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@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/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_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/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/obstacle_velocity_limiter/** maxime.clement@tier4.jp -planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp -planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp -planning/planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@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/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/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/bezier_sampler/** maxime.clement@tier4.jp -planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp -planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp -planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp -planning/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/static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp -planning/surround_obstacle_checker/** satoshi.ota@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 +planning/sampling_based_planner/autoware_sampler_common/** maxime.clement@tier4.jp sensing/gnss_poser/** 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 sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp sensing/image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp -sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp @@ -225,9 +230,8 @@ sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp simulator/fault_injection/** keisuke.shima@tier4.jp simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp -simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp -system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai system/bluetooth_monitor/** fumihito.ito@tier4.jp system/component_state_monitor/** isamu.takagi@tier4.jp system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp @@ -249,10 +253,11 @@ system/system_error_monitor/** fumihito.ito@tier4.jp 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 -vehicle/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp -vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp -vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp -vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp -vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@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_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_steer_offset_estimator/** taiki.tanaka@tier4.jp +vehicle/autoware_vehicle_info_utils/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp ### Copied from .github/CODEOWNERS-manual ### 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-arm64.yaml deleted file mode 100644 index a5e00496cc50a..0000000000000 --- a/.github/workflows/build-and-test-arm64.yaml +++ /dev/null @@ -1,55 +0,0 @@ -name: build-and-test-arm64 - -on: - schedule: - - cron: 0 0 * * * - workflow_dispatch: - -jobs: - build-and-test-arm64: - runs-on: [self-hosted, linux, ARM64] - container: ${{ matrix.container }}${{ matrix.container-suffix }} - strategy: - fail-fast: false - matrix: - rosdistro: - - humble - container-suffix: - - "" - - -cuda - include: - - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt - build-depends-repos: build_depends.repos - steps: - - name: Check out repository - uses: actions/checkout@v4 - - - 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 != '' }} - 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: Show disk space after the tasks - run: df -h diff --git a/.github/workflows/build-and-test-daily-arm64.yaml b/.github/workflows/build-and-test-daily-arm64.yaml new file mode 100644 index 0000000000000..ee9caae26e5d6 --- /dev/null +++ b/.github/workflows/build-and-test-daily-arm64.yaml @@ -0,0 +1,67 @@ +name: build-and-test-daily-arm64 + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + build-and-test-daily-arm64: + runs-on: [self-hosted, linux, ARM64] + 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-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 bd422af46bc4d..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: @@ -29,13 +29,17 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt + 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 3ad7b6d434f33..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: @@ -28,13 +33,17 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt + 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 @@ -74,40 +106,3 @@ jobs: - name: Show disk space after the tasks run: df -h - - clang-tidy-differential: - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt-cuda - steps: - - name: Check out repository - uses: actions/checkout@v4 - with: - fetch-depth: 0 - - - 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 modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - - name: Get modified files - id: get-modified-files - uses: tj-actions/changed-files@v35 - with: - files: | - **/*.cpp - **/*.hpp - - - name: Run clang-tidy - if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 - with: - rosdistro: humble - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - 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 diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 520c2cb9983b0..00860986824e6 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -2,14 +2,17 @@ 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: ubuntu-latest + runs-on: [self-hosted, linux, X64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false @@ -17,15 +20,16 @@ jobs: rosdistro: - humble container-suffix: - - "" - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt + 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 @@ -37,6 +41,25 @@ 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: 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 +68,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 +92,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 new file mode 100644 index 0000000000000..a59287a6d9b37 --- /dev/null +++ b/.github/workflows/clang-tidy-differential.yaml @@ -0,0 +1,61 @@ +name: clang-tidy-differential + +on: + pull_request: + types: + - opened + - synchronize + - reopened + - labeled + +jobs: + 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: 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: 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: + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} + + - 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 modified packages + id: get-modified-packages + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + + - name: Get modified files + id: get-modified-files + uses: tj-actions/changed-files@v42 + with: + files: | + **/*.cpp + **/*.hpp + + - name: Run clang-tidy + if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + with: + rosdistro: humble + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + 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/cppcheck-daily.yaml b/.github/workflows/cppcheck-daily.yaml new file mode 100644 index 0000000000000..6a18bfb988b08 --- /dev/null +++ b/.github/workflows/cppcheck-daily.yaml @@ -0,0 +1,59 @@ +name: cppcheck-daily + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + cppcheck-daily: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Run Cppcheck on all files + continue-on-error: true + id: cppcheck + run: | + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --xml . 2> cppcheck-report.xml + shell: bash + + - name: Count errors by error ID and severity + run: | + #!/bin/bash + temp_file=$(mktemp) + grep -oP '(?<=id=")[^"]+" severity="[^"]+' cppcheck-report.xml | sed 's/" severity="/,/g' > "$temp_file" + echo "Error counts by error ID and severity:" + sort "$temp_file" | uniq -c + rm "$temp_file" + shell: bash + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.xml + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml new file mode 100644 index 0000000000000..914abd7df86ea --- /dev/null +++ b/.github/workflows/cppcheck-differential.yaml @@ -0,0 +1,65 @@ +name: cppcheck-differential + +on: + pull_request: + +jobs: + cppcheck-differential: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Get changed files + id: changed-files + run: | + git fetch origin ${{ github.base_ref }} --depth=1 + git diff --name-only FETCH_HEAD ${{ github.sha }} > changed_files.txt + cat changed_files.txt + + - name: Run Cppcheck on changed files + continue-on-error: true + id: cppcheck + run: | + files=$(cat changed_files.txt | grep -E '\.(cpp|hpp)$' || true) + if [ -n "$files" ]; then + echo "Running Cppcheck on changed files: $files" + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions $files 2> cppcheck-report.txt + else + echo "No C++ files changed." + touch cppcheck-report.txt + fi + shell: bash + + - name: Show cppcheck-report result + run: | + cat cppcheck-report.txt + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.txt + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/.github/workflows/dco.yaml b/.github/workflows/dco.yaml new file mode 100644 index 0000000000000..db7ace467c658 --- /dev/null +++ b/.github/workflows/dco.yaml @@ -0,0 +1,21 @@ +name: DCO +# ref: https://github.com/anchore/syft/pull/2926/files +on: + pull_request: +jobs: + dco: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Setup Python 3.x + uses: actions/setup-python@v5 + with: + python-version: 3.x + + - name: Check DCO + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + run: | + pip3 install -U dco-check + dco-check --verbose --exclude-pattern 'pre-commit-ci\[bot\]@users\.noreply\.github\.com' 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/openai-pr-reviewer.yaml b/.github/workflows/openai-pr-reviewer.yaml deleted file mode 100644 index 4ecbd93198276..0000000000000 --- a/.github/workflows/openai-pr-reviewer.yaml +++ /dev/null @@ -1,39 +0,0 @@ -name: Code Review By ChatGPT - -permissions: - contents: read - pull-requests: write - -on: - pull_request_target: - types: - - labeled - pull_request_review_comment: - types: [created] - -concurrency: - group: ${{ github.repository }}-${{ github.event.number || github.head_ref || - github.sha }}-${{ github.workflow }}-${{ github.event_name == - 'pull_request_review_comment' && 'pr_comment' || 'pr' }} - cancel-in-progress: ${{ github.event_name != 'pull_request_review_comment' }} - -jobs: - prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 - with: - label: tag:openai-pr-reviewer - review: - needs: prevent-no-label-execution - if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} - runs-on: ubuntu-latest - steps: - - uses: fluxninja/openai-pr-reviewer@latest - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - OPENAI_API_KEY: ${{ secrets.OPENAI_API_KEY }} - with: - debug: false - review_simple_changes: false - review_comment_lgtm: false - openai_light_model: gpt-3.5-turbo-0613 - openai_heavy_model: gpt-3.5-turbo-0613 # The default is to use GPT4, which needs to be changed to join ChatGPT Plus. diff --git a/.github/workflows/pr-agent.yaml b/.github/workflows/pr-agent.yaml index f4794b7497402..81b192a1c2095 100644 --- a/.github/workflows/pr-agent.yaml +++ b/.github/workflows/pr-agent.yaml @@ -26,6 +26,13 @@ jobs: env: OPENAI_KEY: ${{ secrets.OPENAI_KEY_PR_AGENT }} GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - github_action_config.auto_review: "false" - github_action_config.auto_describe: "false" - github_action_config.auto_improve: "false" + github_action_config.auto_review: false + github_action_config.auto_describe: false + github_action_config.auto_improve: false + # https://github.com/Codium-ai/pr-agent/blob/main/pr_agent/settings/configuration.toml + pr_description.publish_labels: false + config.model: gpt-4o + config.model_turbo: gpt-4o + config.max_model_tokens: 64000 + pr_code_suggestions.max_context_tokens: 12000 + pr_code_suggestions.commitable_code_suggestions: true 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-daily.yaml b/.github/workflows/spell-check-daily.yaml new file mode 100644 index 0000000000000..dcee449abc728 --- /dev/null +++ b/.github/workflows/spell-check-daily.yaml @@ -0,0 +1,20 @@ +name: spell-check-daily + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + spell-check-daily: + 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 + 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/spell-check-partial.yaml b/.github/workflows/spell-check-partial.yaml deleted file mode 100644 index cf5eaa71e5741..0000000000000 --- a/.github/workflows/spell-check-partial.yaml +++ /dev/null @@ -1,21 +0,0 @@ -name: spell-check-partial - -on: - pull_request: - schedule: - - cron: 0 0 * * * - workflow_dispatch: - -jobs: - spell-check-partial: - 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: - cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json - local-cspell-json: .cspell.json - incremental-files-only: false 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/.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/build_depends.repos b/build_depends.repos index 32854cc34e362..bd93897d81d68 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -20,10 +20,6 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git version: main - core/external/autoware_auto_msgs: - type: git - url: https://github.com/tier4/autoware_auto_msgs.git - version: tier4/main # universe universe/external/tier4_autoware_msgs: type: git @@ -45,3 +41,7 @@ repositories: type: git url: https://github.com/tier4/glog.git version: v0.6.0_t4-ros + universe/external/ament_cmake: # TODO(mitsudome-r): remove when https://github.com/ament/ament_cmake/pull/448 is merged + type: git + url: https://github.com/autowarefoundation/ament_cmake.git + version: feat/faster_ament_libraries_deduplicate diff --git a/common/.pages b/common/.pages new file mode 100644 index 0000000000000..7a87ae2711e10 --- /dev/null +++ b/common/.pages @@ -0,0 +1,58 @@ +nav: + - 'Introduction': common + - 'Testing Libraries': + - 'autoware_testing': common/autoware_testing/design/autoware_testing-design + - 'fake_test_node': common/fake_test_node/design/fake_test_node-design + - 'Test Utils': common/autoware_test_utils + - '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 + - 'interpolation': common/interpolation + - 'Kalman Filter': common/kalman_filter + - '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 + - 'QP Interface': common/qp_interface/design/qp_interface-design + - 'Signal Processing': + - 'Introduction': common/signal_processing + - 'Butterworth Filter': common/signal_processing/documentation/ButterworthFilter + - 'TensorRT Common': common/tensorrt_common + - 'autoware_universe_utils': common/autoware_universe_utils + - 'traffic_light_utils': common/traffic_light_utils + - 'TVM Utility': + - 'Introduction': common/tvm_utility + - 'YOLOv2 Tiny Example': common/tvm_utility/tvm-utility-yolo-v2-tiny-tests + - 'RVIZ2 Plugins': + - 'autoware_perception_rviz_plugin': common/autoware_perception_rviz_plugin + - 'autoware_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin + - 'autoware_mission_details_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin + - 'bag_time_manager_rviz_plugin': common/bag_time_manager_rviz_plugin + - 'polar_grid': common/polar_grid/Readme + - 'tier4_adapi_rviz_plugin': common/tier4_adapi_rviz_plugin + - 'tier4_api_utils': common/tier4_api_utils + - 'tier4_camera_view_rviz_plugin': common/tier4_camera_view_rviz_plugin + - 'tier4_datetime_rviz_plugin': common/tier4_datetime_rviz_plugin + - 'tier4_localization_rviz_plugin': common/tier4_localization_rviz_plugin + - 'tier4_perception_rviz_plugin': common/tier4_perception_rviz_plugin + - 'tier4_planning_rviz_plugin': common/tier4_planning_rviz_plugin + - 'tier4_state_rviz_plugin': common/tier4_state_rviz_plugin + - 'tier4_system_rviz_plugin': common/tier4_system_rviz_plugin + - 'tier4_traffic_light_rviz_plugin': common/tier4_traffic_light_rviz_plugin + - 'tier4_vehicle_rviz_plugin': common/tier4_vehicle_rviz_plugin + - 'traffic_light_recognition_marker_publisher': common/traffic_light_recognition_marker_publisher/Readme + - 'Node': + - 'Goal Distance Calculator': common/goal_distance_calculator/Readme + - 'Path Distance Calculator': common/path_distance_calculator/Readme + - 'Others': + - 'autoware_ad_api_specs': common/autoware_ad_api_specs + - 'component_interface_specs': common/component_interface_specs + - 'component_interface_tools': common/component_interface_tools + - 'component_interface_utils': common/component_interface_utils diff --git a/common/README.md b/common/README.md new file mode 100644 index 0000000000000..05f85de61286e --- /dev/null +++ b/common/README.md @@ -0,0 +1,16 @@ +# Common + +## Getting Started + +The Autoware.Universe Common folder consists of common and testing libraries that are used by other Autoware components, as well as useful plugins for visualization in RVIZ2. + +!!! note + + In addition to the ones listed in this folder, users can also have a look at some of the add-ons in the `autoware_tools/common` documentation [page](https://autowarefoundation.github.io/autoware_tools/main/common/mission_planner_rviz_plugin/). + +## Highlights + +Some of the commonly used libraries are: + +1. `autoware_universe_utils` +2. `autoware_motion_utils` diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp new file mode 100644 index 0000000000000..09144c1d8ff50 --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp @@ -0,0 +1,36 @@ +// Copyright 2024 The Autoware Contributors +// +// 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_AD_API_SPECS__SYSTEM_HPP_ +#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ + +#include + +#include + +namespace autoware_ad_api::system +{ + +struct Heartbeat +{ + using Message = autoware_adapi_v1_msgs::msg::Heartbeat; + static constexpr char name[] = "/api/system/heartbeat"; + static constexpr size_t depth = 10; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace autoware_ad_api::system + +#endif // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ diff --git a/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp index d3bda57b3374f..cc2fb0e41c372 100644 --- a/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp @@ -36,7 +36,7 @@ using TimeStamp = builtin_interfaces::msg::Time; /// \brief Helper class to check existence of header file in compile time: /// https://stackoverflow.com/a/16000226/2325407 -template +template struct HasHeader : std::false_type { }; @@ -48,60 +48,60 @@ struct HasHeader : std::true_type /////////// Template declarations -/// Get frame id from message. nullptr_t is used to prevent template ambiguity on +/// Get frame id from message. std::nullptr_t is used to prevent template ambiguity on /// SFINAE specializations. Provide a default value on specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template const std::string & get_frame_id(const T & msg) noexcept; -/// Get a reference to the frame id from message. nullptr_t is used to prevent +/// Get a reference to the frame id from message. std::nullptr_t is used to prevent /// template ambiguity on SFINAE specializations. Provide a default value on /// specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template std::string & get_frame_id(T & msg) noexcept; -/// Get stamp from message. nullptr_t is used to prevent template ambiguity on +/// Get stamp from message. std::nullptr_t is used to prevent template ambiguity on /// SFINAE specializations. Provide a default value on specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template const TimeStamp & get_stamp(const T & msg) noexcept; -/// Get a reference to the stamp from message. nullptr_t is used to prevent +/// Get a reference to the stamp from message. std::nullptr_t is used to prevent /// template ambiguity on SFINAE specializations. Provide a default value on /// specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template TimeStamp & get_stamp(T & msg) noexcept; /////////////// Default specializations for message types that contain a header. -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> const std::string & get_frame_id(const T & msg) noexcept { return msg.header.frame_id; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> std::string & get_frame_id(T & msg) noexcept { return msg.header.frame_id; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> TimeStamp & get_stamp(T & msg) noexcept { return msg.header.stamp; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> TimeStamp get_stamp(const T & msg) noexcept { return msg.header.stamp; diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt deleted file mode 100644 index ee819b7cd797c..0000000000000 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_geometry) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - include/autoware_auto_geometry/spatial_hash.hpp - include/autoware_auto_geometry/intersection.hpp - include/autoware_auto_geometry/spatial_hash_config.hpp - src/spatial_hash.cpp - src/bounding_box.cpp -) - -if(BUILD_TESTING) - set(GEOMETRY_GTEST geometry_gtest) - set(GEOMETRY_SRC test/src/test_geometry.cpp - test/src/test_convex_hull.cpp - test/src/test_hull_pockets.cpp - test/src/test_interval.cpp - test/src/lookup_table.cpp - test/src/test_area.cpp - test/src/test_common_2d.cpp - test/src/test_intersection.cpp - ) - ament_add_ros_isolated_gtest(${GEOMETRY_GTEST} ${GEOMETRY_SRC}) - target_compile_options(${GEOMETRY_GTEST} PRIVATE -Wno-conversion -Wno-sign-conversion) - target_include_directories(${GEOMETRY_GTEST} PRIVATE "test/include" "include") - ament_target_dependencies(${GEOMETRY_GTEST} - "autoware_auto_common" - "autoware_auto_geometry_msgs" - "autoware_auto_planning_msgs" - "autoware_auto_vehicle_msgs" - "geometry_msgs" - "osrf_testing_tools_cpp") - target_link_libraries(${GEOMETRY_GTEST} ${PROJECT_NAME}) -endif() - -ament_auto_package() diff --git a/common/autoware_auto_geometry/design/interval.md b/common/autoware_auto_geometry/design/interval.md deleted file mode 100644 index 4fe65ff8e0310..0000000000000 --- a/common/autoware_auto_geometry/design/interval.md +++ /dev/null @@ -1,111 +0,0 @@ -# Interval - -The interval is a standard 1D real-valued interval. -The class implements a representation and operations on the interval type and guarantees interval validity on construction. -Basic operations and accessors are implemented, as well as other common operations. -See 'Example Usage' below. - -## Target use cases - -- Range or containment checks. - The interval class simplifies code that involves checking membership of a value to a range, or intersecting two ranges. - It also provides consistent behavior and consistent handling of edge cases. - -## Properties - -- **empty**: An empty interval is equivalent to an empty set. - It contains no elements. - It is a valid interval, but because it is empty, the notion of measure (length) is undefined; the measure of an empty interval is _not_ zero. - The implementation represents the measure of an empty interval with `NaN`. -- **zero measure**: An interval with zero measure is an interval whose bounds are exactly equal. - The measure is zero because the interval contains only a single point, and points have zero measure. - However, because it does contain a single element, the interval is _not_ empty. -- **valid**: A valid interval is either empty or has min/max bounds such that (min <= max). On construction, interval objects are guaranteed to be valid. - An attempt to construct an invalid interval results in a runtime_error exception being thrown. -- **pseudo-immutable**: Once constructed the only way to change the value of an interval is to overwrite it with a new one; an existing object cannot be modified. - -## Conventions - -- All operations on interval objects are defined as static class methods on the interval class. - This is a functional-style of programming that basically turns the class into a namespace that grants functions access to private member variables of the object they operate on. - -## Assumptions - -- The interval is only intended for floating point types. - This is enforced via static assertion. -- The constructor for non-empty intervals takes two arguments 'min' and 'max', and they must be ordered (i.e., min <= max). - If this assumption is violated, an exception is emitted and construction fails. - -## Example Usage - -```c++ -#include "autoware_auto_geometry/interval.hpp" - -#include - -// using-directive is just for illustration; don't do this in practice -using namespace autoware::common::geometry; - -// bounds for example interval -constexpr auto MIN = 0.0; -constexpr auto MAX = 1.0; - -// -// Try to construct an invalid interval. This will give the following error: -// 'Attempted to construct an invalid interval: {"min": 1.0, "max": 0.0}' -// - -try { - const auto i = Interval_d(MAX, MIN); -} catch (const std::runtime_error& e) { - std::cerr << e.what(); -} - -// -// Construct a double precision interval from 0 to 1 -// - -const auto i = Interval_d(MIN, MAX); - -// -// Test accessors and properties -// - -std::cout << Interval_d::min(i) << " " << Interval_d::max(i) << "\n"; -// Prints: 0.0 1.0 - -std::cout << Interval_d::empty(i) << " " << Interval_d::length(i) << "\n"; -// Prints: false 1.0 - -std::cout << Interval_d::contains(i, 0.3) << "\n"; -// Prints: true - -std::cout << Interval_d::is_subset_eq(Interval_d(0.2, 0.4), i) << "\n"; -// Prints: true - -// -// Test operations. -// - -std::cout << Interval_d::intersect(i, Interval(-1.0, 0.3)) << "\n"; -// Prints: {"min": 0.0, "max": 0.3} - -std::cout << Interval_d::project_to_interval(i, 0.5) << " " - << Interval_d::project_to_interval(i, -1.3) << "\n"; -// Prints: 0.5 0.0 - -// -// Distinguish empty/zero measure -// - -const auto i_empty = Interval(); -const auto i_zero_length = Interval(0.0, 0.0); - -std::cout << Interval_d::empty(i_empty) << " " - << Interval_d::empty(i_zero_length) << "\n"; -// Prints: true false - -std::cout << Interval_d::zero_measure(i_empty) << " " - << Interval_d::zero_measure(i_zero_length) << "\n"; -// Prints: false false -``` diff --git a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md deleted file mode 100644 index f651c218bc80d..0000000000000 --- a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md +++ /dev/null @@ -1,51 +0,0 @@ -# 2D Convex Polygon Intersection - -Two convex polygon's intersection can be visualized on the image below as the blue area: - - - -## Purpose / Use cases - -Computing the intersection between two polygons can be useful in many applications of scene -understanding. It can be used to estimate collision detection, shape alignment, shape -association and in any application that deals with the objects around the perceiving agent. - -## Design - -[\(Livermore, Calif, 1977\)](https://doi.org/10.2172/7309916) mention the following -observations about convex polygon intersection: - -- Intersection of two convex polygons is a convex polygon -- A vertex from a polygon that is contained in the other polygon is a vertex of the intersection - shape. (Vertices A, C, D in the shape above) -- An edge from a polygon that is contained in the other polygon is an edge in the intersection - shape. (edge C-D in the shape above) -- Edge intersections between two polygons are vertices in the intersection shape. (Vertices B, - E in the shape above.) - -### Inner-workings / Algorithms - -With the observation mentioned above, the current algorithm operates in the following way: - -- Compute and find the vertices from each polygon that is contained in the other polygon - (Vertices A, C, D) -- Compute and find the intersection points between each polygon (Vertices B, E) -- Compute the convex hull shaped by these vertices by ordering them CCW. - -### Inputs / Outputs / API - -Inputs: - -- Two iterables that contain vertices of the convex polygons ordered in the CCW direction. - -Outputs: - -- A list of vertices of the intersection shape ordered in the CCW direction. - -## Future Work - -- #1230: Applying efficient algorithms. - -## Related issues - -- #983: Integrate vision detections in object tracker diff --git a/common/autoware_auto_geometry/design/spatial-hash-design.md b/common/autoware_auto_geometry/design/spatial-hash-design.md deleted file mode 100644 index 58eecf3ee841b..0000000000000 --- a/common/autoware_auto_geometry/design/spatial-hash-design.md +++ /dev/null @@ -1,99 +0,0 @@ -# Spatial Hash - -The spatial hash is a data structure designed for efficient fixed-radius near-neighbor queries in -low dimensions. - -The fixed-radius near-neighbors problem is defined as follows: - -`For point p, find all points p' s.t. d(p, p') < r` - -Where in this case `d(p, p')` is euclidean distance, and `r` is the fixed -radius. - -For `n` points with an average of `k` neighbors each, this data structure can -perform `m` near-neighbor queries (to generate lists of near-neighbors for `m` -different points) in `O(mk)` time. - -By contrast, using a k-d tree for successive nearest-neighbor queries results in -a running time of `O(m log n)`. - -The spatial hash works as follows: - -- Each point is assigned to a bin in the predefined bounding area defined by - `x_min/x_max` and `y_min/y_max` -- This can be done by converting x and y position into x and y index - respectively - - For example with the bin containing `x_min` and `y_min` as index `(0, 0)` - - The two (or more) indices can then be converted into a single index -- Once every point of interest has been inserted into the hash, near-neighbor - queries can begin: - - The bin of the reference point is first computed - - For each point in each adjacent bin, perform an explicit distance computation - between said point and the reference point. If the distance is below the given - radius, said point is considered to be a near-neighbor - -Under the hood, an `std::unordered_multimap` is used, where the key is a bin/voxel index. -The bin size was computed to be the same as the lookup distance. - - - -In addition, this data structure can support 2D or 3D queries. This is determined during -configuration, and baked into the data structure via the configuration class. The purpose of -this was to avoid if statements in tight loops. The configuration class specializations themselves -use CRTP (Curiously Recurring Template Patterns) to do "static polymorphism", and avoid -a dispatching call. - -## Performance characterization - -### Time - -Insertion is `O(n)` because lookup time for the underlying hashmap is `O(n)` for -hashmaps. In practice, lookup time for hashmaps and thus insertion time should -be `O(1)`. - -Removing a point is `O(1)` because the current API only supports removal via -direct reference to a node. - -Finding `k` near-neighbors is worst case `O(n)` in the case of an adversarial -example, but in practice `O(k)`. - -### Space - -The module consists of the following components: - -- The internal hashmap is `O(n + n + A * n)`, where `A` is an arbitrary - constant (load factor) -- The other components of the spatial hash are `O(n + n)` - -This results in `O(n)` space complexity. - -## States - -The spatial hash's state is dictated by the status of the underlying unordered_multimap. - -The data structure is wholly configured by a -[config](@ref autoware::common::geometry::spatial_hash::Config) class. The constructor -of the class determines in the data structure accepts strictly 2D or strictly 3D queries. - -## Inputs - -The primary method of introducing data into the data structure is via the -[insert](@ref autoware::common::geometry::spatial_hash::SpatialHashBase::insert) method. - -## Outputs - -The primary method of retrieving data from the data structure is via the -[near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near)\(2D -configuration\) -or [near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near) -\(3D configuration\) method. - -The whole data structure can also be traversed using standard constant iterators. - -## Future Work - -- Performance tuning and optimization - -## Related issues - -- #28: Port to autoware.Auto diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp deleted file mode 100644 index 0f3a68e14d442..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp +++ /dev/null @@ -1,188 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief Common functionality for bounding box computation algorithms - -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include -#include -#include -#include - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief Functions and types for generating enclosing bounding boxes around a set of points -namespace bounding_box -{ -using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; - -/// \brief Computes height of bounding box given a full list of points -/// \param[in] begin The start of the list of points -/// \param[in] end One past the end of the list of points -/// \param[out] box A box for which the z component of centroid, corners, and size gets filled -/// \tparam IT An iterator type, must dereference into a point type with float member z, or -/// appropriate point adapter defined -template -void compute_height(const IT begin, const IT end, BoundingBox & box) -{ - float32_t max_z = -std::numeric_limits::max(); - float32_t min_z = std::numeric_limits::max(); - for (auto it = begin; it != end; ++it) { - const float32_t z = point_adapter::z_(*it); - if (z <= min_z) { - min_z = z; - } - if (z >= max_z) { - max_z = z; - } - } - box.centroid.z = (max_z + min_z) * 0.5F; - for (auto & corner : box.corners) { - corner.z = box.centroid.z; - } - box.size.z = (max_z - min_z) * 0.5F; -} - -/// \brief Computes height of bounding box given a full list of points -/// \param[in] begin Iterator pointing to the start of the range of points -/// \param[in] end Iterator pointing the the end of the range of points -/// \param[out] shape A shape in which vertices z values and height field will be set -/// \tparam IT An iterator type, must dereference into a point type with float member z, or -/// appropriate point adapter defined -template -void compute_height(const IT begin, const IT end, autoware_auto_perception_msgs::msg::Shape & shape) -{ - float32_t max_z = -std::numeric_limits::max(); - float32_t min_z = std::numeric_limits::max(); - for (auto it = begin; it != end; ++it) { - const float32_t z = point_adapter::z_(*it); - if (z <= min_z) { - min_z = z; - } - if (z >= max_z) { - max_z = z; - } - } - for (auto & corner : shape.footprint.points) { - corner.z = min_z; - } - shape.dimensions.z = max_z - min_z; -} - -namespace details -{ -/// Templated alias for getting a type without CV or reference qualification -template -using base_type = typename std::remove_cv::type>::type; - -/// Templated alias for an array of 4 objects of type PointT -template -using Point4 = std::array; - -/// \brief Helper struct for compile time introspection of array size from -/// Ref: https://stackoverflow.com/questions/16866033 -template -struct array_size; -template -struct array_size> -{ - static std::size_t const size = N; -}; -static_assert( - array_size>::size == 4U, - "Bounding box does not have the right number of corners"); - -/// \brief Compute length, width, area of bounding box -/// \param[in] corners Corners of the current bounding box -/// \param[out] ret A point struct used to hold size of box defined by corners -void GEOMETRY_PUBLIC -size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret); -/// \brief Computes centroid and orientation of a box given corners -/// \param[in] corners Array of final corners of bounding box -/// \param[out] box Message to have corners, orientation, and centroid updated -void GEOMETRY_PUBLIC -finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box); - -/// \brief given support points and two orthogonal directions, compute corners of bounding box -/// \tparam PointT Type of a point, must have float members x and y` -/// \tparam IT An iterator dereferenceable into PointT -/// \param[out] corners Gets filled with corner points of bounding box -/// \param[in] supports Iterators referring to support points of current bounding box -/// e.g. bounding box is touching these points -/// \param[in] directions Directions of each edge of the bounding box from each support point -template -void compute_corners( - decltype(BoundingBox::corners) & corners, const Point4 & supports, - const Point4 & directions) -{ - for (uint32_t idx = 0U; idx < corners.size(); ++idx) { - const uint32_t jdx = (idx != 3U) ? (idx + 1U) : 0U; - const auto pt = - intersection_2d(*supports[idx], directions[idx], *supports[jdx], directions[jdx]); - // TODO(c.ho) handle error - point_adapter::xr_(corners[idx]) = point_adapter::x_(pt); - point_adapter::yr_(corners[idx]) = point_adapter::y_(pt); - } -} -// TODO(c.ho) type trait enum base - -/// \brief Copy vertices of the given box into a Shape type -/// \param box Box to be converted -/// \return Shape type filled with box vertices -autoware_auto_perception_msgs::msg::Shape GEOMETRY_PUBLIC make_shape(const BoundingBox & box); - -/// \brief Copy centroid and orientation info of the box into Pose type -/// \param box BoundingBox to be converted -/// \return Pose type filled with centroid and orientation from box -geometry_msgs::msg::Pose GEOMETRY_PUBLIC make_pose(const BoundingBox & box); - -/// \brief Fill DetectedObject type with contents from a BoundingBox type -/// \param box BoundingBox to be converted -/// \return Filled DetectedObject type -autoware_auto_perception_msgs::msg::DetectedObject GEOMETRY_PUBLIC -make_detected_object(const BoundingBox & box); - -/// \brief Transform corners from object-local coordinates using the given centroid and orientation -/// \param shape_msg Msg containing corners in object-local coordinates -/// \param centroid Centroid of the polygon whose corners are defined in shape_msg -/// \param orientation Orientation of the polygon -/// \return corners transformed such that their centroid and orientation correspond to the -/// given inputs -std::vector GEOMETRY_PUBLIC get_transformed_corners( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation); - -} // namespace details -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp deleted file mode 100644 index e0f2e66e87ee5..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp +++ /dev/null @@ -1,247 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements 2D pca on a linked list of points to estimate an oriented -/// bounding box - -// cspell: ignore eigenbox, EIGENBOX -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ - -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" - -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ - -/// \brief Simplified 2d covariance matrix -struct Covariance2d -{ - /// \brief Variance in the x direction - float32_t xx; - /// \brief Variance in the y direction - float32_t yy; - /// \brief x-y covariance - float32_t xy; - /// \brief Number of points - std::size_t num_points; -}; // struct Covariance2d - -// cspell: ignore Welford -/// \brief Compute 2d covariance matrix of a list of points using Welford's online algorithm -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \return A 2d covariance matrix for all points in the list -template -Covariance2d covariance_2d(const IT begin, const IT end) -{ - Covariance2d ret{0.0F, 0.0F, 0.0F, 0U}; - float32_t ux = 0.0F; - float32_t uy = 0.0F; - float32_t num_points = 0.0F; - using point_adapter::x_; - using point_adapter::y_; - for (auto it = begin; it != end; ++it) { - ++ret.num_points; - num_points = static_cast(ret.num_points); - const auto & pt = *it; - // update mean x - const float32_t dx = x_(pt) - ux; - ux = ux + (dx / num_points); - // update cov - const float32_t dy = y_(pt) - uy; - ret.xy += (x_(pt) - ux) * (dy); - // update mean y - uy = uy + (dy / num_points); - // update M2 - ret.xx += dx * (x_(pt) - ux); - ret.yy += dy * (y_(pt) - uy); - } - // finalize sample (co-)variance - if (ret.num_points > 1U) { - num_points = num_points - 1.0F; - } - ret.xx /= num_points; - ret.yy /= num_points; - ret.xy /= num_points; - - return ret; -} - -/// \brief Compute eigenvectors and eigenvalues -/// \param[in] cov 2d Covariance matrix -/// \param[out] eig_vec1 First eigenvector -/// \param[out] eig_vec2 Second eigenvector -/// \tparam PointT Point type that has at least float members x and y -/// \return A pair of eigenvalues: The first is the larger eigenvalue -/// \throw std::runtime error if you would get degenerate covariance -template -std::pair eig_2d( - const Covariance2d & cov, PointT & eig_vec1, PointT & eig_vec2) -{ - const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; - const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); - // Add a small fudge to alleviate floating point errors - float32_t disc = ((tr_2 * tr_2) - det) + std::numeric_limits::epsilon(); - if (disc < 0.0F) { - throw std::runtime_error( - "pca_2d: negative discriminant! Should never happen for well formed " - "covariance matrix"); - } - disc = sqrtf(disc); - // compute eigenvalues - const auto ret = std::make_pair(tr_2 + disc, tr_2 - disc); - // compute eigenvectors - using point_adapter::xr_; - using point_adapter::yr_; - // We compare squared value against floating epsilon to make sure that eigen vectors - // are persistent against further calculations. - // (e.g. taking cross product of two eigen vectors) - if (fabsf(cov.xy * cov.xy) > std::numeric_limits::epsilon()) { - xr_(eig_vec1) = cov.xy; - yr_(eig_vec1) = ret.first - cov.xx; - xr_(eig_vec2) = cov.xy; - yr_(eig_vec2) = ret.second - cov.xx; - } else { - if (cov.xx > cov.yy) { - xr_(eig_vec1) = 1.0F; - yr_(eig_vec1) = 0.0F; - xr_(eig_vec2) = 0.0F; - yr_(eig_vec2) = 1.0F; - } else { - xr_(eig_vec1) = 0.0F; - yr_(eig_vec1) = 1.0F; - xr_(eig_vec2) = 1.0F; - yr_(eig_vec2) = 0.0F; - } - } - return ret; -} - -/// \brief Given eigenvectors, compute support (furthest) point in each direction -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \tparam PointT type of a point with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] eig1 First principal component of cluster -/// \param[in] eig2 Second principal component of cluster -/// \param[out] support Array to get filled with extreme points in each of the principal -/// components -/// \return whether or not eig2 is ccw wrt eig1 -template -bool8_t compute_supports( - const IT begin, const IT end, const PointT & eig1, const PointT & eig2, Point4 & support) -{ - const bool8_t ret = cross_2d(eig1, eig2) >= 0.0F; - std::array metrics{ - {-std::numeric_limits::max(), -std::numeric_limits::max(), - std::numeric_limits::max(), std::numeric_limits::max()}}; - for (auto it = begin; it != end; ++it) { - const PointT & pt = *it; - float32_t val = dot_2d(ret ? eig1 : eig2, pt); - if (val > metrics[0U]) { - metrics[0U] = val; - support[0U] = it; - } - if (val < metrics[2U]) { - metrics[2U] = val; - support[2U] = it; - } - val = dot_2d(ret ? eig2 : eig1, pt); - if (val > metrics[1U]) { - metrics[1U] = val; - support[1U] = it; - } - if (val < metrics[3U]) { - metrics[3U] = val; - support[3U] = it; - } - } - return ret; -} - -/// \brief Compute bounding box given a pair of basis directions -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \tparam PointT Point type of the lists, must have float members x and y -/// \param[in] ax1 First basis direction, assumed to be normal to ax2 -/// \param[in] ax2 Second basis direction, assumed to be normal to ax1, assumed to be ccw wrt ax1 -/// \param[in] supports Array of iterators referring to points that are most extreme in each basis -/// direction. Should be result of compute_supports function -/// \return A bounding box based on the basis axes and the support points -template -BoundingBox compute_bounding_box( - const PointT & ax1, const PointT & ax2, const Point4 & supports) -{ - decltype(BoundingBox::corners) corners; - const Point4 directions{{ax1, ax2, minus_2d(ax1), minus_2d(ax2)}}; - compute_corners(corners, supports, directions); - - // build box - BoundingBox bbox; - finalize_box(corners, bbox); - size_2d(corners, bbox.size); - return bbox; -} -} // namespace details - -/// \brief Compute oriented bounding box using PCA. This uses all points in a list, and does not -/// modify the list. The resulting bounding box is not necessarily minimum in any way -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \return An oriented bounding box in x-y. This bounding box has no height information -template -BoundingBox eigenbox_2d(const IT begin, const IT end) -{ - // compute covariance - const details::Covariance2d cov = details::covariance_2d(begin, end); - - // compute eigenvectors - using PointT = details::base_type; - PointT eig1; - PointT eig2; - const auto eig_v = details::eig_2d(cov, eig1, eig2); - - // find extreme points - details::Point4 supports; - const bool8_t is_ccw = details::compute_supports(begin, end, eig1, eig2, supports); - // build box - if (is_ccw) { - std::swap(eig1, eig2); - } - BoundingBox bbox = details::compute_bounding_box(eig1, eig2, supports); - bbox.value = eig_v.first; - - return bbox; -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp deleted file mode 100644 index 07fd6c989cedc..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp +++ /dev/null @@ -1,281 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements 2D pca on a linked list of points to estimate an oriented -/// bounding box - -// cspell: ignore LFIT, lfit -// LFIT means "L-Shape Fitting" -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ - -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" - -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ - -/// \brief A representation of the M matrix for the L-fit algorithm -struct LFitWs -{ - /// \brief Number of points in the first partition - std::size_t p; - /// \brief Number of points in the second partition - std::size_t q; - // assume matrix of form: [a b; c d] - /// \brief Sum of x values in first partition - float32_t m12a; - /// \brief Sum of y values in first partition - float32_t m12b; - /// \brief Sum of y values in second partition - float32_t m12c; - /// \brief Negative sum of x values in second partition - float32_t m12d; - // m22 is a symmetric matrix - /// \brief Sum_p x_2 + sum_q y_2 - float32_t m22a; - /// \brief Sum_p x*y - sum_q x*y - float32_t m22b; - /// \brief Sum_p y_2 + sum_x y_2 - float32_t m22d; -}; // struct LFitWs - -/// \brief Initialize M matrix for L-fit algorithm -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \param[out] ws A representation of the M matrix to get initialized -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -template -void init_lfit_ws(const IT begin, const IT end, const std::size_t size, LFitWs & ws) -{ - ws.p = 1UL; - ws.q = size - 1UL; - // init P terms (first partition) - using point_adapter::x_; - using point_adapter::y_; - const auto & pt = *begin; - const float32_t px = x_(pt); - const float32_t py = y_(pt); - // assume matrix of form: [a b; c d] - ws.m12a = px; - ws.m12b = py; - ws.m12c = 0.0F; - ws.m12d = 0.0F; - // m22 is a symmetric matrix - ws.m22a = px * px; - ws.m22b = px * py; - ws.m22d = py * py; - auto it = begin; - ++it; - for (; it != end; ++it) { - const auto & qt = *it; - const float32_t qx = x_(qt); - const float32_t qy = y_(qt); - ws.m12c += qy; - ws.m12d -= qx; - ws.m22a += qy * qy; - ws.m22b -= qx * qy; - ws.m22d += qx * qx; - } -} - -/// \brief Solves the L fit problem for a given M matrix -/// \tparam PointT The point type of the cluster being L-fitted -/// \param[in] ws A representation of the M Matrix -/// \param[out] dir The best fit direction for this partition/M matrix -/// \return The L2 residual for this fit (the score, lower is better) -template -float32_t solve_lfit(const LFitWs & ws, PointT & dir) -{ - const float32_t pi = 1.0F / static_cast(ws.p); - const float32_t qi = 1.0F / static_cast(ws.q); - const Covariance2d M{// matrix of form [x, z; z y] - ws.m22a - (((ws.m12a * ws.m12a) * pi) + ((ws.m12c * ws.m12c) * qi)), - ws.m22d - (((ws.m12b * ws.m12b) * pi) + ((ws.m12d * ws.m12d) * qi)), - ws.m22b - (((ws.m12a * ws.m12b) * pi) + ((ws.m12c * ws.m12d) * qi)), 0UL}; - PointT eig1; - const auto eig_v = eig_2d(M, eig1, dir); - return eig_v.second; -} - -/// \brief Increments L fit M matrix with information in the point -/// \tparam PointT The point type -/// \param[in] pt The point to increment the M matrix with -/// \param[inout] ws A representation of the M matrix -template -void increment_lfit_ws(const PointT & pt, LFitWs & ws) -{ - const float32_t px = point_adapter::x_(pt); - const float32_t py = point_adapter::y_(pt); - ws.m12a += px; - ws.m12b += py; - ws.m12c -= py; - ws.m12d += px; - ws.m22b += 2.0F * px * py; - const float32_t px2y2 = (px - py) * (px + py); - ws.m22a += px2y2; - ws.m22d -= px2y2; -} - -/// \tparam IT An iterator type that should dereference into a point type with float members x and y -template -class LFitCompare -{ -public: - /// \brief Constructor, initializes normal direction - /// \param[in] hint A 2d vector with the direction that will be used to order the - /// point list - explicit LFitCompare(const PointT & hint) - : m_nx(point_adapter::x_(hint)), m_ny(point_adapter::y_(hint)) - { - } - - /// \brief Comparator operation, returns true if the projection of a the tangent line - /// is smaller than the projection of b - /// \param[in] p The first point for comparison - /// \param[in] q The second point for comparison - /// \return True if a has a smaller projection than b on the tangent line - bool8_t operator()(const PointT & p, const PointT & q) const - { - using point_adapter::x_; - using point_adapter::y_; - return ((x_(p) * m_nx) + (y_(p) * m_ny)) < ((x_(q) * m_nx) + (y_(q) * m_ny)); - } - -private: - const float32_t m_nx; - const float32_t m_ny; -}; // class LFitCompare - -/// \brief The main implementation of L-fitting a bounding box to a list of points. -/// Assumes sufficiently valid, large enough, and appropriately ordered point list -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \return A bounding box that minimizes the LFit residual -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -template -BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::size_t size) -{ - // initialize M - LFitWs ws{}; - init_lfit_ws(begin, end, size, ws); - // solve initial problem - details::base_type best_normal; - float32_t min_eig = solve_lfit(ws, best_normal); - // solve subsequent problems - auto it = begin; - ++it; - for (; it != end; ++it) { - // update M - ws.p += 1U; - ws.q -= 1U; - if (ws.q == 0U) { // checks for q = 0 case - break; - } - increment_lfit_ws(*it, ws); - // solve incremented problem - decltype(best_normal) dir; - const float32_t score = solve_lfit(ws, dir); - // update optima - if (score < min_eig) { - min_eig = score; - best_normal = dir; - } - } - // can recover best corner point, but don't care, need to cover all points - const auto inv_norm = 1.0F / norm_2d(best_normal); - if (!std::isnormal(inv_norm)) { - throw std::runtime_error{"LFit: Abnormal norm"}; - } - best_normal = times_2d(best_normal, inv_norm); - auto best_tangent = get_normal(best_normal); - // find extreme points - Point4 supports; - const bool8_t is_ccw = details::compute_supports(begin, end, best_normal, best_tangent, supports); - if (is_ccw) { - std::swap(best_normal, best_tangent); - } - BoundingBox bbox = details::compute_bounding_box(best_normal, best_tangent, supports); - bbox.value = min_eig; - - return bbox; -} -} // namespace details - -/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed -/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation" -/// \return An oriented bounding box in x-y. This bounding box has no height information -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \param[in] hint An iterator pointing to the point whose normal will be used to sort the list -/// \return A pair where the first element is an iterator pointing to the nearest point, and the -/// second element is the size of the list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \throw std::domain_error If the number of points is too few -template -BoundingBox lfit_bounding_box_2d( - const IT begin, const IT end, const PointT hint, const std::size_t size) -{ - if (size <= 1U) { - throw std::domain_error("LFit requires >= 2 points!"); - } - // NOTE: assumes points are in base_link/sensor frame! - // sort along tangent wrt sensor origin - // lint -e522 NOLINT Not a pure function: data structure iterators are pointing to is modified - std::partial_sort(begin, end, end, details::LFitCompare{hint}); - - return details::lfit_bounding_box_2d_impl(begin, end, size); -} - -/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed -/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation". -/// This implementation sorts the list using std::sort -/// \return An oriented bounding box in x-y. This bounding box has no height information -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -template -BoundingBox lfit_bounding_box_2d(const IT begin, const IT end) -{ - // use the principal component as the sorting direction - const auto cov = details::covariance_2d(begin, end); - using PointT = details::base_type; - PointT eig1; - PointT eig2; - (void)details::eig_2d(cov, eig1, eig2); - (void)eig2; - return lfit_bounding_box_2d(begin, end, eig1, cov.num_points); -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp deleted file mode 100644 index fb75384eb07cb..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp +++ /dev/null @@ -1,280 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements the rotating calipers algorithm for minimum oriented bounding boxes - -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/convex_hull.hpp" - -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ -/// \brief Find which (next) edge has smallest angle delta to directions, rotate directions -/// \param[inout] edges Array of edges on polygon after each anchor point (in ccw order). -/// E.g. if anchor point = p_i, edge = P[i+1] - P[i] -/// \param[inout] directions Array of directions of current bounding box (in ccw order) -/// \tparam PointT Point type of the lists, must have float members x and y -/// \return index of array to advance, e.g. the one with the smallest angle between edge and dir -template -uint32_t update_angles(const Point4 & edges, Point4 & directions) -{ - // find smallest angle to next - float32_t best_cos_th = -std::numeric_limits::max(); - float32_t best_edge_dir_mag = 0.0F; - uint32_t advance_idx = 0U; - for (uint32_t idx = 0U; idx < edges.size(); ++idx) { - const float32_t edge_dir_mag = std::max( - norm_2d(edges[idx]) * norm_2d(directions[idx]), std::numeric_limits::epsilon()); - const float32_t cos_th = dot_2d(edges[idx], directions[idx]) / edge_dir_mag; - if (cos_th > best_cos_th) { - best_cos_th = cos_th; - best_edge_dir_mag = edge_dir_mag; - advance_idx = idx; - } - } - - // update directions by smallest angle - const float32_t sin_th = - cross_2d(directions[advance_idx], edges[advance_idx]) / best_edge_dir_mag; - for (uint32_t idx = 0U; idx < edges.size(); ++idx) { - rotate_2d(directions[idx], best_cos_th, sin_th); - } - - return advance_idx; -} - -/// \brief Given support points i, find direction of edge: e = P[i+1] - P[i] -/// \tparam PointT Point type of the lists, must have float members x and y -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] support Array of points that are most extreme in 4 directions for current OBB -/// \param[out] edges An array to be filled with the polygon edges next from support points -template -void init_edges(const IT begin, const IT end, const Point4 & support, Point4 & edges) -{ - for (uint32_t idx = 0U; idx < support.size(); ++idx) { - auto tmp_it = support[idx]; - ++tmp_it; - const PointT & q = (tmp_it == end) ? *begin : *tmp_it; - edges[idx] = minus_2d(q, *support[idx]); - } -} - -/// \brief Scan through list to find support points for bounding box oriented in direction of -/// u = P[1] - P[0] -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[out] support Array that gets filled with pointers to points that are most extreme in -/// each direction aligned with and orthogonal to the first polygon edge. -/// Most extreme = max/min wrt u = P[1]-P[0] (in the dot/cross product sense) -template -void init_bbox(const IT begin, const IT end, Point4 & support) -{ - // compute initial orientation using first two points - auto pt_it = begin; - ++pt_it; - const auto u = minus_2d(*pt_it, *begin); - support[0U] = begin; - std::array metric{ - {-std::numeric_limits::max(), -std::numeric_limits::max(), - std::numeric_limits::max()}}; - // track u * p, fabsf(u x p), and -u * p - for (pt_it = begin; pt_it != end; ++pt_it) { - // check points against orientation for run_ptr - const auto & pt = *pt_it; - // u * p - const float32_t up = dot_2d(u, pt); - if (up > metric[0U]) { - metric[0U] = up; - support[1U] = pt_it; - } - // -u * p - if (up < metric[2U]) { - metric[2U] = up; - support[3U] = pt_it; - } - // u x p - const float32_t uxp = cross_2d(u, pt); - if (uxp > metric[1U]) { - metric[1U] = uxp; - support[2U] = pt_it; - } - } -} -/// \brief Compute the minimum bounding box for a convex hull using the rotating calipers method. -/// This function may possibly also be used for computing the width of a convex hull, as it uses the -/// external supports of a single convex hull. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \param[in] metric_fn A functor determining what measure the bounding box is minimum with respect -/// to -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -/// \tparam MetricF A functor that computes a float measure from the x and y size (width and length) -/// of a bounding box, assumed to be packed in a Point32 message. -/// \throw std::domain_error if the list of points is too small to reasonable generate a bounding -/// box -template -BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF metric_fn) -{ - using PointT = base_type; - // sanity check TODO(c.ho) more checks (up to n = 2?) - if (begin == end) { - throw std::domain_error("Malformed list, size = " + std::to_string(std::distance(begin, end))); - } - // initial scan to get anchor points - Point4 support; - init_bbox(begin, end, support); - // initialize directions accordingly - Point4 directions; - { // I just don't want the temporary variable floating around - auto tmp = support[0U]; - ++tmp; - directions[0U] = minus_2d(*tmp, *support[0U]); - directions[1U] = get_normal(directions[0U]); - directions[2U] = minus_2d(directions[0U]); - directions[3U] = minus_2d(directions[1U]); - } - // initialize edges - Point4 edges; - init_edges(begin, end, support, edges); - // size of initial guess - BoundingBox bbox; - decltype(BoundingBox::corners) best_corners; - compute_corners(best_corners, support, directions); - size_2d(best_corners, bbox.size); - bbox.value = metric_fn(bbox.size); - // rotating calipers step: incrementally advance, update angles, points, compute area - for (auto it = begin; it != end; ++it) { - // find smallest angle to next, update directions - const uint32_t advance_idx = update_angles(edges, directions); - // recompute area - decltype(BoundingBox::corners) corners; - compute_corners(corners, support, directions); - decltype(BoundingBox::size) tmp_size; - size_2d(corners, tmp_size); - const float32_t tmp_value = metric_fn(tmp_size); - // update best if necessary - if (tmp_value < bbox.value) { - // corners: memcpy is fine since I know corners and best_corners are distinct - (void)std::memcpy(&best_corners[0U], &corners[0U], sizeof(corners)); - // size - bbox.size = tmp_size; - bbox.value = tmp_value; - } - // Step to next iteration of calipers - { - // update smallest support - auto next_it = support[advance_idx]; - ++next_it; - const auto run_it = (end == next_it) ? begin : next_it; - support[advance_idx] = run_it; - // update edges - next_it = run_it; - ++next_it; - const PointT & next = (end == next_it) ? (*begin) : (*next_it); - edges[advance_idx] = minus_2d(next, *run_it); - } - } - - finalize_box(best_corners, bbox); - - // TODO(christopher.ho) check if one of the sizes is too small, fuzz corner 1 and 2 - // This doesn't cause any issues now, it shouldn't happen in practice, and even if it did, - // it would probably get smoothed out by prediction. But, this could cause issues with the - // collision detection algorithms (i.e GJK), but probably not. - - return bbox; -} -} // namespace details - -/// \brief Compute the minimum area bounding box given a convex hull of points. -/// This function is exposed in case a user might already have a convex hull via other methods. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \return A minimum area bounding box, value field is the area -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -template -BoundingBox minimum_area_bounding_box(const IT begin, const IT end) -{ - const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t { - return pt.x * pt.y; - }; - return details::rotating_calipers_impl(begin, end, metric_fn); -} - -/// \brief Compute the minimum perimeter bounding box given a convex hull of points -/// This function is exposed in case a user might already have a convex hull via other methods. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \return A minimum perimeter bounding box, value field is half the perimeter -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -template -BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end) -{ - const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t { - return pt.x + pt.y; - }; - return details::rotating_calipers_impl(begin, end, metric_fn); -} - -/// \brief Compute the minimum area bounding box given an unstructured list of points. -/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and -/// without memory allocation. -/// \param[inout] list A list of points to form a hull around, gets reordered -/// \return A minimum area bounding box, value field is the area -/// \tparam PointT Point type of the lists, must have float members x and y -template -BoundingBox minimum_area_bounding_box(std::list & list) -{ - const auto last = convex_hull(list); - return minimum_area_bounding_box(list.cbegin(), last); -} - -/// \brief Compute the minimum perimeter bounding box given an unstructured list of points -/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and -/// without memory allocation. -/// \param[inout] list A list of points to form a hull around, gets reordered -/// \return A minimum perimeter bounding box, value field is half the perimeter -/// \tparam PointT Point type of the lists, must have float members x and y -template -BoundingBox minimum_perimeter_bounding_box(std::list & list) -{ - const auto last = convex_hull(list); - return minimum_perimeter_bounding_box(list.cbegin(), last); -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp deleted file mode 100644 index c4c52928ac19a..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief Main header for user-facing bounding box algorithms: functions and types -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ - -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -} // namespace geometry -} // namespace common -} // namespace autoware -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp deleted file mode 100644 index e3c2e58c9f80e..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp +++ /dev/null @@ -1,587 +0,0 @@ -// Copyright 2017-2021 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file includes common functionality for 2D geometry, such as dot products - -#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; - -namespace comparison = autoware::common::helper_functions::comparisons; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \brief Temporary namespace for point adapter methods, for use with nonstandard point types -namespace point_adapter -{ -/// \brief Gets the x value for a point -/// \return The x value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto x_(const PointT & pt) -{ - return pt.x; -} -/// \brief Gets the x value for a TrajectoryPoint message -/// \return The x value of the point -/// \param[in] pt The point -inline auto x_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.x; -} -/// \brief Gets the y value for a point -/// \return The y value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto y_(const PointT & pt) -{ - return pt.y; -} -/// \brief Gets the y value for a TrajectoryPoint message -/// \return The y value of the point -/// \param[in] pt The point -inline auto y_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.y; -} -/// \brief Gets the z value for a point -/// \return The z value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto z_(const PointT & pt) -{ - return pt.z; -} -/// \brief Gets the z value for a TrajectoryPoint message -/// \return The z value of the point -/// \param[in] pt The point -inline auto z_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.z; -} -/// \brief Gets a reference to the x value for a point -/// \return A reference to the x value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & xr_(PointT & pt) -{ - return pt.x; -} -/// \brief Gets a reference to the x value for a TrajectoryPoint -/// \return A reference to the x value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & xr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.x; -} -/// \brief Gets a reference to the y value for a point -/// \return A reference to The y value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & yr_(PointT & pt) -{ - return pt.y; -} -/// \brief Gets a reference to the y value for a TrajectoryPoint -/// \return A reference to the y value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & yr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.y; -} -/// \brief Gets a reference to the z value for a point -/// \return A reference to the z value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & zr_(PointT & pt) -{ - return pt.z; -} -/// \brief Gets a reference to the z value for a TrajectoryPoint -/// \return A reference to the z value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & zr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.z; -} -} // namespace point_adapter - -namespace details -{ -// Return the next iterator, cycling back to the beginning of the list if you hit the end -template -IT circular_next(const IT begin, const IT end, const IT current) noexcept -{ - auto next = std::next(current); - if (end == next) { - next = begin; - } - return next; -} - -} // namespace details - -/// \tparam T1, T2, T3 point type. Must have point adapters defined or have float members x and y -/// \brief compute whether line segment rp is counter clockwise relative to line segment qp -/// \param[in] pt shared point for both line segments -/// \param[in] r point to check if it forms a ccw angle -/// \param[in] q reference point -/// \return whether angle formed is ccw. Three collinear points is considered ccw -template -inline auto ccw(const T1 & pt, const T2 & q, const T3 & r) -{ - using point_adapter::x_; - using point_adapter::y_; - return (((x_(q) - x_(pt)) * (y_(r) - y_(pt))) - ((y_(q) - y_(pt)) * (x_(r) - x_(pt)))) <= 0.0F; -} - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y -/// \brief compute p x q = p1 * q2 - p2 * q1 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 2d cross product -template -inline auto cross_2d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - return (x_(pt) * y_(q)) - (y_(pt) * x_(q)); -} - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y -/// \brief compute p * q = p1 * q1 + p2 * q2 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 2d scalar dot product -template -inline auto dot_2d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - return (x_(pt) * x_(q)) + (y_(pt) * y_(q)); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the 2d difference between two points, p - q -/// \param[in] p The left hand side -/// \param[in] q The right hand side -/// \return A point with the difference in the x and y fields, all other fields are default -/// initialized -template -T minus_2d(const T & p, const T & q) -{ - T r; - using point_adapter::x_; - using point_adapter::y_; - point_adapter::xr_(r) = x_(p) - x_(q); - point_adapter::yr_(r) = y_(p) - y_(q); - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The unary minus or negation operator applied to a single point's 2d fields -/// \param[in] p The left hand side -/// \return A point with the negation in the x and y fields, all other fields are default -/// initialized -template -T minus_2d(const T & p) -{ - T r; - point_adapter::xr_(r) = -point_adapter::x_(p); - point_adapter::yr_(r) = -point_adapter::y_(p); - return r; -} -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The 2d addition operation, p + q -/// \param[in] p The left hand side -/// \param[in] q The right hand side -/// \return A point with the sum in the x and y fields, all other fields are default -/// initialized -template -T plus_2d(const T & p, const T & q) -{ - T r; - using point_adapter::x_; - using point_adapter::y_; - point_adapter::xr_(r) = x_(p) + x_(q); - point_adapter::yr_(r) = y_(p) + y_(q); - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The scalar multiplication operation, p * a -/// \param[in] p The point value -/// \param[in] a The scalar value -/// \return A point with the scaled x and y fields, all other fields are default -/// initialized -template -T times_2d(const T & p, const float32_t a) -{ - T r; - point_adapter::xr_(r) = static_cast(point_adapter::x_(p)) * a; - point_adapter::yr_(r) = static_cast(point_adapter::y_(p)) * a; - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief solve p + t * u = q + s * v -/// Ref: https://stackoverflow.com/questions/563198/ -/// \param[in] pt anchor point of first line -/// \param[in] u direction of first line -/// \param[in] q anchor point of second line -/// \param[in] v direction of second line -/// \return intersection point -/// \throw std::runtime_error if lines are (nearly) collinear or parallel -template -inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v) -{ - const float32_t num = cross_2d(minus_2d(pt, q), u); - float32_t den = cross_2d(v, u); - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - if (fabsf(den) < FEPS) { - if (fabsf(num) < FEPS) { - // collinear case, anything is ok - den = 1.0F; - } else { - // parallel case, no valid output - throw std::runtime_error( - "intersection_2d: no unique solution (either collinear or parallel)"); - } - } - return plus_2d(q, times_2d(v, num / den)); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief rotate point given precomputed sin and cos -/// \param[inout] pt point to rotate -/// \param[in] cos_th precomputed cosine value -/// \param[in] sin_th precomputed sine value -template -inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th) -{ - const float32_t x = point_adapter::x_(pt); - const float32_t y = point_adapter::y_(pt); - point_adapter::xr_(pt) = (cos_th * x) - (sin_th * y); - point_adapter::yr_(pt) = (sin_th * x) + (cos_th * y); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief rotate by radian angle th in z direction with ccw positive -/// \param[in] pt reference point to rotate -/// \param[in] th_rad angle by which to rotate point -/// \return rotated point -template -inline T rotate_2d(const T & pt, const float32_t th_rad) -{ - T q(pt); // It's reasonable to expect a copy constructor - const float32_t s = sinf(th_rad); - const float32_t c = cosf(th_rad); - rotate_2d(q, c, s); - return q; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief compute q s.t. p T q, or p * q = 0 -/// This is the equivalent of a 90 degree ccw rotation -/// \param[in] pt point to get normal point of -/// \return point normal to p (un-normalized) -template -inline T get_normal(const T & pt) -{ - T q(pt); - point_adapter::xr_(q) = -point_adapter::y_(pt); - point_adapter::yr_(q) = point_adapter::x_(pt); - return q; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief get magnitude of x and y components: -/// \param[in] pt point to get magnitude of -/// \return magnitude of x and y components together -template -inline auto norm_2d(const T & pt) -{ - return sqrtf(static_cast(dot_2d(pt, pt))); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the closest point on line segment p-q to point r -/// Based on equations from https://stackoverflow.com/a/1501725 and -/// http://paulbourke.net/geometry/pointlineplane/ -/// \param[in] p First point defining the line segment -/// \param[in] q Second point defining the line segment -/// \param[in] r Reference point to find the closest point to -/// \return Closest point on line segment p-q to point r -template -inline T closest_segment_point_2d(const T & p, const T & q, const T & r) -{ - const T qp = minus_2d(q, p); - const float32_t len2 = static_cast(dot_2d(qp, qp)); - T ret = p; - if (len2 > std::numeric_limits::epsilon()) { - const Interval_f unit_interval(0.0f, 1.0f); - const float32_t val = static_cast(dot_2d(minus_2d(r, p), qp)) / len2; - const float32_t t = Interval_f::clamp_to(unit_interval, val); - ret = plus_2d(p, times_2d(qp, t)); - } - return ret; -} -// -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the closest point on the line going through p-q to point r -// Obtained by simplifying closest_segment_point_2d. -/// \param[in] p First point defining the line -/// \param[in] q Second point defining the line -/// \param[in] r Reference point to find the closest point to -/// \return Closest point on line p-q to point r -/// \throw std::runtime_error if the two points coincide and hence don't uniquely -// define a line -template -inline T closest_line_point_2d(const T & p, const T & q, const T & r) -{ - const T qp = minus_2d(q, p); - const float32_t len2 = dot_2d(qp, qp); - T ret = p; - if (len2 > std::numeric_limits::epsilon()) { - const float32_t t = dot_2d(minus_2d(r, p), qp) / len2; - ret = plus_2d(p, times_2d(qp, t)); - } else { - throw std::runtime_error( - "closet_line_point_2d: line ill-defined because given points coincide"); - } - return ret; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the distance from line segment p-q to point r -/// \param[in] p First point defining the line segment -/// \param[in] q Second point defining the line segment -/// \param[in] r Reference point to find the distance from the line segment to -/// \return Distance from point r to line segment p-q -template -inline auto point_line_segment_distance_2d(const T & p, const T & q, const T & r) -{ - const T pq_r = minus_2d(closest_segment_point_2d(p, q, r), r); - return norm_2d(pq_r); -} - -/// \brief Make a 2D unit vector given an angle. -/// \tparam T Point type. Must have point adapters defined or have float members x and y -/// \param th Angle in radians -/// \return Unit vector in the direction of the given angle. -template -inline T make_unit_vector2d(float th) -{ - T r; - point_adapter::xr_(r) = std::cos(th); - point_adapter::yr_(r) = std::sin(th); - return r; -} - -/// \brief Compute squared euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x and y -/// \tparam T2 point type. Must have point adapters defined or have float members x and y -/// \param a point 1 -/// \param b point 2 -/// \return squared euclidean distance -template -inline OUT squared_distance_2d(const T1 & a, const T2 & b) -{ - const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); - const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); - return (x * x) + (y * y); -} - -/// \brief Compute euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x and y -/// \tparam T2 point type. Must have point adapters defined or have float members x and y -/// \param a point 1 -/// \param b point 2 -/// \return euclidean distance -template -inline OUT distance_2d(const T1 & a, const T2 & b) -{ - return std::sqrt(squared_distance_2d(a, b)); -} - -/// \brief Check the given point's position relative the infinite line passing -/// from p1 to p2. Logic based on http://geomalgorithms.com/a01-_area.html#isLeft() -/// \tparam T T point type. Must have point adapters defined or have float members x and y -/// \param p1 point 1 laying on the infinite line -/// \param p2 point 2 laying on the infinite line -/// \param q point to be checked against the line -/// \return > 0 for point q left of the line through p1 to p2 -/// = 0 for point q on the line through p1 to p2 -/// < 0 for point q right of the line through p1 to p2 -template -inline auto check_point_position_to_line_2d(const T & p1, const T & p2, const T & q) -{ - return cross_2d(minus_2d(p2, p1), minus_2d(q, p1)); -} - -/// Check if all points are ordered in x-y plane (in either clockwise or counter-clockwise -/// direction): This function does not check for convexity -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Beginning of point sequence -/// \param[in] end One past the last of the point sequence -/// \return Whether or not all point triples p_i, p_{i+1}, p_{i+2} are in a particular order. -/// Returns true for collinear points as well -template -bool all_ordered(const IT begin, const IT end) noexcept -{ - // Short circuit: a line or point is always CCW or otherwise ill-defined - if (std::distance(begin, end) <= 2U) { - return true; - } - bool is_first_point_cw = false; - // Can't use std::all_of because that works directly on the values - for (auto line_start = begin; line_start != end; ++line_start) { - const auto line_end = details::circular_next(begin, end, line_start); - const auto query_point = details::circular_next(begin, end, line_end); - // Check if 3 points starting from current point are in clockwise direction - const bool is_cw = comparison::abs_lte( - check_point_position_to_line_2d(*line_start, *line_end, *query_point), 0.0F, 1e-3F); - if (is_cw) { - if (line_start == begin) { - is_first_point_cw = true; - } else { - if (!is_first_point_cw) { - return false; - } - } - } else if (is_first_point_cw) { - return false; - } - } - return true; -} - -/// Compute the area of a convex hull, points are assumed to be ordered (in either CW or CCW) -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Iterator pointing to the beginning of the polygon points -/// \param[in] end Iterator pointing to one past the last of the polygon points -/// \return The area of the polygon, in squared of whatever units your points are in -template -auto area_2d(const IT begin, const IT end) noexcept -{ - using point_adapter::x_; - using point_adapter::y_; - using T = decltype(x_(*begin)); - auto area = T{}; // zero initialization - // use the approach of https://www.mathwords.com/a/area_convex_polygon.htm - for (auto it = begin; it != end; ++it) { - const auto next = details::circular_next(begin, end, it); - area += x_(*it) * y_(*next); - area -= x_(*next) * y_(*it); - } - return std::abs(T{0.5} * area); -} - -/// Compute area of convex hull, throw if points are not ordered (convexity check is not -/// implemented) -/// \throw std::domain_error if points are not ordered either CW or CCW -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Iterator pointing to the beginning of the polygon points -/// \param[in] end Iterator pointing to one past the last of the polygon points -/// \return The area of the polygon, in squared of whatever units your points are in -template -auto area_checked_2d(const IT begin, const IT end) -{ - if (!all_ordered(begin, end)) { - throw std::domain_error{"Cannot compute area: points are not ordered"}; - } - return area_2d(begin, end); -} - -/// \brief Check if the given point is inside or on the edge of the given polygon -/// \tparam IteratorType iterator type. The value pointed to by this must have point adapters -/// defined or have float members x and y -/// \tparam PointType point type. Must have point adapters defined or have float members x and y -/// \param start_it iterator pointing to the first vertex of the polygon -/// \param end_it iterator pointing to the last vertex of the polygon. The vertices should be in -/// order. -/// \param p point to be searched -/// \return True if the point is inside or on the edge of the polygon. False otherwise -template -bool is_point_inside_polygon_2d( - const IteratorType & start_it, const IteratorType & end_it, const PointType & p) -{ - std::int32_t winding_num = 0; - - for (IteratorType it = start_it; it != end_it; ++it) { - auto next_it = std::next(it); - if (next_it == end_it) { - next_it = start_it; - } - if (point_adapter::y_(*it) <= point_adapter::y_(p)) { - // Upward crossing edge - if (point_adapter::y_(*next_it) >= point_adapter::y_(p)) { - if (comparison::abs_gt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { - ++winding_num; - } else { - if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) { - return true; - } - } - } - } else { - // Downward crossing edge - if (point_adapter::y_(*next_it) <= point_adapter::y_(p)) { - if (comparison::abs_lt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { - --winding_num; - } else { - if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) { - return true; - } - } - } - } - } - return winding_num != 0; -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp deleted file mode 100644 index 74cd210dec586..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file includes common functionality for 3D geometry, such as dot products - -#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x, y and z -/// \brief compute p * q = p1 * q1 + p2 * q2 + p3 * 13 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 3d scalar dot product -template -inline auto dot_3d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - using point_adapter::z_; - return (x_(pt) * x_(q)) + (y_(pt) * y_(q) + z_(pt) * z_(q)); -} - -/// \brief Compute 3D squared euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x y and z -/// \tparam T2 point type. Must have point adapters defined or have float members x y and z -/// \param a point 1 -/// \param b point 2 -/// \return squared 3D euclidean distance -template -inline OUT squared_distance_3d(const T1 & a, const T2 & b) -{ - const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); - const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); - const auto z = static_cast(point_adapter::z_(a)) - static_cast(point_adapter::z_(b)); - return (x * x) + (y * y) + (z * z); -} - -/// \brief Compute 3D euclidean distance between two points -/// \tparam T1 point type. Must have point adapters defined or have float members x y and z -/// \tparam T2 point type. Must have point adapters defined or have float members x y and z -/// \param a point 1 -/// \param b point 2 -/// \return 3D euclidean distance -template -inline OUT distance_3d(const T1 & a, const T2 & b) -{ - return std::sqrt(squared_distance_3d(a, b)); -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp deleted file mode 100644 index ae81c55ad6b55..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp +++ /dev/null @@ -1,195 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements the monotone chain algorithm to compute 2D convex hulls on linked -/// lists of points - -#ifndef AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -// lint -e537 NOLINT pclint vs cpplint -#include -// lint -e537 NOLINT pclint vs cpplint -#include -#include -#include - -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief Contains computation geometry functions not intended for the end user to directly use -namespace details -{ - -/// \brief Moves points comprising the lower convex hull from points to hull. -/// \param[inout] points A list of points, assumed to be sorted in lexical order -/// \param[inout] hull An empty list of points, assumed to have same allocator as points -/// (for splice) -/// \tparam PointT The point type for the points list -/// \tparam HullT the point type for the hull list -template -void form_lower_hull(std::list & points, std::list & hull) -{ - auto hull_it = hull.cbegin(); - auto point_it = points.cbegin(); - // This ensures that the points we splice to back to the end of list are not processed - const auto iters = points.size(); - for (auto idx = decltype(iters){0}; idx < iters; ++idx) { - // splice points from tail of hull to tail of list until point from head of list satisfies ccw - bool8_t is_ccw = true; - while ((hull.cbegin() != hull_it) && is_ccw) { - const auto current_hull_it = hull_it; - --hull_it; - is_ccw = ccw(*hull_it, *current_hull_it, *point_it); - if (!is_ccw) { - hull_it = current_hull_it; - break; - } - // return this node to list for consideration in upper hull - points.splice(points.end(), hull, current_hull_it); - } - const auto last_point_it = point_it; - ++point_it; - // Splice head of list to tail of (lower) hull - hull.splice(hull.end(), points, last_point_it); - // point_it has been advanced, hull_it has been advanced (to where point_it was previously) - hull_it = last_point_it; - } - // loop is guaranteed not to underflow because a node can be removed from points AT MOST once - // per loop iteration. The loop is upper bounded to the prior size of the point list -} -/// \brief Moves points comprising the lower convex hull from points to hull. -/// \param[inout] points A list of points, assumed to be sorted in lexical order, and to contain -/// the leftmost point -/// \param[inout] hull A list of points, assumed to have same allocator as points (for splice), -/// and to contain the lower hull (minus the left-most point) -/// \tparam PointT The point type for the points list -/// \tparam HullT the point type for the hull list -template -void form_upper_hull(std::list & points, std::list & hull) -{ - // TODO(c.ho) consider reverse iterators, not sure if they work with splice() - auto hull_it = hull.cend(); - --hull_it; - const auto lower_hull_end = hull_it; - auto point_it = points.cend(); - --point_it; - // This ensures that the points we splice to back to the head of list are not processed - const auto iters = points.size(); - for (auto idx = decltype(iters){0}; idx < iters; ++idx) { - // splice points from tail of hull to head of list until ccw is satisfied with tail of list - bool8_t is_ccw = true; - while ((lower_hull_end != hull_it) && is_ccw) { - const auto current_hull_it = hull_it; - --hull_it; - is_ccw = ccw(*hull_it, *current_hull_it, *point_it); - if (!is_ccw) { - hull_it = current_hull_it; - break; - } - points.splice(points.begin(), hull, current_hull_it); - } - const auto last_point_it = point_it; - --point_it; - // Splice points from tail of lexically ordered point list to tail of hull - hull.splice(hull.end(), points, last_point_it); - hull_it = last_point_it; - } - // loop is guaranteed not to underflow because a node can be removed from points AT MOST once - // per loop iteration. The loop is upper bounded to the prior size of the point list -} - -/// \brief A static memory implementation of convex hull computation. Shuffles points around the -/// deque such that the points of the convex hull of the deque of points are first in the -/// deque, with the internal points following in an unspecified order. -/// The head of the deque will be the point with the smallest x value, with the other -/// points following in a counter-clockwise manner (from a top down view/facing -z direction) -/// \param[inout] list A list of nodes that will be pruned down and reordered into a ccw convex hull -/// \return An iterator pointing to one after the last point contained in the hull -/// \tparam PointT Type of a point, must have x and y float members -template -typename std::list::const_iterator convex_hull_impl(std::list & list) -{ - // Functor that return whether a <= b in the lexical sense (a.x < b.x), sort by y if tied - const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t { - using point_adapter::x_; - using point_adapter::y_; - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - return (fabsf(x_(a) - x_(b)) > FEPS) ? (x_(a) < x_(b)) : (y_(a) < y_(b)); - }; - list.sort(lexical_comparator); - - // Temporary list to store points - std::list tmp_hull_list{list.get_allocator()}; - - // Shuffle lower hull points over to tmp_hull_list - form_lower_hull(list, tmp_hull_list); - - // Resort list since we can't guarantee the order TODO(c.ho) is this true? - list.sort(lexical_comparator); - // splice first hull point to beginning of list to ensure upper hull is properly closed - // This is done after sorting because we know exactly where it should go, and keeping it out of - // sorting reduces complexity somewhat - list.splice(list.begin(), tmp_hull_list, tmp_hull_list.begin()); - - // build upper hull - form_upper_hull(list, tmp_hull_list); - // Move hull to beginning of list, return iterator pointing to one after the convex hull - const auto ret = list.begin(); - // first move left-most point to ensure it is first - auto tmp_it = tmp_hull_list.end(); - --tmp_it; - list.splice(list.begin(), tmp_hull_list, tmp_it); - // Then move remainder of hull points - list.splice(ret, tmp_hull_list); - return ret; -} -} // namespace details - -/// \brief A static memory implementation of convex hull computation. Shuffles points around the -/// deque such that the points of the convex hull of the deque of points are first in the -/// deque, with the internal points following in an unspecified order. -/// -/// The head of the deque will be the point with the smallest x value, with the other -/// points following in a counter-clockwise manner (from a top down view/facing -z -/// direction). If the point list is 3 or smaller, nothing is done (e.g. the ordering result -/// as previously stated does not hold). -/// \param[inout] list A list of nodes that will be reordered into a ccw convex hull -/// \return An iterator pointing to one after the last point contained in the hull -/// \tparam PointT Type of a point, must have x and y float members -template -typename std::list::const_iterator convex_hull(std::list & list) -{ - return (list.size() <= 3U) ? list.end() : details::convex_hull_impl(list); -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp deleted file mode 100644 index cd9fd49f71635..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp +++ /dev/null @@ -1,111 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland. 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements an algorithm for getting a list of "pockets" in the convex -/// hull of a non-convex simple polygon. - -#ifndef AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -#include -#include -#include -#include -#include - -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \brief Compute a list of "pockets" of a simple polygon -/// (https://en.wikipedia.org/wiki/Simple_polygon), that is, the areas that make -/// up the difference between the polygon and its convex hull. -/// This currently has a bug: -// * "Rollover" is not properly handled - if a pocket contains the segment from -// the last point in the list to the first point (which is still part of the -// polygon), that does not get added -/// -/// \param[in] polygon_start Start iterator for the simple polygon (has to be on convex hull) -/// \param[in] polygon_end End iterator for the simple polygon -/// \param[in] convex_hull_start Start iterator for the convex hull of the simple polygon -/// \param[in] convex_hull_end End iterator for the convex hull of the simple polygon -/// \return A vector of vectors of the iterator value type. Each inner vector contains the points -/// for one pocket. We return by value instead of as iterator pairs, because it is possible -/// that the edge connecting the final point in the list and the first point in the list is -/// part of a pocket as well, and this is awkward to represent using iterators into the -/// original collection. -/// -/// \tparam Iter1 Iterator to a point type -/// \tparam Iter2 Iterator to a point type (can be the same as Iter1, but does not need to be) -template -typename std::vector::value_type>> hull_pockets( - const Iter1 polygon_start, const Iter1 polygon_end, const Iter2 convex_hull_start, - const Iter2 convex_hull_end) -{ - auto pockets = std::vector::value_type>>{}; - if (std::distance(polygon_start, polygon_end) <= 3) { - return pockets; - } - - // Function to check whether a point is in the convex hull. - const auto in_convex_hull = [convex_hull_start, convex_hull_end](Iter1 p) { - return std::any_of(convex_hull_start, convex_hull_end, [p](auto hull_entry) { - return norm_2d(minus_2d(hull_entry, *p)) < std::numeric_limits::epsilon(); - }); - }; - - // We go through the points of the polygon only once, adding pockets to the list of pockets - // as we detect them. - std::vector::value_type> current_pocket{}; - for (auto it = polygon_start; it != polygon_end; it = std::next(it)) { - if (in_convex_hull(it)) { - if (current_pocket.size() > 1) { - current_pocket.emplace_back(*it); - pockets.push_back(current_pocket); - } - current_pocket.clear(); - current_pocket.emplace_back(*it); - } else { - current_pocket.emplace_back(*it); - } - } - - // At this point, we have reached the end of the polygon, but have not considered the connection - // of the final point back to the first. In case the first point is part of a pocket as well, we - // have some points left in current_pocket, and we add one final pocket that is made up by those - // points plus the first point in the collection. - if (current_pocket.size() > 1) { - current_pocket.push_back(*polygon_start); - pockets.push_back(current_pocket); - } - - return pockets; -} -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp deleted file mode 100644 index 08c70c3a5a6be..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/convex_hull.hpp" - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -using autoware::common::geometry::closest_line_point_2d; -using autoware::common::geometry::convex_hull; -using autoware::common::geometry::dot_2d; -using autoware::common::geometry::get_normal; -using autoware::common::geometry::minus_2d; -using autoware::common::geometry::norm_2d; -using autoware::common::geometry::times_2d; -using autoware_auto_perception_msgs::msg::BoundingBox; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; - -using Point = geometry_msgs::msg::Point32; - -namespace details -{ - -/// Alias for a std::pair of two points -using Line = std::pair; - -/// \tparam Iter1 Iterator over point-types that must have point adapters -// defined or have float members x and y -/// \brief Compute a sorted list of faces of a polyhedron given a list of points -/// \param[in] start Start iterator of the list of points -/// \param[in] end End iterator of the list of points -/// \return The list of faces -template -std::vector get_sorted_face_list(const Iter start, const Iter end) -{ - // First get a sorted list of points - convex_hull does that by modifying its argument - auto corner_list = std::list(start, end); - const auto first_interior_point = convex_hull(corner_list); - - std::vector face_list{}; - auto itLast = corner_list.begin(); - auto itNext = std::next(itLast, 1); - do { - face_list.emplace_back(Line{*itLast, *itNext}); - itLast = itNext; - itNext = std::next(itNext, 1); - } while ((itNext != first_interior_point) && (itNext != corner_list.end())); - - face_list.emplace_back(Line{*itLast, corner_list.front()}); - - return face_list; -} - -/// \brief Append points of the polygon `internal` that are contained in the polygon `external`. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -void append_contained_points( - const Iterable1T & external, const Iterable2T & internal, - std::list & result) -{ - std::copy_if( - internal.begin(), internal.end(), std::back_inserter(result), [&external](const auto & pt) { - return common::geometry::is_point_inside_polygon_2d(external.begin(), external.end(), pt); - }); -} - -/// \brief Append the intersecting points between two polygons into the output list. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -void append_intersection_points( - const Iterable1T & polygon1, const Iterable2T & polygon2, - std::list & result) -{ - using FloatT = decltype(point_adapter::x_(std::declval())); - using Interval = common::geometry::Interval; - - auto get_edge = [](const auto & list, const auto & iterator) { - const auto next_it = std::next(iterator); - const auto & next_pt = (next_it != list.end()) ? *next_it : list.front(); - return std::make_pair(*iterator, next_pt); - }; - - // Get the max absolute value out of two intervals and a scalar. - auto compute_eps_scale = [](const auto & i1, const auto & i2, const auto val) { - auto get_abs_max = [](const auto & interval) { - return std::max(std::fabs(Interval::min(interval)), std::fabs(Interval::max(interval))); - }; - return std::max(std::fabs(val), std::max(get_abs_max(i1), get_abs_max(i2))); - }; - - // Compare each edge from polygon1 to each edge from polygon2 - for (auto corner1_it = polygon1.begin(); corner1_it != polygon1.end(); ++corner1_it) { - const auto & edge1 = get_edge(polygon1, corner1_it); - - Interval edge1_x_interval{ - std::min(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second)), - std::max(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second))}; - - Interval edge1_y_interval{ - std::min(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second)), - std::max(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second))}; - - for (auto corner2_it = polygon2.begin(); corner2_it != polygon2.end(); ++corner2_it) { - try { - const auto & edge2 = get_edge(polygon2, corner2_it); - const auto & intersection = common::geometry::intersection_2d( - edge1.first, minus_2d(edge1.second, edge1.first), edge2.first, - minus_2d(edge2.second, edge2.first)); - - Interval edge2_x_interval{ - std::min(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second)), - std::max(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second))}; - - Interval edge2_y_interval{ - std::min(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second)), - std::max(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second))}; - - // cspell: ignore feps - // feps means "Float EPSilon" - - // The accumulated floating point error depends on the magnitudes of each end of the - // intervals. Hence the upper bound of the absolute magnitude should be taken into account - // while computing the epsilon. - const auto max_feps_scale = std::max( - compute_eps_scale(edge1_x_interval, edge2_x_interval, point_adapter::x_(intersection)), - compute_eps_scale(edge1_y_interval, edge2_y_interval, point_adapter::y_(intersection))); - const auto feps = max_feps_scale * std::numeric_limits::epsilon(); - // Only accept intersections that lie on both of the line segments (edges) - if ( - Interval::contains(edge1_x_interval, point_adapter::x_(intersection), feps) && - Interval::contains(edge2_x_interval, point_adapter::x_(intersection), feps) && - Interval::contains(edge1_y_interval, point_adapter::y_(intersection), feps) && - Interval::contains(edge2_y_interval, point_adapter::y_(intersection), feps)) { - result.push_back(intersection); - } - } catch (const std::runtime_error &) { - // Parallel lines. TODO(yunus.caliskan): #1229 - continue; - } - } - } -} - -} // namespace details - -// TODO(s.me) implement GJK(+EPA) algorithm as well as per Chris Ho's suggestion -/// \tparam Iter Iterator over point-types that must have point adapters -// defined or have float members x and y -/// \brief Check if polyhedra defined by two given sets of points intersect -// This uses SAT for doing the actual checking -// (https://en.wikipedia.org/wiki/Hyperplane_separation_theorem#Use_in_collision_detection) -/// \param[in] begin1 Start iterator to first list of point types -/// \param[in] end1 End iterator to first list of point types -/// \param[in] begin2 Start iterator to first list of point types -/// \param[in] end2 End iterator to first list of point types -/// \return true if the boxes collide, false otherwise. -template -bool intersect(const Iter begin1, const Iter end1, const Iter begin2, const Iter end2) -{ - // Obtain sorted lists of faces of both boxes, merge them into one big list of faces - auto faces = details::get_sorted_face_list(begin1, end1); - const auto faces_2 = details::get_sorted_face_list(begin2, end2); - faces.reserve(faces.size() + faces_2.size()); - faces.insert(faces.end(), faces_2.begin(), faces_2.end()); - - // Also look at last line - for (const auto & face : faces) { - // Compute normal vector to the face and define a closure to get progress along it - const auto normal = get_normal(minus_2d(face.second, face.first)); - auto get_position_along_line = [&normal](auto point) { - return dot_2d(normal, minus_2d(point, Point{})); - }; - - // Define a function to get the minimum and maximum projected position of the corners - // of a given bounding box along the normal line of the face - auto get_projected_min_max = [&get_position_along_line, &normal](Iter begin, Iter end) { - const auto zero_point = Point{}; - auto min_corners = get_position_along_line(closest_line_point_2d(normal, zero_point, *begin)); - auto max_corners = min_corners; - - for (auto & point = begin; point != end; ++point) { - const auto point_projected = closest_line_point_2d(normal, zero_point, *point); - const auto position_along_line = get_position_along_line(point_projected); - min_corners = std::min(min_corners, position_along_line); - max_corners = std::max(max_corners, position_along_line); - } - return std::pair{min_corners, max_corners}; - }; - - // Perform the actual computations for the extent computation - auto minmax_1 = get_projected_min_max(begin1, end1); - auto minmax_2 = get_projected_min_max(begin2, end2); - - // Check for any intersections - const auto eps = std::numeric_limits::epsilon(); - if (minmax_1.first > minmax_2.second + eps || minmax_2.first > minmax_1.second + eps) { - // Found separating hyperplane, stop - return false; - } - } - - // No separating hyperplane found, boxes collide - return true; -} - -/// \brief Get the intersection between two polygons. The polygons should be provided in an -/// identical format to the output of `convex_hull` function as in the corners should be ordered -/// in a CCW fashion. -/// The computation is done by: -/// * Find the corners of each polygon that are contained by the other polygon. -/// * Find the intersection points between two polygons -/// * Combine these points and order CCW to get the final polygon. -/// The criteria for intersection is better explained in: -/// "Area of intersection of arbitrary polygons" (Livermore, Calif, 1977) -/// See https://www.osti.gov/servlets/purl/7309916/, chapter II - B -/// TODO(yunus.caliskan): This is a naive implementation. We should scan for a more efficient -/// algorithm: #1230 -/// \tparam Iterable1T A container class that has stl style iterators defined. -/// \tparam Iterable2T A container class that has stl style iterators defined. -/// \tparam PointT Point type that have the adapters for the x and y fields. -/// set to `Point1T` -/// \param polygon1 A convex polygon -/// \param polygon2 A convex polygon -/// \return The resulting conv -template < - template class Iterable1T, template class Iterable2T, typename PointT> -std::list convex_polygon_intersection2d( - const Iterable1T & polygon1, const Iterable2T & polygon2) -{ - std::list result; - details::append_contained_points(polygon1, polygon2, result); - details::append_contained_points(polygon2, polygon1, result); - details::append_intersection_points(polygon1, polygon2, result); - const auto end_it = common::geometry::convex_hull(result); - result.resize(static_cast(std::distance(result.cbegin(), end_it))); - return result; -} - -/// \brief Compute the intersection over union of two 2d convex polygons. If any of the polygons -/// span a zero area, the result is 0.0. -/// \tparam Iterable1T A container class that has stl style iterators defined. -/// \tparam Iterable2T A container class that has stl style iterators defined. -/// \tparam Point1T Point type that have the adapters for the x and y fields. -/// \tparam Point2T Point type that have the adapters for the x and y fields. -/// \param polygon1 A convex polygon -/// \param polygon2 A convex polygon -/// \return (Intersection / Union) between two given polygons. -/// \throws std::domain_error If there is any inconsistency on the underlying geometrical -/// computation. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -common::types::float32_t convex_intersection_over_union_2d( - const Iterable1T & polygon1, const Iterable2T & polygon2) -{ - constexpr auto eps = std::numeric_limits::epsilon(); - const auto intersection = convex_polygon_intersection2d(polygon1, polygon2); - - const auto intersection_area = - common::geometry::area_2d(intersection.begin(), intersection.end()); - - if (intersection_area < eps) { - return 0.0F; // There's either no intersection or the points are collinear - } - - const auto polygon1_area = common::geometry::area_2d(polygon1.begin(), polygon1.end()); - const auto polygon2_area = common::geometry::area_2d(polygon2.begin(), polygon2.end()); - - const auto union_area = polygon1_area + polygon2_area - intersection_area; - if (union_area < eps) { - throw std::domain_error("IoU is undefined for polygons with a zero union area"); - } - - return intersection_area / union_area; -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp deleted file mode 100644 index 54be2c32b1d05..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp +++ /dev/null @@ -1,358 +0,0 @@ -// Copyright 2020 Mapless AI, Inc. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/** - * @brief Data structure to contain scalar interval bounds. - * - * @post The interval is guaranteed to be valid upon successful construction. An - * interval [min, max] is "valid" if it is empty (min/max = NaN) or its bounds - * are ordered (min <= max). - */ -template -class Interval -{ - static_assert(std::is_floating_point::value, "Intervals only support floating point types."); - -public: - /** - * @brief Compute equality. - * - * Two intervals compare equal iff they are both valid and they are both - * either empty or have equal bounds. - * - * @note This is defined inline because the class is templated. - * - * @return True iff the intervals compare equal. - */ - friend bool operator==(const Interval & i1, const Interval & i2) - { - const auto min_eq = (Interval::min(i1) == Interval::min(i2)); - const auto max_eq = (Interval::max(i1) == Interval::max(i2)); - const auto bounds_equal = (min_eq && max_eq); - const auto both_empty = (Interval::empty(i1) && Interval::empty(i2)); - return both_empty || bounds_equal; - } - - /** - * @brief Compute inequality and the logical negation of equality. - * @note This is defined inline because the class is templated. - */ - friend bool operator!=(const Interval & i1, const Interval & i2) { return !(i1 == i2); } - - /** - * @brief Stream overload for Interval types. - * - * @note Output precision is fixed inside the function definition, and the - * serialization is JSON compatible. - * - * @note The serialization is lossy. It is used for debugging and for - * generating exception strings. - */ - friend std::ostream & operator<<(std::ostream & os, const Interval & i) - { - constexpr auto PRECISION = 5; - - // Internal helper to format the output. - const auto readable_extremum = [](const T & val) { - if (val == std::numeric_limits::lowest()) { - return std::string("REAL_MIN"); - } - if (val == std::numeric_limits::max()) { - return std::string("REAL_MAX"); - } - - std::stringstream ss; - ss.setf(std::ios::fixed, std::ios::floatfield); - ss.precision(PRECISION); - ss << val; - return ss.str(); - }; - - // Do stream output. - if (Interval::empty(i)) { - return os << "{}"; - } - return os << "{\"min\": " << readable_extremum(Interval::min(i)) - << ", \"max\": " << readable_extremum(Interval::max(i)) << "}"; - } - - /** - * @brief Test whether the two intervals have bounds within epsilon of each - * other. - * - * @note If both intervals are empty, this returns true. If only one is empty, - * this returns false. - */ - static bool abs_eq(const Interval & i1, const Interval & i2, const T & eps); - - /** @brief The minimum bound of the interval. */ - static T min(const Interval & i) { return i.min_; } - - /** @brief The maximum bound of the interval. */ - static T max(const Interval & i) { return i.max_; } - - /** - * @brief Return the measure (length) of the interval. - * - * @note For empty or invalid intervals, NaN is returned. See Interval::empty - * for note on distinction between measure zero and the emptiness property. - */ - static T measure(const Interval & i); - - /** - * @brief Utility for checking whether an interval has zero measure. - * - * @note For empty or invalid intervals, false is returned. See - * Interval::empty for note on distinction between measure zero and the - * emptiness property. - * - * @return True iff the interval has zero measure. - */ - static bool zero_measure(const Interval & i); - - /** - * @brief Whether the interval is empty or not. - * - * @note Emptiness refers to whether the interval contains any points and is - * thus a distinct property from measure: an interval is non-empty if contains - * only a single point even though its measure in that case is zero. - * - * @return True iff the interval is empty. - */ - static bool empty(const Interval & i); - - /** - * @brief Test for whether a given interval contains a given value within the given epsilon - * @return True iff 'interval' contains 'value'. - */ - static bool contains( - const Interval & i, const T & value, const T & eps = std::numeric_limits::epsilon()); - - /** - * @brief Test for whether 'i1' subseteq 'i2' - * @return True iff i1 subseteq i2. - */ - static bool is_subset_eq(const Interval & i1, const Interval & i2); - - /** - * @brief Compute the intersection of two intervals as a new interval. - */ - static Interval intersect(const Interval & i1, const Interval & i2); - - /** - * @brief Clamp a scalar 'val' onto 'interval'. - * @return If 'val' in 'interval', return 'val'; otherwise return the nearer - * interval bound. - */ - static T clamp_to(const Interval & i, T val); - - /** - * @brief Constructor: initialize an empty interval with members set to NaN. - */ - Interval(); - - /** - * @brief Constructor: specify exact interval bounds. - * - * @note An empty interval is specified by setting both bounds to NaN. - * @note An exception is thrown if the specified bounds are invalid. - * - * @post Construction is successful iff the interval is valid. - */ - Interval(const T & min, const T & max); - -private: - static constexpr T NaN = std::numeric_limits::quiet_NaN(); - - T min_; - T max_; - - /** - * @brief Verify that the bounds are valid in an interval. - * @note This method is private because it can only be used in the - * constructor. Once an interval has been constructed, its bounds are - * guaranteed to be valid. - */ - static bool bounds_valid(const Interval & i); -}; // class Interval - -//------------------------------------------------------------------------------ - -typedef Interval Interval_d; -typedef Interval Interval_f; - -//------------------------------------------------------------------------------ - -template -constexpr T Interval::NaN; - -//------------------------------------------------------------------------------ - -template -Interval::Interval() : min_(Interval::NaN), max_(Interval::NaN) -{ -} - -//------------------------------------------------------------------------------ - -template -Interval::Interval(const T & min, const T & max) : min_(min), max_(max) -{ - if (!Interval::bounds_valid(*this)) { - std::stringstream ss; - ss << "Attempted to construct an invalid interval: " << *this; - throw std::runtime_error(ss.str()); - } -} - -//------------------------------------------------------------------------------ - -template -bool Interval::abs_eq(const Interval & i1, const Interval & i2, const T & eps) -{ - namespace comp = helper_functions::comparisons; - - const auto both_empty = Interval::empty(i1) && Interval::empty(i2); - const auto both_non_empty = !Interval::empty(i1) && !Interval::empty(i2); - - const auto mins_equal = comp::abs_eq(Interval::min(i1), Interval::min(i2), eps); - const auto maxes_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps); - const auto both_non_empty_equal = both_non_empty && mins_equal && maxes_equal; - - return both_empty || both_non_empty_equal; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::zero_measure(const Interval & i) -{ - // An empty interval will return false because (NaN == NaN) is false. - return Interval::min(i) == Interval::max(i); -} - -//------------------------------------------------------------------------------ - -template -bool Interval::empty(const Interval & i) -{ - return std::isnan(Interval::min(i)) && std::isnan(Interval::max(i)); -} - -//------------------------------------------------------------------------------ - -template -bool Interval::bounds_valid(const Interval & i) -{ - const auto is_ordered = (Interval::min(i) <= Interval::max(i)); - - // Check for emptiness explicitly because it requires both bounds to be NaN - return Interval::empty(i) || is_ordered; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::is_subset_eq(const Interval & i1, const Interval & i2) -{ - const auto lower_contained = (Interval::min(i1) >= Interval::min(i2)); - const auto upper_contained = (Interval::max(i1) <= Interval::max(i2)); - return lower_contained && upper_contained; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::contains(const Interval & i, const T & value, const T & eps) -{ - return common::helper_functions::comparisons::abs_gte(value, Interval::min(i), eps) && - common::helper_functions::comparisons::abs_lte(value, Interval::max(i), eps); -} - -//------------------------------------------------------------------------------ - -template -T Interval::measure(const Interval & i) -{ - return Interval::max(i) - Interval::min(i); -} - -//------------------------------------------------------------------------------ - -template -Interval Interval::intersect(const Interval & i1, const Interval & i2) -{ - // Construct intersection assuming an intersection exists. - try { - const auto either_empty = Interval::empty(i1) || Interval::empty(i2); - const auto min = std::max(Interval::min(i1), Interval::min(i2)); - const auto max = std::min(Interval::max(i1), Interval::max(i2)); - return either_empty ? Interval() : Interval(min, max); - } catch (...) { - } - - // Otherwise, the intersection is empty. - return Interval(); -} - -//------------------------------------------------------------------------------ - -template -T Interval::clamp_to(const Interval & i, T val) -{ - // clamp val to min - val = std::max(val, Interval::min(i)); - - // clamp val to max - val = std::min(val, Interval::max(i)); - - return Interval::empty(i) ? Interval::NaN : val; -} - -//------------------------------------------------------------------------------ - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp deleted file mode 100644 index 7b8867ca096ae..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file contains a 1D linear lookup table implementation - -#ifndef AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace helper_functions -{ - -namespace -{ - -/** - * @brief Compute a scaling between 'a' and 'b' - * @note scaling is clamped to be in the interval [0, 1] - */ -template -T interpolate(const T & a, const T & b, U scaling) -{ - static const geometry::Interval UNIT_INTERVAL(static_cast(0), static_cast(1)); - scaling = geometry::Interval::clamp_to(UNIT_INTERVAL, scaling); - - const auto m = (b - a); - const auto offset = static_cast(m) * scaling; - return a + static_cast(offset); -} - -// TODO(c.ho) support more forms of interpolation as template functor -// Actual lookup logic, assuming all invariants hold: -// Throw if value is not finite -template -T lookup_impl_1d(const std::vector & domain, const std::vector & range, const T value) -{ - if (!std::isfinite(value)) { - throw std::domain_error{"Query value is not finite (NAN or INF)"}; - } - if (value <= domain.front()) { - return range.front(); - } else if (value >= domain.back()) { - return range.back(); - } else { - // Fall through to normal case - } - - auto second_idx{0U}; - for (auto idx = 1U; idx < domain.size(); ++idx) { - if (value < domain[idx]) { - second_idx = idx; - break; - } - } - // T must be a floating point between 0 and 1 - const auto num = static_cast(value - domain[second_idx - 1U]); - const auto den = static_cast(domain[second_idx] - domain[second_idx - 1U]); - const auto t = num / den; - const auto val = interpolate(range[second_idx - 1U], range[second_idx], t); - return static_cast(val); -} - -// Check invariants for table lookup: -template -void check_table_lookup_invariants(const std::vector & domain, const std::vector & range) -{ - if (domain.size() != range.size()) { - throw std::domain_error{"Domain's size does not match range's"}; - } - if (domain.empty()) { - throw std::domain_error{"Empty domain or range"}; - } - // Check if sorted: Can start at 1 because not empty - for (auto idx = 1U; idx < domain.size(); ++idx) { - if (domain[idx] <= domain[idx - 1U]) { // This is safe because indexing starts at 1 - throw std::domain_error{"Domain is not sorted"}; - } - } -} -} // namespace - -/// Do a 1D table lookup: Does some semi-expensive O(N) error checking first. -/// If query value fall out of the domain, then the value at the corresponding edge of the domain is -/// returned. -/// \param[in] domain The domain, or set of x values -/// \param[in] range The range, or set of y values -/// \param[in] value The point in the domain to query, x -/// \return A linearly interpolated value y, corresponding to the query, x -/// \throw std::domain_error If domain or range is empty -/// \throw std::domain_error If range is not the same size as domain -/// \throw std::domain_error If domain is not sorted -/// \throw std::domain_error If value is not finite (NAN or INF) -/// \tparam T The type of the function, must be interpolatable -template -T lookup_1d(const std::vector & domain, const std::vector & range, const T value) -{ - check_table_lookup_invariants(domain, range); - - return lookup_impl_1d(domain, range, value); -} - -/// A class wrapping a 1D lookup table. Intended for more frequent lookups. Error checking is pushed -/// into the constructor and not done in the lookup function call -/// \tparam T The type of the function, must be interpolatable -template -class LookupTable1D -{ -public: - /// Constructor - /// \param[in] domain The domain, or set of x values - /// \param[in] range The range, or set of y values - /// \throw std::domain_error If domain or range is empty - /// \throw std::domain_error If range is not the same size as domain - /// \throw std::domain_error If domain is not sorted - LookupTable1D(const std::vector & domain, const std::vector & range) - : m_domain{domain}, m_range{range} - { - check_table_lookup_invariants(m_domain, m_range); - } - - /// Move constructor - /// \param[in] domain The domain, or set of x values - /// \param[in] range The range, or set of y values - /// \throw std::domain_error If domain or range is empty - /// \throw std::domain_error If range is not the same size as domain - /// \throw std::domain_error If domain is not sorted - LookupTable1D(std::vector && domain, std::vector && range) - : m_domain{domain}, m_range{range} - { - check_table_lookup_invariants(m_domain, m_range); - } - - /// Do a 1D table lookup - /// If query value fall out of the domain, then the value at the corresponding edge of the domain - /// is returned. - /// \param[in] value The point in the domain to query, x - /// \return A linearly interpolated value y, corresponding to the query, x - /// \throw std::domain_error If value is not finite - T lookup(const T value) const { return lookup_impl_1d(m_domain, m_range, value); } - /// Get the domain table - const std::vector & domain() const noexcept { return m_domain; } - /// Get the range table - const std::vector & range() const noexcept { return m_range; } - -private: - std::vector m_domain; - std::vector m_range; -}; // class LookupTable1D - -} // namespace helper_functions -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp deleted file mode 100644 index 8936e2c17d779..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp +++ /dev/null @@ -1,332 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in -/// 2D - -#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ - -#include "autoware_auto_geometry/spatial_hash_config.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include - -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup -namespace spatial_hash -{ - -/// \brief An implementation of the spatial hash or integer lattice data structure for efficient -/// (O(1)) near neighbor queries. -/// \tparam PointT The point type stored in this data structure. Must have float members x, y, and z -/// -/// This implementation can support both 2D and 3D queries -/// (though only one type per data structure), and can support queries of varying radius. This data -/// structure cannot do near neighbor lookups for euclidean distance in arbitrary dimensions. -template -class GEOMETRY_PUBLIC SpatialHashBase -{ - using Index3 = details::Index3; - // lint -e{9131} NOLINT There's no other way to make this work in a static assert - static_assert( - std::is_same::value || std::is_same::value, - "SpatialHash only works with Config2d or Config3d"); - -public: - using Hash = std::unordered_multimap; - using IT = typename Hash::const_iterator; - /// \brief Wrapper around an iterator and a distance (from some query point) - class Output - { - public: - /// \brief Constructor - /// \param[in] iterator An iterator pointing to some point - /// \param[in] distance The euclidean distance (2d or 3d) to a reference point - Output(const IT iterator, const float32_t distance) : m_iterator(iterator), m_distance(distance) - { - } - /// \brief Get stored point - /// \return A const reference to the stored point - const PointT & get_point() const { return m_iterator->second; } - /// \brief Get underlying iterator - /// \return A copy of the underlying iterator - IT get_iterator() const { return m_iterator; } - /// \brief Convert to underlying point - /// \return A reference to the underlying point - operator const PointT &() const { return get_point(); } - /// \brief Convert to underlying iterator - /// \return A copy of the iterator - operator IT() const { return get_iterator(); } - /// \brief Get distance to reference point - /// \return The distance - float32_t get_distance() const { return m_distance; } - - private: - IT m_iterator; - float32_t m_distance; - }; // class Output - using OutputVector = typename std::vector; - - /// \brief Constructor - /// \param[in] cfg The configuration object for this class - explicit SpatialHashBase(const ConfigT & cfg) - : m_config{cfg}, - m_hash(), - m_neighbors{}, // TODO(c.ho) reserve, but there's no default constructor for output - m_bins_hit{}, // zero initialization (and below) - m_neighbors_found{} - { - } - - /// \brief Inserts point - /// \param[in] pt The Point to insert - /// \return Iterator pointing to the inserted point - /// \throw std::length_error If the data structure is at capacity - IT insert(const PointT & pt) - { - if (size() >= capacity()) { - throw std::length_error{"SpatialHash: Cannot insert past capacity"}; - } - return insert_impl(pt); - } - - /// \brief Inserts a range of points - /// \param[in] begin The start of the range of points to insert - /// \param[in] end The end of the range of points to insert - /// \tparam IteratorT The iterator type - /// \throw std::length_error If the range of points to insert exceeds the data structure's - /// capacity - template - void insert(IteratorT begin, IteratorT end) - { - // This check is here for strong exception safety - if ((size() + std::distance(begin, end)) > capacity()) { - throw std::length_error{"SpatialHash: Cannot multi-insert past capacity"}; - } - for (IteratorT it = begin; it != end; ++it) { - (void)insert_impl(*it); - } - } - - /// \brief Removes the specified element from the data structure - /// \param[in] point An iterator pointing to a point to be removed - /// \return An iterator pointing to the element after the erased element - /// \throw std::domain_error If pt is invalid or does not belong to this data structure - /// - /// \note There is no reliable way to check if an iterator is invalid. The checks here are - /// based on a heuristic and is not guaranteed to find all invalid iterators. This method - /// should be used with care and only on valid iterators - IT erase(const IT point) - { - if (end() == m_hash.find(point->first)) { - throw std::domain_error{"SpatialHash: Attempting to erase invalid iterator"}; - } - return m_hash.erase(point); - } - - /// \brief Reset the state of the data structure - void clear() { m_hash.clear(); } - /// \brief Get current number of element stored in this data structure - /// \return Number of stored elements - Index size() const { return m_hash.size(); } - /// \brief Get the maximum capacity of the data structure - /// \return The capacity of the data structure - Index capacity() const { return m_config.get_capacity(); } - /// \brief Whether the hash is empty - /// \return True if data structure is empty - bool8_t empty() const { return m_hash.empty(); } - /// \brief Get iterator to beginning of data structure - /// \return Iterator - IT begin() const { return m_hash.begin(); } - /// \brief Get iterator to end of data structure - /// \return Iterator - IT end() const { return m_hash.end(); } - /// \brief Get iterator to beginning of data structure - /// \return Iterator - IT cbegin() const { return begin(); } - /// \brief Get iterator to end of data structure - /// \return Iterator - IT cend() const { return end(); } - - /// \brief Get the number of bins touched during the lifetime of this object, for debugging and - /// size tuning - /// \return The total number of bins touched during near() queries - Index bins_hit() const { return m_bins_hit; } - - /// \brief Get number of near neighbors found during the lifetime of this object, for debugging - /// and size tuning - /// \return The total number of neighbors found during near() queries - Index neighbors_found() const { return m_neighbors_found; } - -protected: - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] z The z component of the reference point, respected only if the spatial hash is not - /// 2D. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near_impl( - const float32_t x, const float32_t y, const float32_t z, const float32_t radius) - { - // reset output - m_neighbors.clear(); - // Compute bin, bin range - const Index3 ref_idx = m_config.index3(x, y, z); - const float32_t radius2 = radius * radius; - const details::BinRange idx_range = m_config.bin_range(ref_idx, radius); - Index3 idx = idx_range.first; - // For bins in radius - do { // guaranteed to have at least the bin ref_idx is in - // update book-keeping - ++m_bins_hit; - // Iterating in a square/cube pattern is easier than constructing sphere pattern - if (m_config.is_candidate_bin(ref_idx, idx, radius2)) { - // For point in bin - const Index jdx = m_config.index(idx); - const auto range = m_hash.equal_range(jdx); - for (auto it = range.first; it != range.second; ++it) { - const auto & pt = it->second; - const float32_t dist2 = m_config.distance_squared(x, y, z, pt); - if (dist2 <= radius2) { - // Only compute true distance if necessary - m_neighbors.emplace_back(it, sqrtf(dist2)); - } - } - } - } while (m_config.next_bin(idx_range, idx)); - // update book-keeping - m_neighbors_found += m_neighbors.size(); - return m_neighbors; - } - -private: - /// \brief Internal insert method with no error checking - /// \param[in] pt The Point to insert - GEOMETRY_LOCAL IT insert_impl(const PointT & pt) - { - // Compute bin - const Index idx = - m_config.bin(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt)); - // Insert into bin - return m_hash.insert(std::make_pair(idx, pt)); - } - - const ConfigT m_config; - Hash m_hash; - OutputVector m_neighbors; - Index m_bins_hit; - Index m_neighbors_found; -}; // class SpatialHashBase - -/// \brief The class to be used for specializing on -/// apex_app::common::geometry::spatial_hash::SpatialHashBase to provide different function -/// signatures on 2D and 3D configurations -/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z -template -class GEOMETRY_PUBLIC SpatialHash; - -/// \brief Explicit specialization of SpatialHash for 2D configuration -/// \tparam PointT The point type stored in this data structure. -template -class GEOMETRY_PUBLIC SpatialHash : public SpatialHashBase -{ -public: - using OutputVector = typename SpatialHashBase::OutputVector; - - explicit SpatialHash(const Config2d & cfg) : SpatialHashBase(cfg) {} - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const float32_t x, const float32_t y, const float32_t radius) - { - return this->near_impl(x, y, 0.0F, radius); - } - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] pt The reference point. Only the x and y members are respected. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const PointT & pt, const float32_t radius) - { - return near(point_adapter::x_(pt), point_adapter::y_(pt), radius); - } -}; - -/// \brief Explicit specialization of SpatialHash for 3D configuration -/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z -template -class GEOMETRY_PUBLIC SpatialHash : public SpatialHashBase -{ -public: - using OutputVector = typename SpatialHashBase::OutputVector; - - explicit SpatialHash(const Config3d & cfg) : SpatialHashBase(cfg) {} - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] z The z component of the reference point, respected only if the spatial hash is not - /// 2D. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near( - const float32_t x, const float32_t y, const float32_t z, const float32_t radius) - { - return this->near_impl(x, y, z, radius); - } - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] pt The reference point. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const PointT & pt, const float32_t radius) - { - return near(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt), radius); - } -}; - -template -using SpatialHash2d = SpatialHash; -template -using SpatialHash3d = SpatialHash; -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp deleted file mode 100644 index 24c4d6e879d4a..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp +++ /dev/null @@ -1,450 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in -/// 2D - -#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup -namespace spatial_hash -{ -/// \brief Index type for identifying bins of the hash/lattice -using Index = std::size_t; -/// \brief Spatial hash functionality not intended to be used by an external user -namespace details -{ -/// \brief Internal struct for packing three indices together -/// -/// The use of this struct publicly is a violation of our coding standards, but I claim it's -/// fine because (a) it's details, (b) it is literally three unrelated members packaged together. -/// This type is needed for conceptual convenience so I don't have massive function parameter -/// lists -struct GEOMETRY_PUBLIC Index3 -{ - Index x; - Index y; - Index z; -}; // struct Index3 - -using BinRange = std::pair; -} // namespace details - -/// \brief The base class for the configuration object for the SpatialHash class -/// \tparam Derived The type of the derived class to support static polymorphism/CRTP -template -class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp -{ -public: - /// \brief Constructor for spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] min_z The minimum z value for the spatial hash - /// \param[in] max_z The maximum z value for the spatial hash - /// \param[in] radius The look up radius - /// \param[in] capacity The maximum number of points the spatial hash can store - Config( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity) - : m_min_x{min_x}, - m_min_y{min_y}, - m_min_z{min_z}, - m_max_x{max_x}, - m_max_y{max_y}, - m_max_z{max_z}, - m_side_length{radius}, - m_side_length2{radius * radius}, - m_side_length_inv{1.0F / radius}, - m_capacity{capacity}, - m_max_x_idx{check_basis_direction(min_x, max_x)}, - m_max_y_idx{check_basis_direction(min_y, max_y)}, - m_max_z_idx{check_basis_direction(min_z, max_z)}, - m_y_stride{m_max_x_idx + 1U}, - m_z_stride{m_y_stride * (m_max_y_idx + 1U)} - { - if (radius <= 0.0F) { - throw std::domain_error("Error constructing SpatialHash: must have positive side length"); - } - - if ((m_max_y_idx + m_y_stride) > std::numeric_limits::max() / 2U) { - // TODO(c.ho) properly check for multiplication overflow - throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); - } - // small fudging to prevent weird boundary effects - // (e.g (x=x_max, y) rolls index over to (x=0, y+1) - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - // lint -e{1938} read only access is fine NOLINT - m_max_x -= FEPS; - m_max_y -= FEPS; - m_max_z -= FEPS; - if ((m_z_stride + m_max_z_idx) > std::numeric_limits::max() / 2U) { - // TODO(c.ho) properly check for multiplication overflow - throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); - } - // I don't know if this is even possible given other checks - if (std::numeric_limits::max() == m_max_z_idx) { - throw std::domain_error("SpatialHash::Config: max z index exceeds reasonable value"); - } - } - - /// \brief Given a reference index triple, compute the first and last bin - /// \param[in] ref The reference index triple - /// \param[in] radius The allowable radius for any point in the reference bin to any point in the - /// range - /// \return A pair where the first element is an index triple of the first bin, and the second - /// element is an index triple for the last bin - details::BinRange bin_range(const details::Index3 & ref, const float radius) const - { - // Compute distance in units of voxels - const Index i_radius = static_cast(std::ceil(radius / m_side_length)); - // Dumb ternary because potentially unsigned Index type - const Index x_min = (ref.x > i_radius) ? (ref.x - i_radius) : 0U; - const Index y_min = (ref.y > i_radius) ? (ref.y - i_radius) : 0U; - const Index z_min = (ref.z > i_radius) ? (ref.z - i_radius) : 0U; - // In 2D mode, m_max_z should be 0, same with ref.z - const Index x_max = std::min(ref.x + i_radius, m_max_x_idx); - const Index y_max = std::min(ref.y + i_radius, m_max_y_idx); - const Index z_max = std::min(ref.z + i_radius, m_max_z_idx); - // return bottom-left portion of cube and top-right portion of cube - return {{x_min, y_min, z_min}, {x_max, y_max, z_max}}; - } - - /// \brief Get next index within a given range - /// \return True if idx is valid and still in range - /// \param[in] range The max and min bin indices - /// \param[inout] idx The index to be incremented, updated even if a negative result occurs - bool8_t next_bin(const details::BinRange & range, details::Index3 & idx) const - { - // TODO(c.ho) is there any way to make this neater without triple nested if? - bool8_t ret = true; - ++idx.x; - if (idx.x > range.second.x) { - idx.x = range.first.x; - ++idx.y; - if (idx.y > range.second.y) { - idx.y = range.first.y; - ++idx.z; - if (idx.z > range.second.z) { - ret = false; - } - } - } - return ret; - } - /// \brief Get the maximum capacity of the spatial hash - /// \return The capacity - Index get_capacity() const { return m_capacity; } - - /// \brief Getter for the side length, equivalently the lookup radius - float32_t radius2() const { return m_side_length2; } - - //////////////////////////////////////////////////////////////////////////////////////////// - // "Polymorphic" API - /// \brief Compute the single index given a point - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The combined index of the bin for a given point - Index bin(const float32_t x, const float32_t y, const float32_t z) const - { - return this->impl().bin_(x, y, z); - } - /// \brief Compute whether the query bin and reference bin could possibly contain a pair of points - /// such that their distance is within a certain threshold - /// \param[in] ref The index triple of the bin containing the reference point - /// \param[in] query The index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if query and ref could possibly hold points within reference distance to one - /// another - bool is_candidate_bin( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const - { - return this->impl().valid(ref, query, ref_distance2); - } - /// \brief Compute the decomposed index given a point - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const - { - return this->impl().index3_(x, y, z); - } - /// \brief Compute the composed single index given a decomposed index - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index(const details::Index3 & idx) const { return this->impl().index_(idx); } - /// \brief Compute the squared distance between the two points - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (2d or 3d) - template - float32_t distance_squared( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - return this->impl().distance_squared_(x, y, z, pt); - } - -protected: - /// \brief Computes the index in the x basis direction - /// \param[in] x The x value of a point - /// \return The x offset of the index - Index x_index(const float32_t x) const - { - return static_cast( - std::floor((std::min(std::max(x, m_min_x), m_max_x) - m_min_x) * m_side_length_inv)); - } - /// \brief Computes the index in the y basis direction - /// \param[in] y The y value of a point - /// \return The x offset of the index - Index y_index(const float32_t y) const - { - return static_cast( - std::floor((std::min(std::max(y, m_min_y), m_max_y) - m_min_y) * m_side_length_inv)); - } - /// \brief Computes the index in the z basis direction - /// \param[in] z The z value of a point - /// \return The x offset of the index - Index z_index(const float32_t z) const - { - return static_cast( - std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv)); - } - /// \brief Compose the provided index offsets - Index bin_impl(const Index xdx, const Index ydx) const { return xdx + (ydx * m_y_stride); } - /// \brief Compose the provided index offsets - Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const - { - return bin_impl(xdx, ydx) + (zdx * m_z_stride); - } - - /// \brief The index offset of a point given it's x and y values - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \return The index of the point in the 2D case, or the offset within a z-slice of the hash - Index bin_impl(const float32_t x, const float32_t y) const - { - return bin_impl(x_index(x), y_index(y)); - } - /// \brief The index of a point given it's x, y and z values - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const - { - return bin_impl(x, y) + (z_index(z) * m_z_stride); - } - /// \brief The distance between two indices as a float, where adjacent indices have zero - /// distance (e.g. dist(0, 1) = 0) - float32_t idx_distance(const Index ref_idx, const Index query_idx) const - { - /// Not using fabs because Index is (possibly) unsigned - const Index i_dist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx); - float32_t dist = static_cast(i_dist) - 1.0F; - return std::max(dist, 0.0F); - } - - /// \brief Get side length squared - float side_length2() const { return m_side_length2; } - -private: - /// \brief Sanity check a range in a basis direction - /// \return The number of voxels in the given basis direction - Index check_basis_direction(const float32_t min, const float32_t max) const - { - if (min >= max) { - throw std::domain_error("SpatialHash::Config: must have min < max"); - } - // This family of checks is to ensure that you don't get weird casting effects due to huge - // floating point values - const float64_t dmax = static_cast(max); - const float64_t dmin = static_cast(min); - const float64_t width = (dmax - dmin) * static_cast(m_side_length_inv); - constexpr float64_t flt_max = static_cast(std::numeric_limits::max()); - if (flt_max <= width) { - throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit"); - } - return static_cast(width); - } - float32_t m_min_x; - float32_t m_min_y; - float32_t m_min_z; - float32_t m_max_x; - float32_t m_max_y; - float32_t m_max_z; - float32_t m_side_length; - float32_t m_side_length2; - float32_t m_side_length_inv; - Index m_capacity; - Index m_max_x_idx; - Index m_max_y_idx; - Index m_max_z_idx; - Index m_y_stride; - Index m_z_stride; -}; // class Config - -/// \brief Configuration class for a 2d spatial hash -class GEOMETRY_PUBLIC Config2d : public Config -{ -public: - /// \brief Config constructor for 2D spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] radius The lookup distance - /// \param[in] capacity The maximum number of points the spatial hash can store - Config2d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t radius, const Index capacity); - /// \brief The index of a point given it's x, y and z values, 2d implementation - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Determine if a bin could possibly hold a point within a distance to any point in a - /// reference bin for the 2D case - /// \param[in] ref The decomposed index triple of the reference bin - /// \param[in] query The decomposed index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if the reference bin and query bin could possibly hold a point within the - /// reference distance - bool valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const; - /// \brief Compute the decomposed index given a point, 2d implementation - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Compute the composed single index given a decomposed index, 2d implementation - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index_(const details::Index3 & idx) const; - /// \brief Compute the squared distance between the two points, 2d implementation - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (2d) - template - float32_t distance_squared_( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - (void)z; - const float32_t dx = x - point_adapter::x_(pt); - const float32_t dy = y - point_adapter::y_(pt); - return (dx * dx) + (dy * dy); - } -}; // class Config2d - -/// \brief Configuration class for a 3d spatial hash -class GEOMETRY_PUBLIC Config3d : public Config -{ -public: - /// \brief Config constructor for a 3d spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] min_z The minimum z value for the spatial hash - /// \param[in] max_z The maximum z value for the spatial hash - /// \param[in] radius The lookup distance - /// \param[in] capacity The maximum number of points the spatial hash can store - Config3d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity); - /// \brief The index of a point given it's x, y and z values, 3d implementation - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Determine if a bin could possibly hold a point within a distance to any point in a - /// reference bin for the 3D case - /// \param[in] ref The decomposed index triple of the reference bin - /// \param[in] query The decomposed index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if the reference bin and query bin could possibly hold a point within the - /// reference distance - bool valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const; - /// \brief Compute the decomposed index given a point, 3d implementation - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Compute the composed single index given a decomposed index, 3d implementation - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index_(const details::Index3 & idx) const; - /// \brief Compute the squared distance between the two points, 3d implementation - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (3d) - template - float32_t distance_squared_( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - const float32_t dx = x - point_adapter::x_(pt); - const float32_t dy = y - point_adapter::y_(pt); - const float32_t dz = z - point_adapter::z_(pt); - return (dx * dx) + (dy * dy) + (dz * dz); - } -}; // class Config3d -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp deleted file mode 100644 index 8972246997997..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllexport) -#define GEOMETRY_LOCAL -#else // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllimport) -#define GEOMETRY_LOCAL -#endif // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#elif defined(__linux__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(QNX) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#else // defined(__linux__) -#error "Unsupported Build Configuration" -#endif // defined(__WIN32) -#endif // AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_geometry/package.xml b/common/autoware_auto_geometry/package.xml deleted file mode 100644 index f53412298a485..0000000000000 --- a/common/autoware_auto_geometry/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_auto_geometry - 1.0.0 - Geometry related algorithms - Satoshi Ota - Apache License 2.0 - - Apex.AI, Inc. - - ament_cmake_auto - autoware_cmake - - autoware_auto_common - autoware_auto_geometry_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 - autoware_auto_vehicle_msgs - geometry_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - osrf_testing_tools_cpp - - - ament_cmake - - diff --git a/common/autoware_auto_geometry/src/bounding_box.cpp b/common/autoware_auto_geometry/src/bounding_box.cpp deleted file mode 100644 index 3a4ea96a151a2..0000000000000 --- a/common/autoware_auto_geometry/src/bounding_box.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" - -#include -// cspell: ignore eigenbox -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -// cspell: ignore lfit -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -#include - -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ -//////////////////////////////////////////////////////////////////////////////// -void size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret) -{ - ret.x = std::max( - norm_2d(minus_2d(corners[1U], corners[0U])), std::numeric_limits::epsilon()); - ret.y = std::max( - norm_2d(minus_2d(corners[2U], corners[1U])), std::numeric_limits::epsilon()); -} -//////////////////////////////////////////////////////////////////////////////// -void finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box) -{ - (void)std::copy(&corners[0U], &corners[corners.size()], &box.corners[0U]); - // orientation: this is robust to lines - const float32_t yaw_rad_2 = - atan2f(box.corners[2U].y - box.corners[1U].y, box.corners[2U].x - box.corners[1U].x) * 0.5F; - box.orientation.w = cosf(yaw_rad_2); - box.orientation.z = sinf(yaw_rad_2); - // centroid - box.centroid = times_2d(plus_2d(corners[0U], corners[2U]), 0.5F); -} - -autoware_auto_perception_msgs::msg::Shape make_shape(const BoundingBox & box) -{ - autoware_auto_perception_msgs::msg::Shape retval; - // Polygon is 2D rectangle - geometry_msgs::msg::Polygon polygon; - auto & points = polygon.points; - points.resize(4); - - // Note that the x and y of size field in BoundingBox should be swapped when being used to - // compute corners. - // origin of reference system: centroid of bbox - points[0].x = -0.5F * box.size.y; - points[0].y = -0.5F * box.size.x; - points[0].z = -box.size.z; - - points[1] = points[0]; - points[1].y += box.size.x; - - points[2] = points[1]; - points[2].x += box.size.y; - - points[3] = points[2]; - points[3].y -= box.size.x; - - retval.footprint = polygon; - retval.dimensions.z = 2 * box.size.z; - - return retval; -} - -autoware_auto_perception_msgs::msg::DetectedObject make_detected_object(const BoundingBox & box) -{ - autoware_auto_perception_msgs::msg::DetectedObject ret; - - ret.kinematics.pose_with_covariance.pose.position.x = static_cast(box.centroid.x); - ret.kinematics.pose_with_covariance.pose.position.y = static_cast(box.centroid.y); - ret.kinematics.pose_with_covariance.pose.position.z = static_cast(box.centroid.z); - ret.kinematics.pose_with_covariance.pose.orientation.x = static_cast(box.orientation.x); - ret.kinematics.pose_with_covariance.pose.orientation.y = static_cast(box.orientation.y); - ret.kinematics.pose_with_covariance.pose.orientation.z = static_cast(box.orientation.z); - ret.kinematics.pose_with_covariance.pose.orientation.w = static_cast(box.orientation.w); - ret.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; - - ret.shape = make_shape(box); - - ret.existence_probability = 1.0F; - - autoware_auto_perception_msgs::msg::ObjectClassification label; - label.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; - label.probability = 1.0F; - ret.classification.emplace_back(std::move(label)); - - return ret; -} - -std::vector GEOMETRY_PUBLIC get_transformed_corners( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation) -{ - std::vector retval(shape_msg.footprint.points.size()); - geometry_msgs::msg::TransformStamped tf; - tf.transform.rotation = orientation; - tf.transform.translation.x = centroid.x; - tf.transform.translation.y = centroid.y; - tf.transform.translation.z = centroid.z; - - for (size_t i = 0U; i < shape_msg.footprint.points.size(); ++i) { - tf2::doTransform(shape_msg.footprint.points[i], retval[i], tf); - } - return retval; -} - -} // namespace details -/////////////////////////////////////////////////////////////////////////////// -// pre-compilation -using autoware::common::types::PointXYZIF; -template BoundingBox minimum_area_bounding_box(std::list & list); -template BoundingBox minimum_perimeter_bounding_box(std::list & list); -using PointXYZIFVIT = std::vector::iterator; -template BoundingBox eigenbox_2d(const PointXYZIFVIT begin, const PointXYZIFVIT end); -template BoundingBox lfit_bounding_box_2d( - const PointXYZIFVIT begin, const PointXYZIFVIT end); -using geometry_msgs::msg::Point32; -template BoundingBox minimum_area_bounding_box(std::list & list); -template BoundingBox minimum_perimeter_bounding_box(std::list & list); -using Point32VIT = std::vector::iterator; -template BoundingBox eigenbox_2d(const Point32VIT begin, const Point32VIT end); -template BoundingBox lfit_bounding_box_2d(const Point32VIT begin, const Point32VIT end); -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware diff --git a/common/autoware_auto_geometry/src/spatial_hash.cpp b/common/autoware_auto_geometry/src/spatial_hash.cpp deleted file mode 100644 index ffd91aaa08b3a..0000000000000 --- a/common/autoware_auto_geometry/src/spatial_hash.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/spatial_hash.hpp" - -#include -// lint -e537 NOLINT repeated include file due to cpplint rule -#include -// lint -e537 NOLINT repeated include file due to cpplint rule -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace spatial_hash -{ -//////////////////////////////////////////////////////////////////////////////// -Config2d::Config2d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t radius, const Index capacity) -: Config(min_x, max_x, min_y, max_y, {}, std::numeric_limits::min(), radius, capacity) -{ -} -//////////////////////////////////////////////////////////////////////////////// -Index Config2d::bin_(const float32_t x, const float32_t y, const float32_t z) const -{ - (void)z; - return bin_impl(x, y); -} -//////////////////////////////////////////////////////////////////////////////// -bool Config2d::valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const -{ - const float dx = idx_distance(ref.x, query.x); - const float dy = idx_distance(ref.y, query.y); - // minor algebraic manipulation happening below - return (((dx * dx) + (dy * dy)) * side_length2()) <= ref_distance2; -} -//////////////////////////////////////////////////////////////////////////////// -details::Index3 Config2d::index3_(const float32_t x, const float32_t y, const float32_t z) const -{ - (void)z; - return {x_index(x), y_index(y), Index{}}; // zero initialization -} -//////////////////////////////////////////////////////////////////////////////// -Index Config2d::index_(const details::Index3 & idx) const -{ - return bin_impl(idx.x, idx.y); -} -//////////////////////////////////////////////////////////////////////////////// -Config3d::Config3d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity) -: Config(min_x, max_x, min_y, max_y, min_z, max_z, radius, capacity) -{ -} -//////////////////////////////////////////////////////////////////////////////// -Index Config3d::bin_(const float32_t x, const float32_t y, const float32_t z) const -{ - return bin_impl(x, y, z); -} -//////////////////////////////////////////////////////////////////////////////// -bool Config3d::valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const -{ - const float dx = idx_distance(ref.x, query.x); - const float dy = idx_distance(ref.y, query.y); - const float dz = idx_distance(ref.z, query.z); - // minor algebraic manipulation happening below - return (((dx * dx) + (dy * dy) + (dz * dz)) * side_length2()) <= ref_distance2; -} -//////////////////////////////////////////////////////////////////////////////// -details::Index3 Config3d::index3_(const float32_t x, const float32_t y, const float32_t z) const -{ - return {x_index(x), y_index(y), z_index(z)}; // zero initialization -} -//////////////////////////////////////////////////////////////////////////////// -Index Config3d::index_(const details::Index3 & idx) const -{ - return bin_impl(idx.x, idx.y, idx.z); -} -//////////////////////////////////////////////////////////////////////////////// -template class SpatialHash; -template class SpatialHash; -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp deleted file mode 100644 index a179fbde5f151..0000000000000 --- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp +++ /dev/null @@ -1,550 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef TEST_BOUNDING_BOX_HPP_ -#define TEST_BOUNDING_BOX_HPP_ - -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -// cspell: ignore lfit -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -#include - -#include -#include -#include - -using autoware::common::geometry::point_adapter::x_; -using autoware::common::geometry::point_adapter::xr_; -using autoware::common::geometry::point_adapter::y_; -using autoware::common::geometry::point_adapter::yr_; -using autoware_auto_perception_msgs::msg::BoundingBox; - -template -class BoxTest : public ::testing::Test -{ -protected: - std::list points; - BoundingBox box; - - void minimum_area_bounding_box() - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::minimum_area_bounding_box(points); - // apex_test_tools::memory_test::stop(); - } - - void minimum_perimeter_bounding_box() - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::minimum_perimeter_bounding_box(points); - // apex_test_tools::memory_test::stop(); - } - - // cspell: ignore eigenbox - template - void eigenbox(const IT begin, const IT end) - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::eigenbox_2d(begin, end); - // apex_test_tools::memory_test::stop(); - } - template - void lfit_bounding_box_2d(const IT begin, const IT end) - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::lfit_bounding_box_2d(begin, end); - // apex_test_tools::memory_test::stop(); - } - - PointT make(const float x, const float y) - { - PointT ret; - xr_(ret) = x; - yr_(ret) = y; - return ret; - } - - void check(const float cx, const float cy, const float sx, const float sy, const float val) const - { - constexpr float32_t TOL = 1.0E-4F; - ASSERT_LT(fabsf(box.size.x - sx), TOL); - ASSERT_LT(fabsf(box.size.y - sy), TOL); - ASSERT_LT(fabsf(box.value - val), TOL) << box.value; - - ASSERT_LT(fabsf(box.centroid.x - cx), TOL); - ASSERT_LT(fabsf(box.centroid.y - cy), TOL); - } - - void test_corners(const std::vector & expect, const float TOL = 1.0E-6F) const - { - for (uint32_t idx = 0U; idx < 4U; ++idx) { - bool found = false; - for (auto & p : expect) { - if (fabsf(x_(p) - box.corners[idx].x) < TOL && fabsf(y_(p) - box.corners[idx].y) < TOL) { - found = true; - break; - } - } - ASSERT_TRUE(found) << idx << ": " << box.corners[idx].x << ", " << box.corners[idx].y; - } - } - - /// \brief th_deg - phi_deg, normalized to +/- 180 deg - float32_t angle_distance_deg(const float th_deg, const float phi_deg) const - { - return fmodf((th_deg - phi_deg) + 540.0F, 360.0F) - 180.0F; - } - - /// \brief converts a radian value to a degree value - float32_t rad2deg(const float rad_val) const { return rad_val * 57.2958F; } - - void test_orientation(const float ref_angle_deg, const float TOL = 1.0E-4F) const - { - bool found = false; - const float angle_deg = rad2deg(2.0F * asinf(box.orientation.z)); - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 90.0F)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 180.0F)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 270.0F)) < TOL; - ASSERT_TRUE(found) << angle_deg; - } -}; // BoxTest - -// Instantiate tests for given types, add more types here as they are used -using PointTypesBoundingBox = - ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(BoxTest, PointTypesBoundingBox, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -// TODO(c.ho) consider typed and parameterized tests: -// https://stackoverflow.com/questions/3258230/passing-a-typename-and-string-to-parameterized-test-using-google-test - -/////////////////////////////////////////// -TYPED_TEST(BoxTest, PointSegmentDistance) -{ - using autoware::common::geometry::closest_segment_point_2d; - using autoware::common::geometry::point_line_segment_distance_2d; - // normal case - TypeParam p = this->make(-1.0F, 0.0F); - TypeParam q = this->make(-1.0F, 2.0F); - TypeParam r = this->make(1.0F, 1.0F); - TypeParam t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -1.0F); - ASSERT_FLOAT_EQ(y_(t), 1.0F); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 2.0F); - // boundary case - p = this->make(1.0F, 0.0F); - q = this->make(-2.0F, 0.0F); - r = this->make(-5.0F, 0.0F); - t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -2.0F); - ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 3.0F); - // singular case - p = this->make(1.0F, 5.0F); - q = this->make(1.0F, 5.0F); - r = this->make(1.0F, 1.0F); - t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), 1.0F); - ASSERT_FLOAT_EQ(y_(t), 5.0F); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 4.0F); -} - -TYPED_TEST(BoxTest, ClosestPointOnLine) -{ - using autoware::common::geometry::closest_line_point_2d; - // normal case - TypeParam p = this->make(-1.0F, 0.0F); - TypeParam q = this->make(-1.0F, 2.0F); - TypeParam r = this->make(1.0F, 1.0F); - TypeParam t = closest_line_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -1.0F); - ASSERT_FLOAT_EQ(y_(t), 1.0F); - // out-of-boundary case - p = this->make(1.0F, 0.0F); - q = this->make(-2.0F, 0.0F); - r = this->make(-5.0F, 0.0F); - t = closest_line_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -5.0F); - ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); - // singular case - p = this->make(1.0F, 5.0F); - q = this->make(1.0F, 5.0F); - r = this->make(1.0F, 1.0F); - EXPECT_THROW(t = closest_line_point_2d(p, q, r), std::runtime_error); -} - -TYPED_TEST(BoxTest, Basic) -{ - const std::vector data{ - {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)}}; - this->points.insert(this->points.begin(), data.begin(), data.end()); - - this->minimum_area_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); - this->test_corners(data); - this->test_orientation(0.0F); - - this->minimum_perimeter_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 2.0F); - this->test_corners(data); - this->test_orientation(0.0F); -} - -// -TYPED_TEST(BoxTest, OrientedTriangle) -{ - this->points.insert(this->points.begin(), {this->make(5, 5), this->make(7, 7), this->make(5, 7)}); - - this->minimum_area_bounding_box(); - - this->check(5.5F, 6.5F, sqrtf(2.0F), 2.0F * sqrtf(2.0F), 4.0F); - this->test_corners({this->make(5, 5), this->make(7, 7), this->make(4, 6), this->make(6, 8)}); - this->test_orientation(45.0F); - - this->minimum_perimeter_bounding_box(); - - this->check(6.0F, 6.0F, 2.0F, 2.0F, 4.0F); - this->test_corners({this->make(5, 5), this->make(7, 7), this->make(5, 7), this->make(7, 5)}); - this->test_orientation(0.0F); -} -// -TYPED_TEST(BoxTest, Hull) -{ - const uint32_t FUZZ_SIZE = 1024U; - const float dx = 9.0F; - const float dy = 15.0F; - const float rx = 10.0F; - const float ry = 5.0F; - const float dth = 0.0F; - - ASSERT_EQ(FUZZ_SIZE % 4U, 0U); - - // fuzz part 1 - for (uint32_t idx = 0U; idx < FUZZ_SIZE; ++idx) { - const float th = ((idx * autoware::common::types::TAU) / FUZZ_SIZE) + dth; - this->points.push_back(this->make(rx * cosf(th) + dx, ry * sinf(th) + dy)); - } - - this->minimum_area_bounding_box(); - - // allow 10cm = 2% of size for tolerance - const float TOL_M = 0.1F; - ASSERT_LT(fabsf(this->box.centroid.x - dx), TOL_M); - ASSERT_LT(fabsf(this->box.centroid.y - dy), TOL_M); - - this->test_corners( - {this->make(dx + rx, dy + ry), this->make(dx - rx, dy + ry), this->make(dx - rx, dy - ry), - this->make(dx + rx, dy - ry)}, - TOL_M); - - this->test_orientation(this->rad2deg(dth), 1.0F); - // allow 1 degree of tolerance - - ASSERT_LT(fabsf(this->box.size.y - 2.0F * rx), TOL_M); - ASSERT_LT(fabsf(this->box.size.x - 2.0F * ry), TOL_M); - ASSERT_FLOAT_EQ(this->box.value, this->box.size.x * this->box.size.y); -} - -// -TYPED_TEST(BoxTest, Collinear) -{ - this->points.insert( - this->points.begin(), {this->make(-2, -2), this->make(-3, -2), this->make(-4, -2), - this->make(-2, -4), this->make(-3, -4), this->make(-4, -4), - this->make(-2, -3), this->make(-2, -3), this->make(-2, -4)}); - - this->minimum_area_bounding_box(); - - this->check(-3.0F, -3.0F, 2.0F, 2.0F, 4.0F); - this->test_corners( - {this->make(-2, -2), this->make(-2, -4), this->make(-4, -4), this->make(-4, -2)}); - this->test_orientation(0.0F); -} - -// -TYPED_TEST(BoxTest, Line1) -{ - this->points.insert( - this->points.begin(), {this->make(-4, 3), this->make(-8, 6), this->make(-12, 9), - this->make(-16, 12), this->make(-20, 15), this->make(-24, 18), - this->make(-28, 21), this->make(-32, 24), this->make(-36, 27)}); - - this->minimum_area_bounding_box(); - - this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); - this->test_orientation(this->rad2deg(atan2f(3, -4))); - this->test_corners( - {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)}); - - this->minimum_perimeter_bounding_box(); - - this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 40.00001F); - this->test_orientation(this->rad2deg(atan2f(3, -4))); - this->test_corners( - {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)}); -} - -// -TYPED_TEST(BoxTest, Line2) -{ - this->points.insert( - this->points.begin(), - {this->make(4, 0), this->make(8, 0), this->make(12, 0), this->make(16, 0), this->make(20, 0), - this->make(24, 0), this->make(28, 0), this->make(32, 0), this->make(36, 0)}); - - this->minimum_area_bounding_box(); - - this->check(20.0F, 0.0F, 1.0E-6F, 32.0F, 3.2E-5F); - this->test_orientation(0.0F); - this->test_corners({this->make(4, 0), this->make(36, 0), this->make(4, 0), this->make(36, 0)}); -} - -// -TYPED_TEST(BoxTest, Line3) -{ - this->points.insert( - this->points.begin(), - {this->make(4, 3), this->make(8, 6), this->make(12, 9), this->make(16, 12), this->make(20, 15), - this->make(24, 18), this->make(28, 21), this->make(32, 24), this->make(36, 27)}); - - this->minimum_area_bounding_box(); - - this->check(20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); - this->test_orientation(this->rad2deg(atan2f(3, 4))); - this->test_corners({this->make(4, 3), this->make(36, 27), this->make(4, 3), this->make(36, 27)}); -} - -/* _ - /_/ <-- first guess is suboptimal - -*/ -TYPED_TEST(BoxTest, SuboptimalInit) -{ - this->points.insert( - this->points.begin(), - {this->make(8, 15), this->make(17, 0), this->make(0, 0), this->make(25, 15)}); - - this->minimum_area_bounding_box(); - - this->check(12.5F, 7.5F, 15.0F, 25.0F, 375.0F); - this->test_orientation(this->rad2deg(atan2f(15, 8))); - // these are approximate. - this->test_corners( - {this->make(0, 0), this->make(25, 15), this->make(11.7647F, 22.0588F), - this->make(13.2353F, -7.05882F)}, - 0.1F); -} - -// -TYPED_TEST(BoxTest, Centered) -{ - this->points.insert( - this->points.begin(), - {this->make(-1, 0), this->make(1, 0), this->make(0, -1), this->make(0, 1)}); - - this->minimum_area_bounding_box(); - - this->check(0.0F, 0.0F, sqrtf(2.0F), sqrtf(2.0F), 2.0F); - this->test_orientation(45.0F); - this->test_corners({this->make(-1, 0), this->make(1, 0), this->make(0, 1), this->make(0, -1)}); -} - -// convex_hull is imperfect in this case, check if this can save the day -TYPED_TEST(BoxTest, OverlappingPoints) -{ - this->points.insert( - this->points.begin(), {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1), - this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)}); - - this->minimum_area_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); - this->test_orientation(0.0F); - this->test_corners({this->make(0, 0), this->make(1, 0), this->make(0, 1), this->make(1, 1)}); -} - -// check that minimum perimeter box is different from minimum area box -TYPED_TEST(BoxTest, Perimeter) -{ - this->points.insert( - this->points.begin(), {this->make(0, 0), this->make(0, 1), this->make(0, 2), this->make(0, 3), - this->make(0, 4), this->make(1, -0.1), // small fudge to force diagonal - this->make(2, 0), this->make(3, 0)}); - - this->minimum_area_bounding_box(); - - this->check(0.54F, 1.28F, 5.0F, 12.0F / 5.0F, 12.0F); - this->test_orientation(-53.13F, 0.001F); - this->test_corners( - {this->make(3, 0), this->make(0, 4), this->make(-1.92, 2.56), this->make(1.08, -1.44)}); - - // eigenbox should produce AABB TODO(c.ho) - // compute minimum perimeter box - this->minimum_perimeter_bounding_box(); - this->check(1.5F, 1.95F, 4.1F, 3.0F, 7.1F); - // perimeter for first box would be 14.8 - this->test_orientation(0.0F, 0.001F); - this->test_corners( - {this->make(3, -0.1), this->make(0, 4), this->make(3, 4), this->make(0, -0.1)}); -} - -// bounding box is diagonal on an L -TYPED_TEST(BoxTest, Eigenbox1) -{ - std::vector v{this->make(0, 0), this->make(0, 1), - this->make(-1, 2), // small fudge to force diagonal - this->make(0, 3), this->make(0, 4), - this->make(1, 0), this->make(2, -1), // small fudge to force diagonal - this->make(3, 0), this->make(4, 0)}; - this->points.insert(this->points.begin(), v.begin(), v.end()); - - // rotating calipers should produce a diagonal box - this->minimum_area_bounding_box(); - const float r = 4.0F; - - this->check(1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * r); - const std::vector diag_corners{ - this->make(4, 0), this->make(0, 4), this->make(-2, 2), this->make(2, -2)}; - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - - //// perimeter should also produce diagonal //// - this->minimum_perimeter_bounding_box(); - this->check( - 1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * (sqrtf(2.0F) + (1.0F / sqrtf(2.0F)))); - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - //// eigenbox should also produce aabb //// - this->eigenbox(v.begin(), v.end()); - // TODO(c.ho) don't care about value.. - this->check(1.0F, 1.0F, r * sqrtf(2.0F), r / sqrtf(2.0F), 4.375F); - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - //// Lfit should produce aabb //// - this->lfit_bounding_box_2d(v.begin(), v.end()); - - this->test_corners( - {this->make(4, -1), this->make(-1, 4), this->make(4, 4), this->make(-1, -1)}, 0.25F); - this->test_orientation(0.0F, 3.0F); -} - -// same as above test, just rotated -TYPED_TEST(BoxTest, Eigenbox2) -{ - std::vector v{this->make(0, 0), this->make(1, 1), - this->make(1, 2), // small fudge to force diagonal - this->make(3, 3), this->make(4, 4), - this->make(1, -1), this->make(1, -2), // small fudge to force diagonal - this->make(3, -3), this->make(4, -4)}; - this->points.insert(this->points.begin(), v.begin(), v.end()); - - const std::vector diag_corners{ - this->make(4, 4), this->make(0, 4), this->make(0, -4), this->make(4, -4)}; - // rotating calipers should produce a aabb - this->minimum_area_bounding_box(); - this->check(2.0F, 0.0F, 8, 4, 32); - this->test_corners(diag_corners); - this->test_orientation(0.0F, 0.001F); - - //// perimeter should also produce aabb //// - this->minimum_perimeter_bounding_box(); - this->check(2.0F, 0.0F, 8, 4, 12); - this->test_corners(diag_corners); - - //// eigenbox should also produce obb //// - this->eigenbox(v.begin(), v.end()); - this->test_orientation(0.0F, 0.001F); - this->test_corners(diag_corners); - //// Lfit should produce obb //// - this->lfit_bounding_box_2d(v.begin(), v.end()); - this->test_corners( - {this->make(3.5, 4.5), this->make(-1, 0), this->make(3.5, -4.5), this->make(8, 0)}, 0.2F); - this->test_orientation(45.0F, 2.0F); -} -// line case for eigenbox -TYPED_TEST(BoxTest, Eigenbox3) -{ - // horizontal line with some noise - std::vector v{this->make(0, 0.00), this->make(1, -0.01), this->make(3, 0.02), - this->make(3, 0.03), this->make(4, -0.04), this->make(-1, 0.01), - this->make(-2, -0.02), this->make(-3, -0.03), this->make(-4, 0.04)}; - this->lfit_bounding_box_2d(v.begin(), v.end()); - this->test_corners( - {this->make(-4, -0.04), this->make(-4, 0.04), this->make(4, -0.04), this->make(4, 0.04)}, 0.2F); - this->test_orientation(0.0F, 1.0F); -} - -// bad case: causes intersection2d to fail -// See https://gitlab.apex.ai/ApexAI/grand_central/issues/2862#note_156875 for more failure cases -TYPED_TEST(BoxTest, IntersectFail) -{ - std::vector vals{ - this->make(-13.9, 0.100006), this->make(-14.1, 0.100006), this->make(-13.9, 0.300003), - this->make(-14.1, 0.300003), this->make(-13.9, 0.5), this->make(-14.1, 0.5), - this->make(-13.9, 0.699997), this->make(-14.1, 0.699997), this->make(-14.3, 0.699997)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); - vals = {this->make(-2.1, 10.1), this->make(-1.89999, 10.1), this->make(-1.89999, 10.3), - this->make(-1.7, 10.3), this->make(-1.5, 10.3), this->make(-1.3, 10.3), - this->make(-0.5, 10.3), this->make(-0.300003, 10.3), this->make(-0.0999908, 10.3), - this->make(0.699997, 10.3), this->make(0.900009, 10.3), this->make(1.3, 10.3), - this->make(1.7, 10.3)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); - vals = {this->make(2.7, -5.5), this->make(2.7, -5.3), this->make(2.7, -5.1), - this->make(2.7, -4.9), this->make(2.7, -4.7), this->make(2.7, -4.5), - this->make(2.7, -4.3), this->make(2.7, -4.1), this->make(2.7, -3.9)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); -} -/// Handle slight floating point underflow case -// Note: raw discriminant checks are disabled because they don't work. I suspect this is due to -// tiny differences in floating point math when using our compile flags -TYPED_TEST(BoxTest, EigUnderflow) -{ - using autoware::common::geometry::bounding_box::details::Covariance2d; - // auto discriminant = [](const Covariance2d cov) -> float32_t { - // // duplicated raw math - // const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; - // const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); - // return (tr_2 * tr_2) - det; - // }; - TypeParam u, v; - const Covariance2d c1{0.0300002, 0.0300002, 5.46677e-08, 0U}; - // EXPECT_LT(discriminant(c1), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c1, u, v)); - const Covariance2d c2{0.034286, 0.0342847, 2.12874e-09, 0U}; - // EXPECT_LT(discriminant(c2), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c2, u, v)); - const Covariance2d c3{0.0300002, 0.0300002, -2.84987e-08, 0U}; - // EXPECT_LT(discriminant(c3), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c3, u, v)); - const Covariance2d c4{0.0300004, 0.0300002, 3.84156e-08, 0U}; - // EXPECT_LT(discriminant(c4), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c4, u, v)); - const Covariance2d c5{0.0300014, 0.0300014, -7.45058e-09, 0U}; - // EXPECT_LT(discriminant(c5), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c5, u, v)); - const Covariance2d c6{0.0400004, 0.0400002, 3.28503e-08, 0U}; - // EXPECT_LT(discriminant(c6), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c6, u, v)); -} - -//////////////////////////////////////////////// - -#endif // TEST_BOUNDING_BOX_HPP_ diff --git a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp deleted file mode 100644 index fc2d97c257b95..0000000000000 --- a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp +++ /dev/null @@ -1,260 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef TEST_SPATIAL_HASH_HPP_ -#define TEST_SPATIAL_HASH_HPP_ - -#include "autoware_auto_geometry/spatial_hash.hpp" - -#include - -#include -#include - -using autoware::common::geometry::spatial_hash::Config2d; -using autoware::common::geometry::spatial_hash::Config3d; -using autoware::common::geometry::spatial_hash::SpatialHash; -using autoware::common::geometry::spatial_hash::SpatialHash2d; -using autoware::common::geometry::spatial_hash::SpatialHash3d; -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedSpatialHashTest : public ::testing::Test -{ -public: - TypedSpatialHashTest() : ref(), EPS(0.001F) - { - ref.x = 0.0F; - ref.y = 0.0F; - ref.z = 0.0F; - } - -protected: - template - void add_points( - SpatialHash & hash, const uint32_t points_per_ring, const uint32_t num_rings, - const float32_t dr, const float32_t dx = 0.0F, const float32_t dy = 0.0F) - { - const float32_t dth = 2.0F * 3.14159F / points_per_ring; - - // insert - float32_t r = dr; - for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { - float32_t th = 0.0F; - for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { - PointT pt; - pt.x = r * cosf(th) + dx; - pt.y = r * sinf(th) + dy; - pt.z = 0.0F; - hash.insert(pt); - th += dth; - } - r += dr; - } - } - PointT ref; - const float32_t EPS; -}; // SpatialHash -// test struct - -// Instantiate tests for given types, add more types here as they are used -using PointTypesSpatialHash = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedSpatialHashTest, PointTypesSpatialHash, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -/////////////////////////////////////////////////////////////// -// TODO(christopher.ho) helper functions to simplify this stuff -/// all points in one bin -TYPED_TEST(TypedSpatialHashTest, OneBin) -{ - using PointT = TypeParam; - const float32_t dr = 1.0F; - Config2d cfg{-10.0F, 10.0F, -10.0F, 10.0F, 3.0F, 1024U}; - SpatialHash2d hash{cfg}; - - // build concentric rings around origin - const uint32_t PTS_PER_RING = 10U; - const uint32_t NUM_RINGS = 1U; - this->add_points(hash, PTS_PER_RING, NUM_RINGS, dr); - - // loop through all points - float r = dr - this->EPS; - for (uint32_t rdx = 0U; rdx < NUM_RINGS + 1U; ++rdx) { - const uint32_t n_pts = rdx * PTS_PER_RING; - const auto & neighbors = hash.near(this->ref, r); - uint32_t points_seen = 0U; - for (const auto & itd : neighbors) { - const PointT & pt = itd; - const float dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); - ASSERT_LT(dist, r); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - ASSERT_EQ(points_seen, n_pts); - r += dr; - // Make sure statistics are consistent - EXPECT_EQ(hash.bins_hit(), 9U * (1U + rdx)); - EXPECT_EQ(hash.neighbors_found(), rdx * PTS_PER_RING); - } - // check iterators etc. - uint32_t count = 0U; - for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { - // TODO(c.ho) check uniqueness of stuff - ++count; - } - EXPECT_EQ(PTS_PER_RING, count); - hash.clear(); - EXPECT_EQ(hash.size(), 0U); - EXPECT_TRUE(hash.empty()); - count = 0U; - for (auto it : hash) { - // TODO(c.ho) check uniqueness of stuff - (void)it; - ++count; - } - EXPECT_EQ(count, 0U); -} -/// test out of bounds points -TYPED_TEST(TypedSpatialHashTest, Oob) -{ - using PointT = TypeParam; - const float32_t dr = 20.0F; - Config2d cfg{-2.0F, 2.0F, -2.0F, 2.0F, dr + this->EPS, 1024U}; - SpatialHash2d hash{cfg}; - - // build concentric rings around origin - const uint32_t PTS_PER_RING = 12U; - this->add_points(hash, PTS_PER_RING, 1U, dr); - - // loop through all points - float32_t r = dr + this->EPS; - const uint32_t n_pts = PTS_PER_RING; - const auto & neighbors = hash.near(this->ref, r); - uint32_t points_seen = 0U; - for (const auto itd : neighbors) { - const PointT & pt = itd; - const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); - ASSERT_LT(dist, r); - ASSERT_GT(dist, 10.0F * sqrtf(2.0F)); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - - ASSERT_EQ(points_seen, n_pts); -} -// 3d test case -TYPED_TEST(TypedSpatialHashTest, 3d) -{ - using PointT = TypeParam; - Config3d cfg{-30.0F, 30.0F, -30.0F, 30.0F, -30.0F, 30.0F, 30.0F, 1024U}; - SpatialHash3d hash{cfg}; - EXPECT_TRUE(hash.empty()); - - // build concentric rings around origin - const uint32_t points_per_ring = 32U; - const uint32_t num_rings = 5U; - const float32_t dth = 2.0F * 3.14159F / points_per_ring; - std::vector pts{}; - - // insert - const float32_t r = 10.0F; - float32_t phi = 0.0f; - for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { - float32_t th = 0.0F; - for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { - PointT pt; - pt.x = r * cosf(th) * cosf(phi); - pt.y = r * sinf(th) * cosf(phi); - pt.z = r * sinf(phi); - pts.push_back(pt); - th += dth; - } - hash.insert(pts.begin(), pts.end()); - pts.clear(); - phi += 1.0f; - } - EXPECT_FALSE(hash.empty()); - EXPECT_EQ(hash.size(), num_rings * points_per_ring); - - // loop through all points - const uint32_t n_pts = num_rings * points_per_ring; - const auto & neighbors = hash.near(this->ref, r + this->EPS); - uint32_t points_seen = 0U; - for (const auto & itd : neighbors) { - const PointT & pt = itd; - const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y) + (pt.z * pt.z)); - ASSERT_LT(dist, r + this->EPS); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - - ASSERT_EQ(points_seen, n_pts); - - // check iterators etc. - uint32_t count = 0U; - for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { - // TODO(c.ho) check uniqueness of stuff - ++count; - } - EXPECT_EQ(n_pts, count); - hash.clear(); - EXPECT_EQ(hash.size(), 0U); - EXPECT_TRUE(hash.empty()); - count = 0U; - for (auto it : hash) { - // TODO(c.ho) check uniqueness of stuff - (void)it; - ++count; - } - EXPECT_EQ(count, 0U); -} - -/// edge cases -TEST(SpatialHashConfig, BadCases) -{ - // negative side length - EXPECT_THROW(Config2d({-30.0F, 30.0F, -30.0F, 30.0F, -1.0F, 1024U}), std::domain_error); - // min_x >= max_x - EXPECT_THROW(Config2d({31.0F, 30.0F, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - // min_y >= max_y - EXPECT_THROW(Config2d({-30.0F, 30.0F, 30.1F, 30.0F, 1.0F, 1024U}), std::domain_error); - // min_z >= max_z - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -30.0F, 30.0F, 31.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - // floating point limit - constexpr float32_t max_float = std::numeric_limits::max(); - EXPECT_THROW(Config2d({-max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), - std::domain_error); - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -30.0F, 30.0F, -max_float, max_float, 1.0F, 1024U}), - std::domain_error); - // y would overflow - // constexpr float32_t big_float = - // static_cast(std::numeric_limits::max() / 4UL); - // EXPECT_THROW(Config({-big_float, big_float, -big_float, big_float, 0.001F, 1024U}), - // std::domain_error); - // z would overflow - // EXPECT_THROW( - // Config3d({-30.0F, 30.0F, -99999.0F, 99999.0F, -99999.0F, 99999.0F, 0.001F, 1024U}), - // std::domain_error); - // TODO(c.ho) re-enable test when we can actually check unsigned integer multiplication overflow -} -#endif // TEST_SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp deleted file mode 100644 index 81865656c55b5..0000000000000 --- a/common/autoware_auto_geometry/test/src/lookup_table.cpp +++ /dev/null @@ -1,166 +0,0 @@ -// Copyright 2017-2020 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/lookup_table.hpp" - -#include - -#include - -#include -#include - -using autoware::common::helper_functions::interpolate; -using autoware::common::helper_functions::lookup_1d; -using autoware::common::helper_functions::LookupTable1D; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class BadCase : public ::testing::Test -{ -}; - -using BadTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(BadCase, BadTypes, ); - -// Empty domain/range -TYPED_TEST(BadCase, Empty) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({}, {}, T{}), std::domain_error); -} - -// Unequal domain/range -TYPED_TEST(BadCase, UnequalDomain) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({T{1}, T{2}}, {T{1}}, T{}), std::domain_error); -} - -// Not sorted domain -TYPED_TEST(BadCase, DomainNotSorted) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({T{2}, T{1}}, {T{1}, T{2}}, T{}), std::domain_error); -} - -//////////////////////////////////////////////////////////////////////////////// - -template -class SanityCheck : public ::testing::Test -{ -public: - using T = Type; - - void SetUp() override - { - domain_ = std::vector{T{1}, T{3}, T{5}}; - range_ = std::vector{T{2}, T{4}, T{0}}; - ASSERT_NO_THROW(table_ = std::make_unique>(domain_, range_)); - } - - bool not_in_domain(const T bad_value) const noexcept - { - return std::find(domain_.begin(), domain_.end(), bad_value) == domain_.end(); - } - - void check(const T expected, const T actual) const noexcept - { - if (std::is_floating_point::value) { - EXPECT_FLOAT_EQ(actual, expected); - } else { - EXPECT_EQ(actual, expected); - } - } - -protected: - std::vector domain_{}; - std::vector range_{}; - std::unique_ptr> table_{}; -}; - -using NormalTypes = ::testing::Types; -TYPED_TEST_SUITE(SanityCheck, NormalTypes, ); - -TYPED_TEST(SanityCheck, Exact) -{ - const auto x = this->domain_[1U]; - const auto result = this->table_->lookup(x); - ASSERT_FALSE(this->not_in_domain(x)); - this->check(result, this->range_[1U]); -} - -TYPED_TEST(SanityCheck, Interpolation) -{ - const auto x = TypeParam{2}; - // Assert x is not identically in domain_ - ASSERT_TRUE(this->not_in_domain(x)); - const auto result = this->table_->lookup(x); - this->check(result, TypeParam{3}); -} - -TYPED_TEST(SanityCheck, AboveRange) -{ - const auto x = TypeParam{999999}; - ASSERT_GT(x, this->domain_.back()); // domain is known to be sorted - const auto result = this->table_->lookup(x); - this->check(result, this->range_.back()); -} - -TYPED_TEST(SanityCheck, BelowRange) -{ - const auto x = TypeParam{0}; - ASSERT_LT(x, this->domain_.front()); // domain is known to be sorted - const auto result = this->table_->lookup(x); - this->check(result, this->range_.front()); -} - -TEST(LookupTableHelpers, Interpolate) -{ - { - const auto scaling = 0.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); - } - - { - const auto scaling = 1.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); - } - - { - const auto scaling = -1.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); - } - - { - const auto scaling = 2.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); - } - - { - const auto scaling = 0.75f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.75f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.125f); - } -} - -// TODO(c.ho) check with more interesting functions diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp deleted file mode 100644 index bc9c28682ed44..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_area.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// Copyright 2021 Apex.AI, 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -#include - -#include -#include -#include - -template -class AreaTest : public ::testing::Test -{ -protected: - DataStructure data_{}; - using Point = typename DataStructure::value_type; - using Real = decltype(autoware::common::geometry::point_adapter::x_(std::declval())); - - auto area() { return autoware::common::geometry::area_checked_2d(data_.begin(), data_.end()); } - - void add_point(Real x, Real y) - { - namespace pa = autoware::common::geometry::point_adapter; - Point p{}; - pa::xr_(p) = x; - pa::yr_(p) = y; - (void)data_.insert(data_.end(), p); - } -}; - -// Data structures to test... -template -using TestTypes_ = ::testing::Types..., std::list...>; -// ... and point types to test -using TestTypes = TestTypes_; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(AreaTest, TestTypes, ); - -// The empty set has zero area -TYPED_TEST(AreaTest, DegenerateZero) -{ - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// An individual point has zero area -TYPED_TEST(AreaTest, DegenerateOne) -{ - this->add_point(0.0, 0.0); - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// An line segment has zero area -TYPED_TEST(AreaTest, DegenerateTwo) -{ - this->add_point(1.0, -1.0); - this->add_point(-3.0, 2.0); - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// Simple triangle -TYPED_TEST(AreaTest, Triangle) -{ - this->add_point(1.0, 0.0); - this->add_point(3.0, 0.0); // 2.0 wide - this->add_point(2.0, 2.0); // 2.0 tall - EXPECT_FLOAT_EQ(2.0, this->area()); // A = (1/2) * b * h -} - -// Rectangle is easy to do computational -TYPED_TEST(AreaTest, Rectangle) -{ - this->add_point(-5.0, -5.0); - this->add_point(-2.0, -5.0); // L = 3 - this->add_point(-2.0, -1.0); // H = 4 - this->add_point(-5.0, -1.0); - EXPECT_FLOAT_EQ(12.0, this->area()); // A = b * h -} - -// Parallelogram is slightly less trivial than a rectangle -TYPED_TEST(AreaTest, Parallelogram) -{ - this->add_point(-5.0, 1.0); - this->add_point(-2.0, 1.0); // L = 3 - this->add_point(-1.0, 3.0); // H = 2 - this->add_point(-4.0, 3.0); - EXPECT_FLOAT_EQ(6.0, this->area()); // A = b * h -} - -// Octagon is analytical and reasonably easy to build -TYPED_TEST(AreaTest, Octagon) -{ - const auto sq2 = std::sqrt(2.0); - const auto a = 1.0; - const auto a2 = a / 2.0; - const auto b = (a + sq2) / 2.0; - this->add_point(-a2, -b); - this->add_point(a2, -b); - this->add_point(b, -a2); - this->add_point(b, a2); - this->add_point(a2, b); - this->add_point(-a2, b); - this->add_point(-b, a2); - this->add_point(-b, -a2); - const auto expect = (2.0 * (1.0 + sq2)) * (a * a); - EXPECT_FLOAT_EQ(expect, this->area()); // A = b * h -} - -// Bad case -TYPED_TEST(AreaTest, NotCcw) -{ - this->add_point(0.0, 0.0); - this->add_point(1.0, 1.0); - this->add_point(1.0, 0.0); - this->add_point(2.0, 1.0); - EXPECT_THROW(this->area(), std::domain_error); -} diff --git a/common/autoware_auto_geometry/test/src/test_common_2d.cpp b/common/autoware_auto_geometry/test/src/test_common_2d.cpp deleted file mode 100644 index baf6967edd47e..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_common_2d.cpp +++ /dev/null @@ -1,197 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/common_2d.hpp" - -#include -#include - -#include - -#include -#include - -using autoware::common::geometry::point_adapter::xr_; -using autoware::common::geometry::point_adapter::yr_; - -// Helper function for adding new points -template -T make_points(const float x, const float y) -{ - T ret; - xr_(ret) = x; - yr_(ret) = y; - return ret; -} - -// PointTypes to be tested -using PointTypes = - ::testing::Types; - -// Wrapper function for stubbing output of -// autoware::common::geometry::check_point_position_to_line_2d -template -int point_position_checker(const T & p1, const T & p2, const T & q) -{ - auto result = autoware::common::geometry::check_point_position_to_line_2d(p1, p2, q); - if (result > 0.0F) { - return 1; - } else if (result < 0.0F) { - return -1; - } - return result; -} - -// Templated struct defining the function parameters for -// autoware::common::geometry::check_point_position_to_line_2d and input_output vector for -// assertion of return values from the function -template -struct PointPositionToLine : public ::testing::Test -{ - struct Parameters - { - T p1; - T p2; - T q; - }; - static std::vector> input_output; -}; - -TYPED_TEST_SUITE_P(PointPositionToLine); - -template -std::vector::Parameters, int>> - PointPositionToLine::input_output{ - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(1.0F, 5.0F)}, -1}, - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-1.0F, 0.5F)}, 1}, - // Check point on the line - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-2.0F, 2.0F)}, 0}, - }; - -TYPED_TEST_P(PointPositionToLine, PointPositionToLineTest) -{ - for (size_t i = 0; i < PointPositionToLine::input_output.size(); ++i) { - const auto & input = PointPositionToLine::input_output[i].first; - EXPECT_EQ( - point_position_checker(input.p1, input.p2, input.q), - PointPositionToLine::input_output[i].second) - << "Index " << i; - } -} - -REGISTER_TYPED_TEST_SUITE_P(PointPositionToLine, PointPositionToLineTest); -// cppcheck-suppress syntaxError -INSTANTIATE_TYPED_TEST_SUITE_P(Test, PointPositionToLine, PointTypes, ); - -///////////////////////////////////////////////////////////////////////////////////// - -// Templated struct defining the function parameters for -// autoware::common::geometry::is_point_inside_polygon_2d and input_output vector for -// assertion of return values from the function -template -struct InsidePolygon : public ::testing::Test -{ - struct Parameters - { - std::vector polygon; - T q; - }; - static std::vector> input_output; -}; - -TYPED_TEST_SUITE_P(InsidePolygon); - -template -std::vector::Parameters, bool>> InsidePolygon::input_output{ - // point inside the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.F, 0.5F)}, - true}, - // point below the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 0.F)}, - false}, - // point above the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 1.75F)}, - false}, - // point on the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 0.5F)}, - true}, -}; - -TYPED_TEST_P(InsidePolygon, InsidePolygonTest) -{ - for (size_t i = 0; i < InsidePolygon::input_output.size(); ++i) { - const auto & input = InsidePolygon::input_output[i].first; - EXPECT_EQ( - autoware::common::geometry::is_point_inside_polygon_2d( - input.polygon.begin(), input.polygon.end(), input.q), - InsidePolygon::input_output[i].second) - << "Index " << i; - } -} - -REGISTER_TYPED_TEST_SUITE_P(InsidePolygon, InsidePolygonTest); -// cppcheck-suppress syntaxError -INSTANTIATE_TYPED_TEST_SUITE_P(Test, InsidePolygon, PointTypes, ); - -TEST(ordered_check, basic) -{ - // CW - std::vector points_list = { - make_points(8.0, 4.0), - make_points(9.0, 1.0), - make_points(3.0, 2.0), - make_points(2.0, 5.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // CCW - points_list = { - make_points(2.0, 5.0), - make_points(3.0, 2.0), - make_points(9.0, 1.0), - make_points(8.0, 4.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Unordered - points_list = { - make_points(2.0, 5.0), - make_points(3.0, 2.0), - make_points(8.0, 4.0), - make_points(9.0, 1.0)}; - EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Unordered - points_list = { - make_points(0.0, 0.0), - make_points(1.0, 1.0), - make_points(1.0, 0.0), - make_points(2.0, 1.0)}; - EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Collinearity - points_list = { - make_points(2.0, 2.0), - make_points(4.0, 4.0), - make_points(6.0, 6.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); -} diff --git a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp deleted file mode 100644 index 8b9bbce36c428..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp +++ /dev/null @@ -1,372 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" - -#include - -#include - -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedConvexHullTest : public ::testing::Test -{ -protected: - std::list list; - - typename std::list::const_iterator convex_hull() - { - const auto ret = autoware::common::geometry::convex_hull(list); - return ret; - } - - void check_hull( - const typename std::list::const_iterator last, const std::vector & expect, - const bool8_t strict = true) - { - uint32_t items = 0U; - for (auto & pt : expect) { - bool8_t found = false; - auto it = list.begin(); - while (it != last) { - constexpr float32_t TOL = 1.0E-6F; - if ( - fabsf(pt.x - it->x) <= TOL && fabsf(pt.y - it->y) <= TOL && - (fabsf(pt.z - it->z) <= TOL || !strict)) // TODO(@estive): z if only strict - { - found = true; - break; - } - ++it; - } - ASSERT_TRUE(found) << items; - ++items; - } - if (strict) { - ASSERT_EQ(items, expect.size()); - } - } - - PointT make(const float32_t x, const float32_t y, const float32_t z) - { - PointT ret; - ret.x = x; - ret.y = y; - ret.z = z; - return ret; - } -}; // class convex_hull_test - -// Instantiate tests for given types, add more types here as they are used -using PointTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedConvexHullTest, PointTypes, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -////////////////////////////////////////// - -/* - 3 - 2 -1 -*/ -TYPED_TEST(TypedConvexHullTest, Triangle) -{ - std::vector expect({this->make(1, 0, 0), this->make(3, 1, 0), this->make(2, 2, 0)}); - this->list.insert(this->list.begin(), expect.begin(), expect.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(this->list.size(), 3U); - // check order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, 3); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->x, 2); - ++it; // node 3 - ASSERT_EQ(it, last); -} -/* - 2 1 - -4 - 3 -*/ -// test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, Quadrilateral) -{ - std::vector expect( - {this->make(-1, -1, 1), this->make(-5, -1, 2), this->make(-2, -6, 3), this->make(-6, -5, 4)}); - this->list.insert(this->list.begin(), expect.begin(), expect.end()); - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(this->list.size(), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, -6); - ++it; // node 4 - ASSERT_FLOAT_EQ(it->x, -2); - ++it; // node 3 - ASSERT_FLOAT_EQ(it->x, -1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, -5); - ++it; // node 2 - ASSERT_EQ(it, last); -} - -// test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, QuadHull) -{ - std::vector data( - {this->make(1, 1, 1), this->make(5, 1, 2), this->make(2, 6, 3), this->make(3, 3, 4), - this->make(6, 5, 5)}); - std::vector expect{{data[0], data[1], data[2], data[4]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, 5); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->x, 6); - ++it; // node 4 - ASSERT_FLOAT_EQ(it->x, 2); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -// a ring plus a bunch of random stuff in the middle -TYPED_TEST(TypedConvexHullTest, Hull) -{ - const uint32_t HULL_SIZE = 13U; - const uint32_t FUZZ_SIZE = 50U; - const float32_t dth = 1.133729384F; // some weird irrational(ish) number - const float32_t r_hull = 20.0F; - const float32_t r_fuzz = 10.0F; - ASSERT_LT(r_fuzz, r_hull); - - std::vector hull; - - uint32_t hull_pts = 0U; - float32_t th = 0.0F; - // hull part 1 - for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++hull_pts; - } - - // fuzz part 1 - uint32_t fuzz_pts = 0U; - for (uint32_t idx = 0U; idx < FUZZ_SIZE / 2U; ++idx) { - const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++fuzz_pts; - } - - // hull part 2 - for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++hull_pts; - } - - // fuzz part 2 - for (uint32_t idx = 0U; idx < FUZZ_SIZE - fuzz_pts; ++idx) { - const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - } - - // hull part 3 - for (uint32_t idx = 0U; idx < HULL_SIZE - hull_pts; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - } - - const auto last = this->convex_hull(); - - this->check_hull(last, hull); - ASSERT_EQ(std::distance(this->list.cbegin(), last), HULL_SIZE); -} - -TYPED_TEST(TypedConvexHullTest, Collinear) -{ - std::vector data( - {this->make(0, 0, 1), this->make(1, 0, 2), this->make(2, 0, 3), this->make(0, 2, 4), - this->make(1, 2, 8), this->make(2, 2, 7), this->make(1, 0, 6), this->make(1, 2, 5), - this->make(1, 1, 0)}); - const std::vector expect{{data[0], data[2], data[3], data[5]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->z, 3); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->z, 7); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->z, 4); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -// degenerate cases -TYPED_TEST(TypedConvexHullTest, OverlappingPoints) -{ - std::vector data( - {this->make(3, -1, 1), this->make(4, -2, 2), this->make(5, -7, 3), this->make(4, -2, 4), - this->make(5, -7, 8), this->make(3, -1, 7), this->make(5, -7, 6), this->make(4, -2, 5), - this->make(3, -1, 0)}); - const std::vector expect{{data[0], data[1], data[2]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 3U); - this->check_hull(last, expect, false); -} - -TYPED_TEST(TypedConvexHullTest, Line) -{ - std::vector data( - {this->make(-3, 3, 1), this->make(-2, 2, 2), this->make(-1, 1, 3), this->make(-8, 8, 4), - this->make(-6, 6, 8), this->make(-4, 4, 7), this->make(-10, 10, 6), this->make(-12, 12, 5), - this->make(-11, 11, 0)}); - const std::vector expect{{data[2], data[7]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 2U); - this->check_hull(last, expect, false); - - // check for order: this part is a little loose - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 5); - ++it; // node 8 - ASSERT_FLOAT_EQ(it->z, 3); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -/* -1 - 4 - - 3 - 2 -*/ -TYPED_TEST(TypedConvexHullTest, LowerHull) -{ - const std::vector data({ - this->make(1, 3, 1), - this->make(2, -2, 2), - this->make(3, -1, 3), - this->make(4, 1, 4), - }); - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - this->check_hull(last, data); - - // check for order: this part is a little loose - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; - ASSERT_FLOAT_EQ(it->z, 2); - ++it; - ASSERT_FLOAT_EQ(it->z, 3); - ++it; - ASSERT_FLOAT_EQ(it->z, 4); - ++it; - ASSERT_EQ(it, last); -} - -// Ensure the leftmost item is properly shuffled -/* - 5 -1 6 - 2 4 - 3 -*/ -TYPED_TEST(TypedConvexHullTest, Root) -{ - const std::vector data({ - this->make(0, 0, 1), - this->make(1, -1, 2), - this->make(3, -2, 3), - this->make(4, 0, 4), - this->make(3, 1, 5), - this->make(1, 0, 6), - }); - const std::vector expect{{data[0], data[1], data[2], data[3], data[4]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 5); - this->check_hull(last, expect); - - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; - ASSERT_FLOAT_EQ(it->z, 2); - ++it; - ASSERT_FLOAT_EQ(it->z, 3); - ++it; - ASSERT_FLOAT_EQ(it->z, 4); - ++it; - ASSERT_FLOAT_EQ(it->z, 5); - ++it; - ASSERT_EQ(it, last); - EXPECT_NE(last, this->list.cend()); - EXPECT_EQ(last->z, 6); -} - -// TODO(c.ho) random input, fuzzing, stress tests diff --git a/common/autoware_auto_geometry/test/src/test_geometry.cpp b/common/autoware_auto_geometry/test/src/test_geometry.cpp deleted file mode 100644 index 0a7dd288d0d79..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_geometry.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2018 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "gtest/gtest.h" -#include "test_bounding_box.hpp" -#include "test_spatial_hash.hpp" - -int32_t main(int32_t argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp deleted file mode 100644 index 9477a83a488ed..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp +++ /dev/null @@ -1,182 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland. 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" -#include "autoware_auto_geometry/hull_pockets.hpp" - -#include - -#include - -#include -#include -#include -#include - -using autoware::common::geometry::point_adapter::x_; -using autoware::common::geometry::point_adapter::y_; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedHullPocketsTest : public ::testing::Test -{ -protected: - PointT make(const float32_t x, const float32_t y, const float32_t z) - { - PointT ret; - ret.x = x; - ret.y = y; - ret.z = z; - return ret; - } -}; // class TypedHullPocketsTest - -using PointTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedHullPocketsTest, PointTypes, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -// Inner test function, shared among all the tests -template -typename std::vector::value_type>> -compute_hull_and_pockets(const Iter polygon_start, const Iter polygon_end) -{ - auto convex_hull_list = - std::list::value_type>{polygon_start, polygon_end}; - const auto cvx_hull_end = autoware::common::geometry::convex_hull(convex_hull_list); - - const typename decltype(convex_hull_list)::const_iterator list_beginning = - convex_hull_list.begin(); - const auto pockets = autoware::common::geometry::hull_pockets( - polygon_start, polygon_end, list_beginning, cvx_hull_end); - - return pockets; -} - -// Test for a triangle - any triangle should really not result in any pockets. -TYPED_TEST(TypedHullPocketsTest, Triangle) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(1, 1, 0)}; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - ASSERT_EQ(pockets.size(), 0u); -} - -// Test for the use case: -// +--------------------+ -// | | -// | | -// | | -// | | -// | | -// | | -// +--------------------+ -// This should not result in any pockets. -TYPED_TEST(TypedHullPocketsTest, Box) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 1, 0), this->make(0, 1, 0)}; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - ASSERT_EQ(pockets.size(), 0u); -} - -// Test for the use case: -// +-----+ +-----+ -// / | | | -// / | | | -// + | | + -// | | | | -// | | | | -// | -------------- | -// | | -// | | -// | | -// | | -// +------------------------------+ -// This should come up with a single box on the top left. -TYPED_TEST(TypedHullPocketsTest, UShape) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(5, 0, 0), this->make(5, 4.5, 0), this->make(4, 5, 0), - this->make(4, 2, 0), this->make(2, 2, 0), this->make(2, 5, 0), this->make(0, 4.5, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 1u); - ASSERT_EQ(pockets[0].size(), 4u); - ASSERT_FLOAT_EQ(x_(pockets[0][0]), 4.0); - ASSERT_FLOAT_EQ(y_(pockets[0][0]), 5.0); - ASSERT_FLOAT_EQ(x_(pockets[0][1]), 4.0); - ASSERT_FLOAT_EQ(y_(pockets[0][1]), 2.0); -} - -// Test for the use case: -// +------+ -// | | -// | | -// | | -// +------------------+ +------+ -// | | -// | | -// | | -// +--------------------------------+ -// -// This should come up with two pockets, a triangle on the top left and one on the -// top right. -TYPED_TEST(TypedHullPocketsTest, TypicalGap) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(10, 0, 0), this->make(10, 2, 0), this->make(8, 2, 0), - this->make(8, 4, 0), this->make(6, 4, 0), this->make(6, 2, 0), this->make(0, 2, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 2u); - ASSERT_EQ(pockets[0].size(), 3u); - ASSERT_EQ(pockets[1].size(), 3u); - // TODO(s.me) check for correct pocket positioning -} - -// Test for the use case: -// -// +-----------------+ -// | | -// | | -// + | -// / | -// / | -// +-----------------+ -// -// This should come up with one pocket, in particular a pocket that contains -// the segment of the final to the first point. -TYPED_TEST(TypedHullPocketsTest, EndsInPocket) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 2, 0), - this->make(0, 2, 0), this->make(0.1, 1, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 1u); - ASSERT_EQ(pockets[0].size(), 3u); - // TODO(s.me) check for correct pocket positioning -} diff --git a/common/autoware_auto_geometry/test/src/test_intersection.cpp b/common/autoware_auto_geometry/test/src/test_intersection.cpp deleted file mode 100644 index 1036c69573c49..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_intersection.cpp +++ /dev/null @@ -1,155 +0,0 @@ -// Copyright 2021 Apex.AI, 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. - -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" -#include "autoware_auto_geometry/intersection.hpp" - -#include - -#include - -struct TestPoint -{ - autoware::common::types::float32_t x; - autoware::common::types::float32_t y; -}; - -struct IntersectionTestParams -{ - std::list polygon1_pts; - std::list polygon2_pts; - std::list expected_intersection_pts; -}; - -void order_ccw(std::list & points) -{ - const auto end_it = autoware::common::geometry::convex_hull(points); - ASSERT_EQ(end_it, points.end()); // Points should have represent a shape -} - -class IntersectionTest : public ::testing::TestWithParam -{ -}; - -TEST_P(IntersectionTest, Basic) -{ - const auto get_ordered_polygon = [](auto polygon) { - order_ccw(polygon); - return polygon; - }; - - const auto polygon1 = get_ordered_polygon(GetParam().polygon1_pts); - const auto polygon2 = get_ordered_polygon(GetParam().polygon2_pts); - const auto expected_intersection = get_ordered_polygon(GetParam().expected_intersection_pts); - - const auto result = autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); - - ASSERT_EQ(result.size(), expected_intersection.size()); - auto expected_shape_it = expected_intersection.begin(); - for (auto result_it = result.begin(); result_it != result.end(); ++result_it) { - EXPECT_FLOAT_EQ(result_it->x, expected_shape_it->x); - EXPECT_FLOAT_EQ(result_it->y, expected_shape_it->y); - ++expected_shape_it; - } -} - -INSTANTIATE_TEST_SUITE_P( - Basic, IntersectionTest, - ::testing::Values( - IntersectionTestParams{{}, {}, {}}, - IntersectionTestParams{// Partial intersection - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {15.0F, 5.0F}, {5.0F, 15.0F}, {15.0F, 15.0F}}, - {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}}, - // Full intersection with overlapping edges - // TODO(yunus.caliskan): enable after #1231 - // IntersectionTestParams{ - // {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, - // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, - // }, - IntersectionTestParams{ - // Fully contained - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, - }, - IntersectionTestParams{ - // Fully contained triangle - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, - }, - IntersectionTestParams{// Triangle rectangle intersection. - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 1.0F}, {5.0F, 9.0F}, {15.0F, 5.0F}}, - {{5.0F, 1.0F}, {5.0F, 9.0F}, {10.0F, 3.0F}, {10.0F, 7.0F}}}, - IntersectionTestParams{// No intersection - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{15.0F, 15.0F}, {20.0F, 15.0F}, {15.0F, 20.0F}, {20.0F, 20.0F}}, - {}} // cppcheck-suppress syntaxError - )); - -TEST(PolygonPointTest, Basic) -{ - GTEST_SKIP(); // TODO(yunus.caliskan): enable after #1231 - std::list polygon{{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}; - order_ccw(polygon); - EXPECT_FALSE(autoware::common::geometry::is_point_inside_polygon_2d( - polygon.begin(), polygon.end(), TestPoint{0.0F, 10.0F})); -} - -// IoU of two intersecting shapes: a pentagon and a square. The test includes pen and paper -// computations for the intermediate steps as assertions. -TEST(IoUTest, PentagonRectangleIntersection) -{ - std::list polygon1{ - {0.0F, 3.0F}, {3.0F, 4.0F}, {6.0F, 3.0F}, {4.0F, 1.0F}, {2.0F, 1.0F}}; - std::list polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}}; - - order_ccw(polygon1); - order_ccw(polygon2); - - const auto intersection = - autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); - const auto expected_intersection_area = - autoware::common::geometry::area_2d(intersection.begin(), intersection.end()); - ASSERT_FLOAT_EQ(expected_intersection_area, 1.5F); // Pen & paper proof. - - const auto expected_union_area = - autoware::common::geometry::area_2d(polygon1.begin(), polygon1.end()) + - autoware::common::geometry::area_2d(polygon2.begin(), polygon2.end()) - - expected_intersection_area; - ASSERT_FLOAT_EQ(expected_union_area, (11.0F + 4.0F - 1.5F)); // Pen & paper proof. - - const auto computed_iou = - autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2); - - EXPECT_FLOAT_EQ(computed_iou, expected_intersection_area / expected_union_area); -} - -// IoU of two non-intersecting rectangles. -TEST(IoUTest, NoIntersection) -{ - std::list polygon1{{0.0F, 0.0F}, {0.0F, 1.0F}, {1.0F, 1.0F}, {1.0F, 0.0F}}; - std::list polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}}; - - order_ccw(polygon1); - order_ccw(polygon2); - - EXPECT_FLOAT_EQ( - autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2), 0.0F); -} diff --git a/common/autoware_auto_geometry/test/src/test_interval.cpp b/common/autoware_auto_geometry/test/src/test_interval.cpp deleted file mode 100644 index 266ab123f5203..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_interval.cpp +++ /dev/null @@ -1,259 +0,0 @@ -// Copyright 2020 Mapless AI, Inc. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include -#include -#include - -using autoware::common::geometry::Interval; -using autoware::common::geometry::Interval_d; -using autoware::common::geometry::Interval_f; - -namespace -{ -const auto Inf = std::numeric_limits::infinity(); -const auto Min = std::numeric_limits::lowest(); -const auto Max = std::numeric_limits::max(); -const auto NaN = std::numeric_limits::quiet_NaN(); -const auto epsilon = 1e-5; -} // namespace - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, AbsEq) -{ - const auto i1 = Interval_d(-1.0, 1.0); - const auto i2 = Interval_d(-1.0 + 0.5 * epsilon, 1.0 + 0.5 * epsilon); - const auto shift = (2.0 * epsilon); - const auto i3 = Interval_d(-1.0 + shift, 1.0 + shift); - const auto i_empty = Interval_d(); - - EXPECT_TRUE(Interval_d::abs_eq(i1, i1, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i1, i2, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i2, i1, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i1, i3, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i3, i1, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i1, i_empty, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i_empty, i1, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i_empty, i_empty, epsilon)); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, IsSubsetEq) -{ - EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(-0.5, 0.5), Interval_d(-1.0, 1.0))); - EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(3.2, 4.2), Interval_d(3.2, 4.2))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(-3.0, -1.0), Interval_d(1.0, 3.0))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(1.0, 3.0), Interval_d(2.0, 4.0))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(), Interval_d())); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ClampTo) -{ - const auto i = Interval_d(-1.0, 1.0); - { - const auto val = 0.0; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, val); - } - - { - const auto val = -3.4; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, Interval_d::min(i)); - } - - { - const auto val = 2.7; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, Interval_d::max(i)); - } - - const auto val = 1.0; - const auto empty_interval = Interval_d(); - const auto projected = Interval_d::clamp_to(empty_interval, val); - EXPECT_TRUE(std::isnan(projected)); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Comparisons) -{ - { - const auto i1 = Interval_d(0.25, 1); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(-0.25, 0.5); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 0.5); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 1); - const auto i2 = Interval_d(0, 1); - EXPECT_TRUE((i1 == i2)); - EXPECT_FALSE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 1); - const auto i2 = Interval_d(); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(); - EXPECT_TRUE((i1 == i2)); - EXPECT_FALSE((i1 != i2)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Contains) -{ - { - const auto i = Interval_d(); - EXPECT_FALSE(Interval_d::contains(i, 0.0)); - } - - { - const auto i = Interval_d(-1.0, 1.0); - EXPECT_TRUE(Interval_d::contains(i, 0.0)); - EXPECT_FALSE(Interval_d::contains(i, 2.0)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Empty) -{ - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(); - const auto i3 = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i3)); - } - - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(0.0, 1.0); - const auto i3 = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i3)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ZeroMeasure) -{ - { - const auto i = Interval_d(0, 1); - EXPECT_FALSE(Interval_d::zero_measure(i)); - } - - { - const auto i = Interval_d(); - EXPECT_FALSE(Interval_d::zero_measure(i)); - } - - { - const auto i = Interval_d(2, 2); - EXPECT_TRUE(Interval_d::zero_measure(i)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, IntersectionMeasure) -{ - { - const auto i1 = Interval_d(-1.0, 1.0); - const auto i2 = Interval_d(-0.5, 1.5); - const auto i = Interval_d::intersect(i1, i2); - EXPECT_EQ(Interval_d::min(i), -0.5); - EXPECT_EQ(Interval_d::max(i), 1.0); - EXPECT_NEAR(Interval_d::measure(i), 1.5, epsilon); - } - - { - const auto i1 = Interval_d(-2.0, -1.0); - const auto i2 = Interval_d(1.0, 2.0); - const auto i = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i)); - EXPECT_TRUE(std::isnan(Interval_d::min(i))); - EXPECT_TRUE(std::isnan(Interval_d::max(i))); - EXPECT_TRUE(std::isnan(Interval_d::measure(i))); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ConstructionMeasure) -{ - { - const auto i = Interval_d(); - EXPECT_TRUE(std::isnan(Interval_d::min(i))); - EXPECT_TRUE(std::isnan(Interval_d::max(i))); - EXPECT_TRUE(std::isnan(Interval_d::measure(i))); - } - - { - const auto i = Interval_d(-1.0, 1.0); - EXPECT_EQ(Interval_d::min(i), -1.0); - EXPECT_EQ(Interval_d::max(i), 1.0); - EXPECT_NEAR(Interval_d::measure(i), 2.0, epsilon); - } - - { - const auto i = Interval_d(0.0, 0.0); - EXPECT_EQ(Interval_d::min(i), 0.0); - EXPECT_EQ(Interval_d::max(i), 0.0); - EXPECT_FALSE(Interval_d::empty(i)); - EXPECT_EQ(Interval_d::measure(i), 0.0); - } - - { - EXPECT_THROW({ Interval_d(1.0, -1.0); }, std::runtime_error); - } -} - -//------------------------------------------------------------------------------ diff --git a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt b/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt deleted file mode 100644 index 8d0469e78c3ac..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,62 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_perception_rviz_plugin) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Qt5 REQUIRED COMPONENTS Widgets) - -set(OD_PLUGIN_LIB_SRC - src/object_detection/detected_objects_display.cpp - src/object_detection/tracked_objects_display.cpp - src/object_detection/predicted_objects_display.cpp -) - -set(OD_PLUGIN_LIB_HEADERS - include/autoware_auto_perception_rviz_plugin/visibility_control.hpp -) -set(OD_PLUGIN_LIB_HEADERS_TO_WRAP - include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp -) - -set(COMMON_HEADERS - include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp -) - -set(COMMON_SRC - src/common/color_alpha_property.cpp - src/object_detection/object_polygon_detail.cpp -) - -foreach(header "${OD_PLUGIN_LIB_HEADERS_TO_WRAP}") - qt5_wrap_cpp(OD_PLUGIN_LIB_HEADERS_MOC "${header}") -endforeach() - -ament_auto_add_library(${PROJECT_NAME} SHARED - ${COMMON_HEADERS} - ${COMMON_SRC} - ${OD_PLUGIN_LIB_HEADERS} - ${OD_PLUGIN_LIB_HEADERS_MOC} - ${OD_PLUGIN_LIB_SRC} -) -target_link_libraries(${PROJECT_NAME} - rviz_common::rviz_common - Qt5::Widgets -) -target_include_directories(${PROJECT_NAME} PRIVATE include) - -# Settings to improve the build as suggested on https://github.com/ros2/rviz/blob/ros2/docs/plugin_development.md -target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -target_compile_definitions(${PROJECT_NAME} PRIVATE "OBJECT_DETECTION_PLUGINS_BUILDING_LIBRARY") - -# Export the plugin to be imported by rviz2 -pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - icons -) diff --git a/common/autoware_auto_perception_rviz_plugin/README.md b/common/autoware_auto_perception_rviz_plugin/README.md deleted file mode 100644 index cb5deb48f411c..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/README.md +++ /dev/null @@ -1,62 +0,0 @@ -# autoware_auto_perception_plugin - -## Purpose - -It is an rviz plugin for visualizing the result from perception module. This package is based on the implementation of the rviz plugin developed by Autoware.Auto. - -See Autoware.Auto design documentation for the original design philosophy. [[1]](https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/blob/master/src/tools/visualization/autoware_rviz_plugins) - - - -## Input Types / Visualization Results - -### DetectedObjects - -#### Input Types - -| Name | Type | Description | -| ---- | ----------------------------------------------------- | ---------------------- | -| | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array | - -#### Visualization Result - -![detected-object-visualization-description](./images/detected-object-visualization-description.jpg) - -### TrackedObjects - -#### Input Types - -| Name | Type | Description | -| ---- | ---------------------------------------------------- | --------------------- | -| | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array | - -#### Visualization Result - -Overwrite tracking results with detection results. - -![tracked-object-visualization-description](./images/tracked-object-visualization-description.jpg) - -### PredictedObjects - -#### Input Types - -| Name | Type | Description | -| ---- | ------------------------------------------------------ | ----------------------- | -| | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array | - -#### Visualization Result - -Overwrite prediction results with tracking results. - -![predicted-object-visualization-description](./images/predicted-object-visualization-description.jpg) - -## References/External links - -[1] - -## Future extensions / Unimplemented parts diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp deleted file mode 100644 index 97479fb68ca9b..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2021 Apex.AI, 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_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ - -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" - -#include - -namespace autoware -{ -namespace rviz_plugins -{ -namespace object_detection -{ -/// \brief Class defining rviz plugin to visualize DetectedObjects -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay -: public ObjectPolygonDisplayBase -{ - Q_OBJECT - -public: - using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; - - DetectedObjectsDisplay(); - -private: - void processMessage(DetectedObjects::ConstSharedPtr msg) override; -}; - -} // namespace object_detection -} // namespace rviz_plugins -} // namespace autoware - -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp deleted file mode 100644 index 4f545d194b2c2..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp +++ /dev/null @@ -1,300 +0,0 @@ -// Copyright 2021 Apex.AI, 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. -/// \brief This file defines some helper functions used by ObjectPolygonDisplayBase class -#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ - -#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace autoware -{ -namespace rviz_plugins -{ -namespace object_detection -{ -namespace detail -{ -// Struct to define all the configurable visual properties of an object of a particular -// classification type -struct ObjectPropertyValues -{ - // Classified type of the object - std::string label; - // Color for the type of the object - std::array color; - // Alpha values for the type of the object - float alpha{0.999F}; -}; - -// Control object marker visualization -enum class ObjectFillType { Skeleton, Fill }; - -// Map defining colors according to value of label field in ObjectClassification msg -const std::map< - autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> - // Color map is based on cityscapes color - kDefaultObjectPropertyValues = { - {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, - {"UNKNOWN", {255, 255, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, - {"PEDESTRIAN", {255, 192, 203}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - {"MOTORCYCLE", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER, - {"TRAILER", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; - -/// \brief Convert the given polygon into a marker representing the shape in 3d -/// \param shape_msg Shape msg to be converted. Corners should be in object-local frame -/// \param centroid Centroid position of the shape in Object.header.frame_id frame -/// \param orientation Orientation of the shape in Object.header.frame_id frame -/// \param color_rgba Color and alpha values to use for the marker -/// \param line_width Line thickness around the object -/// \return Marker ptr. Id and header will have to be set by the caller -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, - const bool & is_orientation_available = true, - const ObjectFillType fill_type = ObjectFillType::Skeleton); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, - const bool & is_orientation_available = true); - -/// \brief Convert the given polygon into a marker representing the shape in 3d -/// \param centroid Centroid position of the shape in Object.header.frame_id frame -/// \return Marker ptr. Id and header will have to be set by the caller -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_label_marker_ptr( - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std::string label, const std_msgs::msg::ColorRGBA & color_rgba); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_existence_probability_marker_ptr( - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_uuid_marker_ptr( - const std::string & uuid, const geometry_msgs::msg::Point & centroid, - const std_msgs::msg::ColorRGBA & color_rgba); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_pose_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const double & confidence_interval_coefficient); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_yaw_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, - const double & confidence_interval_coefficient, const double & line_width); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_velocity_text_marker_ptr( - const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, - const std_msgs::msg::ColorRGBA & color_rgba); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_acceleration_text_marker_ptr( - const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, - const std_msgs::msg::ColorRGBA & color_rgba); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_twist_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_twist_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, - const double & confidence_interval_coefficient); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_yaw_rate_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_yaw_rate_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, - const double & confidence_interval_coefficient, const double & line_width); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_predicted_path_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_path_confidence_marker_ptr( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const std_msgs::msg::ColorRGBA & path_confidence_color); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( - const double start_angle, const double end_angle, const double radius, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points( - const double point_list[][3], const int point_pairs[][2], const int & num_pairs, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( - const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( - const geometry_msgs::msg::Point center, const double radius, - std::vector & points, const int n); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, - std::vector & points); - -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( - const autoware_auto_perception_msgs::msg::PredictedPath & paths, - std::vector & points, const bool is_simple = false); - -/// \brief Convert Point32 to Point -/// \param val Point32 to be converted -/// \return Point type -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Point to_point( - const geometry_msgs::msg::Point32 & val) -{ - geometry_msgs::msg::Point ret; - ret.x = static_cast(val.x); - ret.y = static_cast(val.y); - ret.z = static_cast(val.z); - return ret; -} - -/// \brief Convert to Pose from Point and Quaternion -/// \param point -/// \param orientation -/// \return Pose type -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose to_pose( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Quaternion & orientation) -{ - geometry_msgs::msg::Pose ret; - ret.position = point; - ret.orientation = orientation; - return ret; -} - -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose initPose() -{ - geometry_msgs::msg::Pose pose; - pose.position.x = 0.0; - pose.position.y = 0.0; - pose.position.z = 0.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - return pose; -} - -/// \brief Get the best classification from the list of classifications based on max probability -/// \tparam ClassificationContainerT List type with ObjectClassificationMsg -/// \param labels List of ObjectClassificationMsg objects -/// \param logger_name Name to use for logger in case of a warning (if labels is empty) -/// \return Id of the best classification, Unknown if there is no best label -template -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC - autoware_auto_perception_msgs::msg::ObjectClassification::_label_type - get_best_label(ClassificationContainerT labels, const std::string & logger_name) -{ - const auto best_class_label = std::max_element( - labels.begin(), labels.end(), - [](const auto & a, const auto & b) -> bool { return a.probability < b.probability; }); - if (best_class_label == labels.end()) { - RCLCPP_WARN( - rclcpp::get_logger(logger_name), - "Empty classification field. " - "Treating as unknown"); - return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; - } - return best_class_label->label; -} - -} // namespace detail -} // namespace object_detection -} // namespace rviz_plugins -} // namespace autoware - -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp deleted file mode 100644 index 47cb8383fdada..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ - -#if defined(__WIN32) -#if defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || \ - defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllexport) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#else -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllimport) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#endif -#elif defined(__linux__) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) -#else // defined(_LINUX) -#error "Unsupported Build Configuration" -#endif // defined(_WINDOWS) - -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml deleted file mode 100644 index 2033239824d95..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - autoware_auto_perception_rviz_plugin - 1.0.0 - Contains plugins to visualize object detection outputs - Apex.AI, Inc. - Satoshi Tanaka - Taiki Tanaka - Takeshi Miura - Shunsuke Miura - Yoshi Ri - Apache 2.0 - - ament_cmake - autoware_cmake - - libboost-dev - qtbase5-dev - - autoware_auto_perception_msgs - rviz_common - rviz_default_plugins - - libqt5-widgets - rviz2 - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_auto_perception_rviz_plugin/plugins_description.xml b/common/autoware_auto_perception_rviz_plugin/plugins_description.xml deleted file mode 100644 index 3f56a43558494..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/plugins_description.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - Convert a PredictedObjects to markers and display them. - - autoware_auto_perception_msgs/msg/PredictedObjects - - - - - Convert a TrackedObjects to markers and display them. - - autoware_auto_perception_msgs/msg/TrackedObjects - - - - - Convert a DetectedObjects to markers and display them. - - autoware_auto_perception_msgs/msg/DetectedObjects - - - diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt deleted file mode 100755 index a8ae9ec2d962e..0000000000000 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_tf2) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_tf2_autoware_auto_msgs test/test_tf2_autoware_auto_msgs.cpp) - target_include_directories(test_tf2_autoware_auto_msgs PRIVATE "include") - ament_target_dependencies(test_tf2_autoware_auto_msgs - "autoware_auto_common" - "autoware_auto_perception_msgs" - "autoware_auto_system_msgs" - "autoware_auto_geometry_msgs" - "geometry_msgs" - "tf2" - "tf2_geometry_msgs" - "tf2_ros" - ) -endif() - -ament_auto_package() diff --git a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md b/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md deleted file mode 100644 index aaa719d617373..0000000000000 --- a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md +++ /dev/null @@ -1,183 +0,0 @@ -# autoware_auto_tf2 - -This is the design document for the `autoware_auto_tf2` package. - -## Purpose / Use cases - -In general, users of ROS rely on tf (and its successor, tf2) for publishing and utilizing coordinate -frame transforms. This is true even to the extent that the tf2 contains the packages -`tf2_geometry_msgs` and `tf2_sensor_msgs` which allow for easy conversion to and from the message -types defined in `geometry_msgs` and `sensor_msgs`, respectively. However, AutowareAuto contains -some specialized message types which are not transformable between frames using the ROS 2 library. -The `autoware_auto_tf2` package aims to provide developers with tools to transform applicable -`autoware_auto_msgs` types. In addition to this, this package also provides transform tools for -messages types in `geometry_msgs` missing in `tf2_geometry_msgs`. - -## Design - -While writing `tf2_some_msgs` or contributing to `tf2_geometry_msgs`, compatibility and design -intent was ensured with the following files in the existing tf2 framework: - -- `tf2/convert.h` -- `tf2_ros/buffer_interface.h` - -For example: - -```cpp -void tf2::convert( const A & a,B & b) -``` - -The method `tf2::convert` is dependent on the following: - -```cpp -template - B tf2::toMsg(const A& a); -template - void tf2::fromMsg(const A&, B& b); - -// New way to transform instead of using tf2::doTransform() directly -tf2_ros::BufferInterface::transform(...) -``` - -Which, in turn, is dependent on the following: - -```cpp -void tf2::convert( const A & a,B & b) -const std::string& tf2::getFrameId(const T& t) -const ros::Time& tf2::getTimestamp(const T& t); -``` - -## Current Implementation of tf2_geometry_msgs - -In both ROS 1 and ROS 2 stamped msgs like `Vector3Stamped`, `QuaternionStamped` have associated -functions like: - -- `getTimestamp` -- `getFrameId` -- `doTransform` -- `toMsg` -- `fromMsg` - -In ROS 1, to support `tf2::convert` and need in `doTransform` of the stamped data, non-stamped -underlying data like `Vector3`, `Point`, have implementations of the following functions: - -- `toMsg` -- `fromMsg` - -In ROS 2, much of the `doTransform` method is not using `toMsg` and `fromMsg` as data types from tf2 -are not used. Instead `doTransform` is done using `KDL`, thus functions relating to underlying data -were not added; such as `Vector3`, `Point`, or ported in this commit ros/geometry2/commit/6f2a82. -The non-stamped data with `toMsg` and `fromMsg` are `Quaternion`, `Transform`. `Pose` has the -modified `toMsg` and not used by `PoseStamped`. - -## Plan for autoware_auto_perception_msgs::msg::BoundingBoxArray - -The initial rough plan was to implement some of the common tf2 functions like `toMsg`, `fromMsg`, -and `doTransform`, as needed for all the underlying data types in `BoundingBoxArray`. Examples -of the data types include: `BoundingBox`, `Quaternion32`, and `Point32`. In addition, the -implementation should be done such that upstream contributions could also be made to `geometry_msgs`. - -## Assumptions / Known limits - -Due to conflicts in a function signatures, the predefined template of `convert.h`/ -`transform_functions.h` is not followed and compatibility with `tf2::convert(..)` is broken and -`toMsg` is written differently. - -```cpp -// Old style -geometry_msgs::Vector3 toMsg(const tf2::Vector3& in) -geometry_msgs::Point& toMsg(const tf2::Vector3& in) - -// New style -geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out) -``` - -## Inputs / Outputs / API - - - -The library provides API `doTransform` for the following data-types that are either not available -in `tf2_geometry_msgs` or the messages types are part of `autoware_auto_msgs` and are therefore -custom and not inherently supported by any of the tf2 libraries. The following APIs are provided -for the following data types: - -- `Point32` - -```cpp -inline void doTransform( - const geometry_msgs::msg::Point32 & t_in, - geometry_msgs::msg::Point32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `Quaternion32` (`autoware_auto_msgs`) - -```cpp -inline void doTransform( - const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in, - autoware_auto_geometry_msgs::msg::Quaternion32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `BoundingBox` (`autoware_auto_msgs`) - -```cpp -inline void doTransform( - const BoundingBox & t_in, BoundingBox & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `BoundingBoxArray` - -```cpp -inline void doTransform( - const BoundingBoxArray & t_in, - BoundingBoxArray & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -In addition, the following helper methods are also added: - -- `BoundingBoxArray` - -```cpp -inline tf2::TimePoint getTimestamp(const BoundingBoxArray & t) - -inline std::string getFrameId(const BoundingBoxArray & t) -``` - - - - - - - - - - - - - - -## Future extensions / Unimplemented parts - -## Challenges - -- `tf2_geometry_msgs` does not implement `doTransform` for any non-stamped data types, but it is - possible with the same function template. It is needed when transforming sub-data, with main data - that does have a stamp and can call doTransform on the sub-data with the same transform. Is this a useful upstream contribution? -- `tf2_geometry_msgs` does not have `Point`, `Point32`, does not seem it needs one, also the - implementation of non-standard `toMsg` would not help the convert. -- `BoundingBox` uses 32-bit float like `Quaternion32` and `Point32` to save space, as they are used - repeatedly in `BoundingBoxArray`. While transforming is it better to convert to 64-bit `Quaternion`, - `Point`, or `PoseStamped`, to re-use existing implementation of `doTransform`, or does it need to be - implemented? It may not be simple to template. - - - diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp deleted file mode 100644 index 50c16ba8eaf7d..0000000000000 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ /dev/null @@ -1,147 +0,0 @@ -// Copyright 2020, The Autoware Foundation. -// -// 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. -/// \file -/// \brief This file includes common transform functionality for autoware_auto_msgs - -#ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ -#define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include - -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; -using BoundingBoxArray = autoware_auto_perception_msgs::msg::BoundingBoxArray; -using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; - -namespace tf2 -{ -/******************/ -/** Quaternion32 **/ -/******************/ - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs Quaternion32 type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The Quaternion32 message to transform. - * \param t_out The transformed Quaternion32 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in, - autoware_auto_geometry_msgs::msg::Quaternion32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - KDL::Rotation r_in = KDL::Rotation::Quaternion(t_in.x, t_in.y, t_in.z, t_in.w); - KDL::Rotation out = gmTransformToKDL(transform).M * r_in; - - double qx, qy, qz, qw; - out.GetQuaternion(qx, qy, qz, qw); - t_out.x = static_cast(qx); - t_out.y = static_cast(qy); - t_out.z = static_cast(qz); - t_out.w = static_cast(qw); -} - -/******************/ -/** BoundingBox **/ -/******************/ - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs BoundingBox type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The BoundingBox message to transform. - * \param t_out The transformed BoundingBox message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const BoundingBox & t_in, BoundingBox & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = t_in; - doTransform(t_in.orientation, t_out.orientation, transform); - doTransform(t_in.centroid, t_out.centroid, transform); - doTransform(t_in.corners[0], t_out.corners[0], transform); - doTransform(t_in.corners[1], t_out.corners[1], transform); - doTransform(t_in.corners[2], t_out.corners[2], transform); - doTransform(t_in.corners[3], t_out.corners[3], transform); - // TODO(jitrc): add conversion for other fields of BoundingBox, such as heading, variance, size -} - -/**********************/ -/** BoundingBoxArray **/ -/**********************/ - -/** \brief Extract a timestamp from the header of a BoundingBoxArray message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. - * \param t A timestamped BoundingBoxArray message to extract the timestamp from. - * \return The timestamp of the message. - */ -template <> -inline tf2::TimePoint getTimestamp(const BoundingBoxArray & t) -{ - return tf2_ros::fromMsg(t.header.stamp); -} - -/** \brief Extract a frame ID from the header of a BoundingBoxArray message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. - * \param t A timestamped BoundingBoxArray message to extract the frame ID from. - * \return A string containing the frame ID of the message. - */ -template <> -inline std::string getFrameId(const BoundingBoxArray & t) -{ - return t.header.frame_id; -} - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs BoundingBoxArray type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The BoundingBoxArray to transform, as a timestamped BoundingBoxArray message. - * \param t_out The transformed BoundingBoxArray, as a timestamped BoundingBoxArray message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const BoundingBoxArray & t_in, BoundingBoxArray & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = t_in; - for (auto idx = 0U; idx < t_in.boxes.size(); idx++) { - doTransform(t_out.boxes[idx], t_out.boxes[idx], transform); - } - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; -} - -} // namespace tf2 - -#endif // AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml deleted file mode 100644 index b84e2d77befda..0000000000000 --- a/common/autoware_auto_tf2/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - autoware_auto_tf2 - 1.0.0 - Transform related utilities for different msg types - Jit Ray Chowdhury - Tomoya Kimura - Shumpei Wakabayashi - Satoshi Ota - Apache License 2.0 - - ament_cmake - autoware_cmake - - autoware_auto_common - autoware_auto_geometry_msgs - autoware_auto_perception_msgs - autoware_auto_system_msgs - geometry_msgs - tf2 - tf2_geometry_msgs - tf2_ros - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp deleted file mode 100644 index ce81dd2276870..0000000000000 --- a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp +++ /dev/null @@ -1,318 +0,0 @@ -// Copyright 2020, The Autoware Foundation. -// -// 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. -/// \file -/// \brief This file includes common transform functionally for autoware_auto_msgs - -#include -#include - -#include -#include -#include - -#include - -std::unique_ptr tf_buffer = nullptr; -constexpr double EPS = 1e-3; - -geometry_msgs::msg::TransformStamped filled_transform() -{ - geometry_msgs::msg::TransformStamped t; - t.transform.translation.x = 10; - t.transform.translation.y = 20; - t.transform.translation.z = 30; - t.transform.rotation.w = 0; - t.transform.rotation.x = 1; - t.transform.rotation.y = 0; - t.transform.rotation.z = 0; - t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - t.header.frame_id = "A"; - t.child_frame_id = "B"; - - return t; -} - -TEST(Tf2AutowareAuto, DoTransformPoint32) -{ - const auto trans = filled_transform(); - geometry_msgs::msg::Point32 p1; - p1.x = 1; - p1.y = 2; - p1.z = 3; - - // doTransform - geometry_msgs::msg::Point32 p_out; - tf2::doTransform(p1, p_out, trans); - - EXPECT_NEAR(p_out.x, 11, EPS); - EXPECT_NEAR(p_out.y, 18, EPS); - EXPECT_NEAR(p_out.z, 27, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformPolygon) -{ - const auto trans = filled_transform(); - geometry_msgs::msg::Polygon poly; - geometry_msgs::msg::Point32 p1; - p1.x = 1; - p1.y = 2; - p1.z = 3; - poly.points.push_back(p1); - // doTransform - geometry_msgs::msg::Polygon poly_out; - tf2::doTransform(poly, poly_out, trans); - - EXPECT_NEAR(poly_out.points[0].x, 11, EPS); - EXPECT_NEAR(poly_out.points[0].y, 18, EPS); - EXPECT_NEAR(poly_out.points[0].z, 27, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformQuaternion32) -{ - const auto trans = filled_transform(); - autoware_auto_geometry_msgs::msg::Quaternion32 q1; - q1.w = 0; - q1.x = 0; - q1.y = 0; - q1.z = 1; - - // doTransform - autoware_auto_geometry_msgs::msg::Quaternion32 q_out; - tf2::doTransform(q1, q_out, trans); - - EXPECT_NEAR(q_out.x, 0.0, EPS); - EXPECT_NEAR(q_out.y, 1.0, EPS); - EXPECT_NEAR(q_out.z, 0.0, EPS); - EXPECT_NEAR(q_out.w, 0.0, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformBoundingBox) -{ - const auto trans = filled_transform(); - BoundingBox bb1; - bb1.orientation.w = 0; - bb1.orientation.x = 0; - bb1.orientation.y = 0; - bb1.orientation.z = 1; - bb1.centroid.x = 1; - bb1.centroid.y = 2; - bb1.centroid.z = 3; - bb1.corners[0].x = 4; - bb1.corners[0].y = 5; - bb1.corners[0].z = 6; - bb1.corners[1].x = 7; - bb1.corners[1].y = 8; - bb1.corners[1].z = 9; - bb1.corners[2].x = 10; - bb1.corners[2].y = 11; - bb1.corners[2].z = 12; - bb1.corners[3].x = 13; - bb1.corners[3].y = 14; - bb1.corners[3].z = 15; - - // doTransform - BoundingBox bb_out; - tf2::doTransform(bb1, bb_out, trans); - - EXPECT_NEAR(bb_out.orientation.x, 0.0, EPS); - EXPECT_NEAR(bb_out.orientation.y, 1.0, EPS); - EXPECT_NEAR(bb_out.orientation.z, 0.0, EPS); - EXPECT_NEAR(bb_out.orientation.w, 0.0, EPS); - EXPECT_NEAR(bb_out.centroid.x, 11, EPS); - EXPECT_NEAR(bb_out.centroid.y, 18, EPS); - EXPECT_NEAR(bb_out.centroid.z, 27, EPS); - EXPECT_NEAR(bb_out.corners[0].x, 14, EPS); - EXPECT_NEAR(bb_out.corners[0].y, 15, EPS); - EXPECT_NEAR(bb_out.corners[0].z, 24, EPS); - EXPECT_NEAR(bb_out.corners[1].x, 17, EPS); - EXPECT_NEAR(bb_out.corners[1].y, 12, EPS); - EXPECT_NEAR(bb_out.corners[1].z, 21, EPS); - EXPECT_NEAR(bb_out.corners[2].x, 20, EPS); - EXPECT_NEAR(bb_out.corners[2].y, 9, EPS); - EXPECT_NEAR(bb_out.corners[2].z, 18, EPS); - EXPECT_NEAR(bb_out.corners[3].x, 23, EPS); - EXPECT_NEAR(bb_out.corners[3].y, 6, EPS); - EXPECT_NEAR(bb_out.corners[3].z, 15, EPS); - - // Testing unused fields are unmodified - EXPECT_EQ(bb_out.size, bb1.size); - EXPECT_EQ(bb_out.heading, bb1.heading); - EXPECT_EQ(bb_out.heading_rate, bb1.heading_rate); - EXPECT_EQ(bb_out.variance, bb1.variance); - EXPECT_EQ(bb_out.vehicle_label, bb1.vehicle_label); - EXPECT_EQ(bb_out.signal_label, bb1.signal_label); - EXPECT_EQ(bb_out.class_likelihood, bb1.class_likelihood); -} - -TEST(Tf2AutowareAuto, TransformBoundingBoxArray) -{ - BoundingBox bb1; - bb1.orientation.w = 0; - bb1.orientation.x = 0; - bb1.orientation.y = 0; - bb1.orientation.z = 1; - bb1.centroid.x = 20; - bb1.centroid.y = 21; - bb1.centroid.z = 22; - bb1.corners[0].x = 23; - bb1.corners[0].y = 24; - bb1.corners[0].z = 25; - bb1.corners[1].x = 26; - bb1.corners[1].y = 27; - bb1.corners[1].z = 28; - bb1.corners[2].x = 29; - bb1.corners[2].y = 30; - bb1.corners[2].z = 31; - bb1.corners[3].x = 32; - bb1.corners[3].y = 33; - bb1.corners[3].z = 34; - - BoundingBox bb2; - bb2.orientation.w = 0.707f; - bb2.orientation.x = -0.706f; - bb2.orientation.y = 0; - bb2.orientation.z = 0; - bb2.centroid.x = 50; - bb2.centroid.y = 51; - bb2.centroid.z = 52; - bb2.corners[0].x = 53; - bb2.corners[0].y = 54; - bb2.corners[0].z = 55; - bb2.corners[1].x = 56; - bb2.corners[1].y = 57; - bb2.corners[1].z = 58; - bb2.corners[2].x = 59; - bb2.corners[2].y = 50; - bb2.corners[2].z = 51; - bb2.corners[3].x = 52; - bb2.corners[3].y = 53; - bb2.corners[3].z = 54; - - BoundingBoxArray bba1; - bba1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - bba1.header.frame_id = "A"; - bba1.boxes.push_back(bb1); - bba1.boxes.push_back(bb2); - - // simple api - const auto bba_simple = tf_buffer->transform(bba1, "B", tf2::durationFromSec(2.0)); - - EXPECT_EQ(bba_simple.header.frame_id, "B"); - - // checking boxes[0] - EXPECT_NEAR(bba_simple.boxes[0].orientation.x, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.y, 1.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.w, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.x, 10, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.y, -1, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.z, 8, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].x, 13, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].y, -4, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].z, 5, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].x, 16, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].y, -7, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].z, 2, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].x, 19, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].y, -10, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].z, -1, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].x, 22, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].y, -13, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].z, -4, EPS); - - // checking boxes[1] - EXPECT_NEAR(bba_simple.boxes[1].orientation.x, 0.707, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.y, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.w, 0.707, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.x, 40, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.y, -31, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.z, -22, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].x, 43, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].y, -34, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].z, -25, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].x, 46, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].y, -37, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].z, -28, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].x, 49, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].y, -30, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].z, -21, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].x, 42, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].y, -33, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].z, -24, EPS); - - // advanced api - const auto bba_advanced = - tf_buffer->transform(bba1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); - - EXPECT_EQ(bba_advanced.header.frame_id, "B"); - - // checking boxes[0] - EXPECT_NEAR(bba_advanced.boxes[0].orientation.x, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.y, 1.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.w, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.x, 10, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.y, -1, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.z, 8, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].x, 13, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].y, -4, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].z, 5, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].x, 16, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].y, -7, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].z, 2, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].x, 19, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].y, -10, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].z, -1, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].x, 22, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].y, -13, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].z, -4, EPS); - - // checking boxes[1] - EXPECT_NEAR(bba_advanced.boxes[1].orientation.x, 0.707, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.y, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.w, 0.707, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.x, 40, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.y, -31, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.z, -22, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].x, 43, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].y, -34, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].z, -25, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].x, 46, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].y, -37, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].z, -28, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].x, 49, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].y, -30, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].z, -21, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].x, 42, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].y, -33, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].z, -24, EPS); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf_buffer = std::make_unique(clock); - tf_buffer->setUsingDedicatedThread(true); - - // populate buffer - const geometry_msgs::msg::TransformStamped t = filled_transform(); - tf_buffer->setTransform(t, "test"); - - int ret = RUN_ALL_TESTS(); - return ret; -} diff --git a/common/autoware_grid_map_utils/CMakeLists.txt b/common/autoware_grid_map_utils/CMakeLists.txt new file mode 100644 index 0000000000000..677b59a93a514 --- /dev/null +++ b/common/autoware_grid_map_utils/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_grid_map_utils) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(ament_cmake REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +target_link_libraries(${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_${PROJECT_NAME} + test/test_polygon_iterator.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + find_package(OpenCV REQUIRED) + add_executable(benchmark test/benchmark.cpp) + target_link_libraries(benchmark + ${PROJECT_NAME} + ${OpenCV_LIBS} + ) +endif() + +ament_auto_package() diff --git a/common/autoware_grid_map_utils/README.md b/common/autoware_grid_map_utils/README.md new file mode 100644 index 0000000000000..34adfe230cf78 --- /dev/null +++ b/common/autoware_grid_map_utils/README.md @@ -0,0 +1,51 @@ +# Grid Map Utils + +## Overview + +This packages contains a re-implementation of the `grid_map::PolygonIterator` used to iterate over +all cells of a grid map contained inside some polygon. + +## Algorithm + +This implementation uses the [scan line algorithm](https://en.wikipedia.org/wiki/Scanline_rendering), +a common algorithm used to draw polygons on a rasterized image. +The main idea of the algorithm adapted to a grid map is as follow: + +- calculate intersections between rows of the grid map and the edges of the polygon edges; +- calculate for each row the column between each pair of intersections; +- the resulting `(row, column)` indexes are inside of the polygon. + +More details on the scan line algorithm can be found in the References. + +## API + +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 `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 `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 (`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. + +![Runtime comparison](media/runtime_comparison.png) + +## Future improvements + +There exists variations of the scan line algorithm for multiple polygons. +These can be implemented if we want to iterate over the cells contained in at least one of multiple polygons. + +The current implementation imitate the behavior of the original `grid_map::PolygonIterator` where a cell is selected if its center position is inside the polygon. +This behavior could be changed for example to only return all cells overlapped by the polygon. + +## References + +- +- 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/autoware_grid_map_utils/package.xml b/common/autoware_grid_map_utils/package.xml new file mode 100644 index 0000000000000..d8344b08f2341 --- /dev/null +++ b/common/autoware_grid_map_utils/package.xml @@ -0,0 +1,25 @@ + + + + autoware_grid_map_utils + 0.0.0 + Utilities for the grid_map library + Maxime CLEMENT + Apache License 2.0 + + autoware_cmake + eigen3_cmake_module + + autoware_universe_utils + grid_map_core + grid_map_cv + libopencv-dev + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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/autoware_motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt new file mode 100644 index 0000000000000..4c36ef2f4e70d --- /dev/null +++ b/common/autoware_motion_utils/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Boost REQUIRED) + +ament_auto_add_library(autoware_motion_utils SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + + file(GLOB_RECURSE test_files test/**/*.cpp) + + ament_add_ros_isolated_gtest(test_autoware_motion_utils ${test_files}) + + target_link_libraries(test_autoware_motion_utils + autoware_motion_utils + ) +endif() + +ament_auto_package() diff --git a/common/autoware_motion_utils/README.md b/common/autoware_motion_utils/README.md new file mode 100644 index 0000000000000..df4808a6f68d7 --- /dev/null +++ b/common/autoware_motion_utils/README.md @@ -0,0 +1,117 @@ +# Motion Utils package + +## Definition of terms + +### Segment + +`Segment` in Autoware is the line segment between two successive points as follows. + +![segment](./media/segment.svg){: style="width:600px"} + +The nearest segment index and nearest point index to a certain position is not always th same. +Therefore, we prepare two different utility functions to calculate a nearest index for points and segments. + +## Nearest index search + +In this section, the nearest index and nearest segment index search is explained. + +We have the same functions for the nearest index search and nearest segment index search. +Taking for the example the nearest index search, we have two types of functions. + +The first function finds the nearest index with distance and yaw thresholds. + +```cpp +template +size_t findFirstNearestIndexWithSoftConstraints( + const T & points, const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +``` + +This function finds the first local solution within thresholds. +The reason to find the first local one is to deal with some edge cases explained in the next subsection. + +There are default parameters for thresholds arguments so that you can decide which thresholds to pass to the function. + +1. When both the distance and yaw thresholds are given. + - First, try to find the nearest index with both the distance and yaw thresholds. + - If not found, try to find again with only the distance threshold. + - If not found, find without any thresholds. +2. When only distance are given. + - First, try to find the nearest index the distance threshold. + - If not found, find without any thresholds. +3. When no thresholds are given. + - Find the nearest index. + +The second function finds the nearest index in the lane whose id is `lane_id`. + +```cpp +size_t findNearestIndexFromLaneId( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id); +``` + +### Application to various object + +Many node packages often calculate the nearest index of objects. +We will explain the recommended method to calculate it. + +#### Nearest index for the ego + +Assuming that the path length before the ego is short enough, we expect to find the correct nearest index in the following edge cases by `findFirstNearestIndexWithSoftConstraints` with both distance and yaw thresholds. +Blue circles describes the distance threshold from the base link position and two blue lines describe the yaw threshold against the base link orientation. +Among points in these cases, the correct nearest point which is red can be found. + +![ego_nearest_search](./media/ego_nearest_search.svg) + +Therefore, the implementation is as follows. + +```cpp +const size_t ego_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); +const size_t ego_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); +``` + +#### Nearest index for dynamic objects + +For the ego nearest index, the orientation is considered in addition to the position since the ego is supposed to follow the points. +However, for the dynamic objects (e.g., predicted object), sometimes its orientation may be different from the points order, e.g. the dynamic object driving backward although the ego is driving forward. + +Therefore, the yaw threshold should not be considered for the dynamic object. +The implementation is as follows. + +```cpp +const size_t dynamic_obj_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold); +const size_t dynamic_obj_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold); +``` + +#### Nearest index for traffic objects + +In lanelet maps, traffic objects belong to the specific lane. +With this specific lane's id, the correct nearest index can be found. + +The implementation is as follows. + +```cpp +// first extract `lane_id` which the traffic object belong to. +const size_t traffic_obj_nearest_idx = findNearestIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id); +const size_t traffic_obj_nearest_seg_idx = findNearestSegmentIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id); +``` + +## Path/Trajectory length calculation between designated points + +Based on the discussion so far, the nearest index search algorithm is different depending on the object type. +Therefore, we recommended using the wrapper utility functions which require the nearest index search (e.g., calculating the path length) with each nearest index search. + +For example, when we want to calculate the path length between the ego and the dynamic object, the implementation is as follows. + +```cpp +const size_t ego_nearest_seg_idx = findFirstNearestSegmentIndex(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); +const size_t dyn_obj_nearest_seg_idx = findFirstNearestSegmentIndex(points, dyn_obj_pose, dyn_obj_nearest_dist_threshold); +const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_nearest_seg_idx, dyn_obj_pose, dyn_obj_nearest_seg_idx); +``` + +## For developers + +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. + +`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 91% rename from common/motion_utils/docs/vehicle/vehicle.md rename to common/autoware_motion_utils/docs/vehicle/vehicle.md index 9d33b5ed372c4..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. @@ -83,10 +83,10 @@ This class check whether the vehicle arrive at stop point based on localization #### Subscribed Topics -| Name | Type | Description | -| ---------------------------------------- | ---------------------------------------------- | ---------------- | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | -| `/planning/scenario_planning/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | trajectory | +| Name | Type | Description | +| ---------------------------------------- | ----------------------------------------- | ---------------- | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | +| `/planning/scenario_planning/trajectory` | `autoware_planning_msgs::msg::Trajectory` | trajectory | #### Parameters @@ -116,7 +116,7 @@ bool isVehicleStoppedAtStopPoint(const double stop_duration) Necessary includes: ```c++ -#include +#include ``` 1.Create a checker instance. diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp new file mode 100644 index 0000000000000..9c1f7c876e73e --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp @@ -0,0 +1,23 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ +#define AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ + +namespace autoware::motion_utils +{ +constexpr double overlap_threshold = 0.1; +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp new file mode 100644 index 0000000000000..308528954eccc --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp @@ -0,0 +1,33 @@ +// 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 AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ +#define AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ + +#include +#include +#include +#include +#include +#include + +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 autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp new file mode 100644 index 0000000000000..30eca6927db34 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp @@ -0,0 +1,54 @@ + +// Copyright 2022-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__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ +#define AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include +#include + +namespace autoware::motion_utils +{ +using autoware_adapi_v1_msgs::msg::PlanningBehavior; +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using VelocityFactorBehavior = VelocityFactor::_behavior_type; +using VelocityFactorStatus = VelocityFactor::_status_type; +using geometry_msgs::msg::Pose; + +class VelocityFactorInterface +{ +public: + [[nodiscard]] VelocityFactor get() const { return velocity_factor_; } + void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; } + void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; } + + template + void set( + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail = ""); + +private: + VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN}; + VelocityFactor velocity_factor_{}; +}; + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp new file mode 100644 index 0000000000000..4679e3d9aba91 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 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__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ +#define AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ + +#include + +#include + +#include + +namespace autoware::motion_utils +{ +using geometry_msgs::msg::Pose; + +visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( + const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); + +visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( + const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); + +visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( + const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); + +visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( + const rclcpp::Time & now, const int32_t id); + +visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker( + const rclcpp::Time & now, const int32_t id); + +visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( + const rclcpp::Time & now, const int32_t id); +} // namespace autoware::motion_utils + +#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/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp new file mode 100644 index 0000000000000..19328179932c4 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp @@ -0,0 +1,239 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef 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" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" + +#include + +namespace autoware::motion_utils +{ +/** + * @brief A resampling function for a path(points). Note that in a default setting, position xy are + * resampled by spline interpolation, position z are resampled by linear interpolation, and + * orientation of the resampled path are calculated by a forward difference method + * based on the interpolated position x and y. + * @param input_path input path(point) to resample + * @param resampled_arclength arclength that contains length of each resampling points from initial + * point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @return resampled path(poses) + */ +std::vector resamplePointVector( + const std::vector & points, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true); + +/** + * @brief A resampling function for a path(position). Note that in a default setting, position xy + * are resampled by spline interpolation, position z are resampled by linear interpolation, and + * orientation of the resampled path are calculated by a forward difference method + * based on the interpolated position x and y. + * @param input_path input path(position) to resample + * @param resample_interval resampling interval + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @return resampled path(poses) + */ +std::vector resamplePointVector( + const std::vector & points, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); + +/** + * @brief A resampling function for a path(poses). Note that in a default setting, position xy are + * resampled by spline interpolation, position z are resampled by linear interpolation, and + * orientation of the resampled path are calculated by a forward difference method + * based on the interpolated position x and y. + * @param input_path input path(poses) to resample + * @param resampled_arclength arclength that contains length of each resampling points from initial + * point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @return resampled path(poses) + */ +std::vector resamplePoseVector( + const std::vector & points, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true); + +/** + * @brief A resampling function for a path(poses). Note that in a default setting, position xy are + * resampled by spline interpolation, position z are resampled by linear interpolation, and + * orientation of the resampled path are calculated by a forward difference method + * based on the interpolated position x and y. + * @param input_path input path(poses) to resample + * @param resample_interval resampling interval + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @return resampled path(poses) + */ +std::vector resamplePoseVector( + const std::vector & points, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); + +/** + * @brief A resampling function for a path with lane id. Note that in a default setting, position xy + * are resampled by spline interpolation, position z are resampled by linear interpolation, + * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is + * resampled by linear interpolation. Orientation of the resampled path are calculated by a + * forward difference method based on the interpolated position x and y. Moreover, lane_ids + * and is_final are also interpolated by zero order hold + * @param input_path input path to resample + * @param resampled_arclength arclength that contains length of each resampling points from initial + * point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample + * longitudinal and lateral velocity. Otherwise, it uses linear interpolation + * @return resampled path + */ +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); + +/** + * @brief A resampling function for a path with lane id. Note that in a default setting, position xy + * are resampled by spline interpolation, position z are resampled by linear interpolation, + * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is + * resampled by linear interpolation. Orientation of the resampled path are calculated by a + * forward difference method based on the interpolated position x and y. Moreover, lane_ids + * and is_final are also interpolated by zero order hold + * @param input_path input path to resample + * @param resampled_interval resampling interval point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample + * longitudinal and lateral velocity. Otherwise, it uses linear interpolation + * @param resample_input_path_stop_point If true, resample closest stop point in input path + * @return resampled path + */ +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, + const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); + +/** + * @brief A resampling function for a path. Note that in a default setting, position xy are + * resampled by spline interpolation, position z are resampled by linear interpolation, + * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is + * resampled by linear interpolation. Orientation of the resampled path are calculated by a + * forward difference method based on the interpolated position x and y. + * @param input_path input path to resample + * @param resampled_arclength arclength that contains length of each resampling points from initial + * point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample + * longitudinal and lateral velocity. Otherwise, it uses linear interpolation + * @return resampled path + */ +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); + +/** + * @brief A resampling function for a path. Note that in a default setting, position xy + * are resampled by spline interpolation, position z are resampled by linear interpolation, + * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is + * resampled by linear interpolation. Orientation of the resampled path are calculated by a + * forward difference method based on the interpolated position x and y. + * @param input_path input path to resample + * @param resampled_interval resampling interval point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample + * longitudinal and lateral velocity. Otherwise, it uses linear interpolation + * @param resample_input_path_stop_point If true, resample closest stop point in input path + * @return resampled path + */ +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, + 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_path_stop_point = true); + +/** + * @brief A resampling function for a trajectory. Note that in a default setting, position xy are + * resampled by spline interpolation, position z are resampled by linear interpolation, twist + * informaiton(velocity and acceleration) are resampled by zero_order_hold, and heading rate + * is resampled by linear interpolation. The rest of the category is resampled by linear + * interpolation. Orientation of the resampled path are calculated by a forward difference + * method based on the interpolated position x and y. + * @param input_trajectory input trajectory to resample + * @param resampled_arclength arclength that contains length of each resampling points from initial + * point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample + * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation + * @return resampled trajectory + */ +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true); + +/** + * @brief A resampling function for a trajectory. This function resamples closest stop point, + * terminal point and points by resample interval. Note that in a default setting, position + * xy are resampled by spline interpolation, position z are resampled by linear interpolation, twist + * informaiton(velocity and acceleration) are resampled by zero_order_hold, and heading rate + * is resampled by linear interpolation. The rest of the category is resampled by linear + * interpolation. Orientation of the resampled path are calculated by a forward difference + * method based on the interpolated position x and y. + * @param input_trajectory input trajectory to resample + * @param resampled_interval resampling interval + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample + * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation + * @param resample_input_trajectory_stop_point If true, resample closest stop point in input + * trajectory + * @return resampled trajectory + */ +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, + 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 autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp new file mode 100644 index 0000000000000..5d622c54da452 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp @@ -0,0 +1,127 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ + +#include "autoware/universe_utils/system/backtrace.hpp" + +#include +#include +#include + +#include + +namespace resample_utils +{ +constexpr double close_s_threshold = 1e-6; + +static inline rclcpp::Logger get_logger() +{ + constexpr const char * logger{"autoware_motion_utils.resample_utils"}; + return rclcpp::get_logger(logger); +} + +template +bool validate_size(const T & points) +{ + return points.size() >= 2; +} + +template +bool validate_resampling_range(const T & points, const std::vector & resampling_intervals) +{ + const double points_length = autoware::motion_utils::calcArcLength(points); + return points_length >= resampling_intervals.back(); +} + +template +bool validate_points_duplication(const T & points) +{ + for (size_t i = 0; i < points.size() - 1; ++i) { + 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; + } + } + + return true; +} + +template +bool validate_arguments(const T & input_points, const std::vector & resampling_intervals) +{ + // 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"); + 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"); + 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"); + 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"); + autoware::universe_utils::print_backtrace(); + return false; + } + + return true; +} + +template +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"); + autoware::universe_utils::print_backtrace(); + return false; + } + + // check resampling interval + if (resampling_interval < autoware::motion_utils::overlap_threshold) { + RCLCPP_DEBUG( + get_logger(), "invalid argument: resampling interval is less than %f", + 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"); + autoware::universe_utils::print_backtrace(); + return false; + } + + return true; +} +} // namespace resample_utils + +#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp new file mode 100644 index 0000000000000..d4f88c17c4d70 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp @@ -0,0 +1,120 @@ +// Copyright 2021 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__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" +#include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp" +#include "std_msgs/msg/header.hpp" +#include "tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" + +#include + +namespace autoware::motion_utils +{ +using TrajectoryPoints = std::vector; + +/** + * @brief Convert std::vector to + * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, + const std_msgs::msg::Header & header = std_msgs::msg::Header{}); + +/** + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory); + +template +autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); + throw std::logic_error("Only specializations of convertToPath can be used."); +} + +template <> +inline autoware_planning_msgs::msg::Path convertToPath( + const tier4_planning_msgs::msg::PathWithLaneId & input) +{ + autoware_planning_msgs::msg::Path output{}; + output.header = input.header; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; + output.points.resize(input.points.size()); + for (size_t i = 0; i < input.points.size(); ++i) { + output.points.at(i) = input.points.at(i).point; + } + return output; +} + +template +TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used."); + throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used."); +} + +template <> +inline TrajectoryPoints convertToTrajectoryPoints( + const tier4_planning_msgs::msg::PathWithLaneId & input) +{ + TrajectoryPoints output{}; + for (const auto & p : input.points) { + autoware_planning_msgs::msg::TrajectoryPoint tp; + tp.pose = p.point.pose; + tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; + // since path point doesn't have acc for now + tp.acceleration_mps2 = 0; + output.emplace_back(tp); + } + return output; +} + +template +tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); + throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); +} + +template <> +inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + const TrajectoryPoints & input) +{ + tier4_planning_msgs::msg::PathWithLaneId output{}; + for (const auto & p : input) { + tier4_planning_msgs::msg::PathPointWithLaneId pp; + pp.point.pose = p.pose; + pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; + output.points.emplace_back(pp); + } + return output; +} + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp new file mode 100644 index 0000000000000..5272478cccd78 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp @@ -0,0 +1,96 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_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" + +#include + +#include +#include + +namespace autoware::motion_utils +{ +/** + * @brief An interpolation function that finds the closest interpolated point on the trajectory from + * the given pose + * @param trajectory input trajectory + * @param target_pose target_pose + * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for + * twist information + * @return resampled path(poses) + */ +autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + +/** + * @brief An interpolation function that finds the closest interpolated point on the path from + * the given pose + * @param path input path + * @param target_pose target_pose + * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for + * twist information + * @return resampled path(poses) + */ +tier4_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + +/** + * @brief An interpolation function that finds the closest interpolated point on the path that is a + * certain length away from the given pose + * @param points input path + * @param target_length length from the front point of the path + * @return resampled pose + */ +template +geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double target_length) +{ + if (points.empty()) { + geometry_msgs::msg::Pose interpolated_pose; + return interpolated_pose; + } + + if (points.size() < 2 || target_length < 0.0) { + 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 = 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 autoware::universe_utils::calcInterpolatedPose(curr_pose, next_pose, ratio); + } + accumulated_length += length; + } + + return autoware::universe_utils::getPose(points.back()); +} + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp new file mode 100644 index 0000000000000..0d875e636bad5 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp @@ -0,0 +1,46 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef 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 autoware::motion_utils +{ +std::optional> getPathIndexRangeWithLaneId( + const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); + +size_t findNearestIndexFromLaneId( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id); + +size_t findNearestSegmentIndexFromLaneId( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id); + +// @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle +// center follow the input path +// @param [in] path with position to be followed by the center of the vehicle +// @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle +// center follow the input path NOTE: rear_to_cog is supposed to be positive +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 autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp new file mode 100644 index 0000000000000..da37d04550f5e --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -0,0 +1,2492 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_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 + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_utils +{ +static inline rclcpp::Logger get_logger() +{ + constexpr const char * logger{"autoware_motion_utils.trajectory"}; + return rclcpp::get_logger(logger); +} + +/** + * @brief validate if points container is empty or not + * @param points points of trajectory, path, ... + */ +template +void validateNonEmpty(const T & points) +{ + if (points.empty()) { + autoware::universe_utils::print_backtrace(); + throw std::invalid_argument("[autoware_motion_utils] validateNonEmpty(): Points is empty."); + } +} + +extern template void validateNonEmpty>( + const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); + +/** + * @brief validate a point is in a non-sharp angle between two points or not + * @param point1 front point + * @param point2 point to be validated + * @param point3 back point + */ +template +void validateNonSharpAngle( + const T & point1, const T & point2, const T & point3, + const double angle_threshold = autoware::universe_utils::pi / 4) +{ + 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 = 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) { + autoware::universe_utils::print_backtrace(); + throw std::invalid_argument( + "[autoware_motion_utils] validateNonSharpAngle(): Too sharp angle."); + } +} + +/** + * @brief checks whether a path of trajectory has forward driving direction + * @param points points of trajectory, path, ... + * @return (forward / backward) driving (true / false) + */ +template +std::optional isDrivingForward(const T & points) +{ + if (points.size() < 2) { + return std::nullopt; + } + + // check the first point direction + const auto & first_pose = autoware::universe_utils::getPose(points.at(0)); + const auto & second_pose = autoware::universe_utils::getPose(points.at(1)); + + return autoware::universe_utils::isDrivingForward(first_pose, second_pose); +} + +extern template std::optional +isDrivingForward>( + const std::vector &); +extern template std::optional +isDrivingForward>( + const std::vector &); +extern template std::optional +isDrivingForward>( + const std::vector &); + +/** + * @brief checks whether a path of trajectory has forward driving direction using its longitudinal + * velocity + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @return (forward / backward) driving (true, false, none "if velocity is zero") + */ +template +std::optional isDrivingForwardWithTwist(const T & points_with_twist) +{ + if (points_with_twist.empty()) { + return std::nullopt; + } + if (points_with_twist.size() == 1) { + if (0.0 < autoware::universe_utils::getLongitudinalVelocity(points_with_twist.front())) { + return true; + } + if (0.0 > autoware::universe_utils::getLongitudinalVelocity(points_with_twist.front())) { + return false; + } + return std::nullopt; + } + + return isDrivingForward(points_with_twist); +} + +extern template std::optional +isDrivingForwardWithTwist>( + const std::vector &); +extern template std::optional +isDrivingForwardWithTwist>( + const std::vector &); +extern template std::optional +isDrivingForwardWithTwist>( + const std::vector &); + +/** + * @brief remove overlapping points through points container. + * Overlapping is determined by calculating the distance between 2 consecutive points. + * If the distance between them is less than a threshold, they will be considered overlapping. + * @param points points of trajectory, path, ... + * @param start_idx index to start the overlap remove calculation from through the points + * container. Indices before that index will be considered non-overlapping. Default = 0 + * @return points container without overlapping points + */ +template +T removeOverlapPoints(const T & points, const size_t start_idx = 0) +{ + if (points.size() < start_idx + 1) { + return points; + } + + T dst; + dst.reserve(points.size()); + + for (size_t i = 0; i <= start_idx; ++i) { + dst.push_back(points.at(i)); + } + + constexpr double eps = 1.0E-08; + for (size_t i = start_idx + 1; i < points.size(); ++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; + } + dst.push_back(points.at(i)); + } + + return dst; +} + +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); +extern template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx = 0); +extern template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx = 0); + +/** + * @brief search through points container from specified start and end indices about first matching + * index of a zero longitudinal velocity point. + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @param src_idx start index of the search + * @param dst_idx end index of the search + * @return first matching index of a zero velocity point inside the points container. + */ +template +std::optional searchZeroVelocityIndex( + const T & points_with_twist, const size_t src_idx, const size_t dst_idx) +{ + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + constexpr double epsilon = 1e-3; + for (size_t i = src_idx; i < dst_idx; ++i) { + if (std::fabs(points_with_twist.at(i).longitudinal_velocity_mps) < epsilon) { + return i; + } + } + + return {}; +} + +extern template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx, const size_t dst_idx); + +/** + * @brief search through points container from specified start index till end of points container + * about first matching index of a zero longitudinal velocity point. + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @param src_idx start index of the search + * @return first matching index of a zero velocity point inside the points container. + */ +template +std::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t src_idx) +{ + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + return searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); +} + +extern template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx); + +/** + * @brief search through points container from its start to end about first matching index of a zero + * longitudinal velocity point. + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @return first matching index of a zero velocity point inside the points container. + */ +template +std::optional searchZeroVelocityIndex(const T & points_with_twist) +{ + return searchZeroVelocityIndex(points_with_twist, 0, points_with_twist.size()); +} + +extern template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist); + +/** + * @brief find nearest point index through points container for a given point. + * Finding nearest point is determined by looping through the points container, + * and calculating the 2D squared distance between each point in the container and the given point. + * The index of the point with minimum distance and yaw deviation comparing to the given point will + * be returned. + * @param points points of trajectory, path, ... + * @param point given point + * @return index of nearest point + */ +template +size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & point) +{ + validateNonEmpty(points); + + double min_dist = std::numeric_limits::max(); + size_t min_idx = 0; + + for (size_t i = 0; i < points.size(); ++i) { + const auto dist = autoware::universe_utils::calcSquaredDistance2d(points.at(i), point); + if (dist < min_dist) { + min_dist = dist; + min_idx = i; + } + } + return min_idx; +} + +extern template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +/** + * @brief find nearest point index through points container for a given pose. + * Finding nearest point is determined by looping through the points container, + * and finding the nearest point to the given pose in terms of squared 2D distance and yaw + * deviation. The index of the point with minimum distance and yaw deviation comparing to the given + * pose will be returned. + * @param points points of trajectory, path, ... + * @param pose given pose + * @param max_dist max distance used to get squared distance for finding the nearest point to given + * pose + * @param max_yaw max yaw used for finding nearest point to given pose + * @return index of nearest point (index or none if not found) + */ +template +std::optional findNearestIndex( + const T & points, const geometry_msgs::msg::Pose & pose, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + const double max_squared_dist = max_dist * max_dist; + + double min_squared_dist = std::numeric_limits::max(); + bool is_nearest_found = false; + size_t min_idx = 0; + + for (size_t i = 0; i < points.size(); ++i) { + 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 = autoware::universe_utils::calcYawDeviation( + autoware::universe_utils::getPose(points.at(i)), pose); + if (std::fabs(yaw) > max_yaw) { + continue; + } + + min_squared_dist = squared_dist; + min_idx = i; + is_nearest_found = true; + } + + if (is_nearest_found) { + return min_idx; + } + return std::nullopt; +} + +extern template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + +/** + * @brief calculate longitudinal offset (length along trajectory from seg_idx point to nearest point + * to p_target on trajectory). If seg_idx point is after that nearest point, length is negative. + * Segment is straight path between two continuous points of trajectory. + * @param points points of trajectory, path, ... + * @param seg_idx segment index of point at beginning of length + * @param p_target target point at end of length + * @param throw_exception flag to enable/disable exception throwing + * @return signed length + */ +template +double calcLongitudinalOffsetToSegment( + const T & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + const bool throw_exception = false) +{ + if (seg_idx >= points.size() - 1) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + ": Failed to calculate longitudinal offset because the given segment index is out of the " + "points size."); + autoware::universe_utils::print_backtrace(); + if (throw_exception) { + throw std::out_of_range(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return std::nan(""); + } + + const auto overlap_removed_points = removeOverlapPoints(points, seg_idx); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return std::nan(""); + } + } + + if (seg_idx >= overlap_removed_points.size() - 1) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + ": Longitudinal offset calculation is not supported for the same points."); + autoware::universe_utils::print_backtrace(); + if (throw_exception) { + throw std::runtime_error(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return std::nan(""); + } + + 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}; + + return segment_vec.dot(target_vec) / segment_vec.norm(); +} + +extern template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); + +/** + * @brief find nearest segment index to point. + * Segment is straight path between two continuous points of trajectory. + * When point is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 + * @param points points of trajectory, path, ... + * @param point point to which to find nearest segment index + * @return nearest index + */ +template +size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point & point) +{ + const size_t nearest_idx = findNearestIndex(points, point); + + if (nearest_idx == 0) { + return 0; + } + if (nearest_idx == points.size() - 1) { + return points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, point); + + if (signed_length <= 0) { + return nearest_idx - 1; + } + + return nearest_idx; +} + +extern template size_t findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +/** + * @brief find nearest segment index to pose + * Segment is straight path between two continuous points of trajectory. + * When pose is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 + * @param points points of trajectory, path, .. + * @param pose pose to which to find nearest segment index + * @param max_dist max distance used for finding the nearest index to given pose + * @param max_yaw max yaw used for finding nearest index to given pose + * @return nearest index + */ +template +std::optional findNearestSegmentIndex( + const T & points, const geometry_msgs::msg::Pose & pose, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()) +{ + const auto nearest_idx = findNearestIndex(points, pose, max_dist, max_yaw); + + if (!nearest_idx) { + return std::nullopt; + } + + if (*nearest_idx == 0) { + return 0; + } + if (*nearest_idx == points.size() - 1) { + return points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(points, *nearest_idx, pose.position); + + if (signed_length <= 0) { + return *nearest_idx - 1; + } + + return *nearest_idx; +} + +extern template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + +/** + * @brief calculate lateral offset from p_target (length from p_target to trajectory) using given + * segment index. Segment is straight path between two continuous points of trajectory. + * @param points points of trajectory, path, ... + * @param p_target target point + * @param seg_idx segment index of point at beginning of length + * @param throw_exception flag to enable/disable exception throwing + * @return length (unsigned) + */ +template +double calcLateralOffset( + const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false) +{ + const auto overlap_removed_points = removeOverlapPoints(points, 0); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + e.what()); + return std::nan(""); + } + } + + if (overlap_removed_points.size() == 1) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); + autoware::universe_utils::print_backtrace(); + if (throw_exception) { + throw std::runtime_error(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return std::nan(""); + } + + const auto p_indices = overlap_removed_points.size() - 2; + 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 = 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}; + + const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec); + return cross_vec(2) / segment_vec.norm(); +} + +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); + +/** + * @brief calculate lateral offset from p_target (length from p_target to trajectory). + * The function gets the nearest segment index between the points of trajectory and the given target + * point, then uses that segment index to calculate lateral offset. Segment is straight path between + * two continuous points of trajectory. + * @param points points of trajectory, path, ... + * @param p_target target point + * @param throw_exception flag to enable/disable exception throwing + * @return length (unsigned) + */ +template +double calcLateralOffset( + const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false) +{ + const auto overlap_removed_points = removeOverlapPoints(points, 0); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + e.what()); + return std::nan(""); + } + } + + if (overlap_removed_points.size() == 1) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); + autoware::universe_utils::print_backtrace(); + if (throw_exception) { + throw std::runtime_error(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return std::nan(""); + } + + const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target); + return calcLateralOffset(points, p_target, seg_idx, throw_exception); +} + +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); + +/** + * @brief calculate length of 2D distance between two points, specified by start and end points + * indicies through points container. + * @param points points of trajectory, path, ... + * @param src_idx index of start point + * @param dst_idx index of end point + * @return length of distance between two points. + * Length is positive if dst_idx is greater that src_idx (i.e. after it in trajectory, path, ...) + * and negative otherwise. + */ +template +double calcSignedArcLength(const T & points, const size_t src_idx, const size_t dst_idx) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + + if (src_idx > dst_idx) { + return -calcSignedArcLength(points, dst_idx, src_idx); + } + + double dist_sum = 0.0; + for (size_t i = src_idx; i < dst_idx; ++i) { + dist_sum += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + } + return dist_sum; +} + +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); + +/** + * @brief Computes the partial sums of the elements in the sub-ranges of the range [src_idx, + * dst_idx) and return these sum as vector + * @param points points of trajectory, path, ... + * @param src_idx index of start point + * @param dst_idx index of end point + * @return partial sums container + */ +template +std::vector calcSignedArcLengthPartialSum( + const T & points, const size_t src_idx, const size_t dst_idx) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + if (src_idx + 1 > dst_idx) { + auto copied = points; + std::reverse(copied.begin(), copied.end()); + return calcSignedArcLengthPartialSum(points, dst_idx, src_idx); + } + + std::vector partial_dist; + partial_dist.reserve(dst_idx - src_idx); + + double dist_sum = 0.0; + partial_dist.push_back(dist_sum); + for (size_t i = src_idx; i < dst_idx - 1; ++i) { + dist_sum += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + partial_dist.push_back(dist_sum); + } + return partial_dist; +} + +extern template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +extern template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +extern template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); + +/** + * @brief calculate length of 2D distance between two points, specified by start point and end point + * index of points container. + * @param points points of trajectory, path, ... + * @param src_point start point + * @param dst_idx index of end point + * @return length of distance between two points. + * Length is positive if destination point associated to dst_idx is greater that src_idx (i.e. after + * it in trajectory, path, ...) and negative otherwise. + */ +template +double calcSignedArcLength( + const T & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + + const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_idx); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + + return signed_length_on_traj - signed_length_src_offset; +} + +extern template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); + +/** + * @brief calculate length of 2D distance between two points, specified by start index of points + * container and end point. + * @param points points of trajectory, path, ... + * @param src_idx index of start point + * @param dst_point end point + * @return length of distance between two points + * Length is positive if destination point is greater that source point associated to src_idx (i.e. + * after it in trajectory, path, ...) and negative otherwise. + */ +template +double calcSignedArcLength( + const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + + return -calcSignedArcLength(points, dst_point, src_idx); +} + +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); + +/** + * @brief calculate length of 2D distance between two points, specified by start point and end + * point. + * @param points points of trajectory, path, ... + * @param src_point start point + * @param dst_point end point + * @return length of distance between two points. + * Length is positive if destination point is greater that source point (i.e. after it in + * trajectory, path, ...) and negative otherwise. + * + */ +template +double calcSignedArcLength( + const T & points, const geometry_msgs::msg::Point & src_point, + const geometry_msgs::msg::Point & dst_point) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point); + + const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_seg_idx); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + const double signed_length_dst_offset = + calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + + return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; +} + +extern template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); + +/** + * @brief calculate length of 2D distance for whole points container, from its start to its end. + * @param points points of trajectory, path, ... + * @return length of 2D distance for points container + */ +template +double calcArcLength(const T & points) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + + return calcSignedArcLength(points, 0, points.size() - 1); +} + +extern template double calcArcLength>( + const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); + +/** + * @brief calculate curvature through points container. + * The method used for calculating the curvature is using 3 consecutive points through the points + * container. Then the curvature is the reciprocal of the radius of the circle that passes through + * these three points. + * @details more details here : https://en.wikipedia.org/wiki/Menger_curvature + * @param points points of trajectory, path, ... + * @return calculated curvature container through points container + */ +template +std::vector calcCurvature(const T & points) +{ + std::vector curvature_vec(points.size(), 0.0); + if (points.size() < 3) { + return curvature_vec; + } + + for (size_t i = 1; i < points.size() - 1; ++i) { + 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); + + return curvature_vec; +} + +extern template std::vector +calcCurvature>( + const std::vector & points); +extern template std::vector +calcCurvature>( + const std::vector & points); +extern template std::vector +calcCurvature>( + const std::vector & points); + +/** + * @brief calculate curvature through points container and length of 2d distance for segment used + * for curvature calculation. The method used for calculating the curvature is using 3 consecutive + * points through the points container. Then the curvature is the reciprocal of the radius of the + * circle that passes through these three points. Then length of 2D distance of these points is + * calculated + * @param points points of trajectory, path, ... + * @return Container of pairs, calculated curvature and length of 2D distance for segment used for + * curvature calculation + */ +template +std::vector> calcCurvatureAndArcLength(const T & points) +{ + // Note that arclength is for the segment, not the sum. + 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 = 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); + + return curvature_arc_length_vec; +} + +extern template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +extern template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +extern template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); + +/** + * @brief calculate length of 2D distance between given start point index in points container and + * first point in container with zero longitudinal velocity + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @return Length of 2D distance between start point index in points container and first point in + * container with zero longitudinal velocity + */ +template +std::optional calcDistanceToForwardStopPoint( + const T & points_with_twist, const size_t src_idx = 0) +{ + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + const auto closest_stop_idx = + searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); + if (!closest_stop_idx) { + return std::nullopt; + } + + return std::max(0.0, calcSignedArcLength(points_with_twist, src_idx, *closest_stop_idx)); +} + +extern template std::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const size_t src_idx = 0); + +/** + * @brief calculate the point offset from source point index along the trajectory (or path) (points + * container) + * @param points points of trajectory, path, ... + * @param src_idx index of source point + * @param offset length of offset from source point + * @param throw_exception flag to enable/disable exception throwing + * @return offset point + */ +template +std::optional calcLongitudinalOffsetPoint( + const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + if (points.size() - 1 < src_idx) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); + autoware::universe_utils::print_backtrace(); + if (throw_exception) { + throw std::out_of_range(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return {}; + } + + if (points.size() == 1) { + return {}; + } + + if (src_idx + 1 == points.size() && offset == 0.0) { + return autoware::universe_utils::getPoint(points.at(src_idx)); + } + + if (offset < 0.0) { + auto reverse_points = points; + std::reverse(reverse_points.begin(), reverse_points.end()); + return calcLongitudinalOffsetPoint( + reverse_points, reverse_points.size() - src_idx - 1, -offset); + } + + double dist_sum = 0.0; + + for (size_t i = src_idx; i < points.size() - 1; ++i) { + const auto & p_front = points.at(i); + const auto & p_back = points.at(i + 1); + + 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 autoware::universe_utils::calcInterpolatedPoint( + p_back, p_front, std::abs(dist_res / dist_segment)); + } + } + + // not found (out of range) + return {}; +} + +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); + +/** + * @brief calculate the point offset from source point along the trajectory (or path) (points + * container) + * @param points points of trajectory, path, ... + * @param src_point source point + * @param offset length of offset from source point + * @return offset point + */ +template +std::optional calcLongitudinalOffsetPoint( + const T & points, const geometry_msgs::msg::Point & src_point, const double offset) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: %s", e.what()); + return {}; + } + + if (offset < 0.0) { + auto reverse_points = points; + std::reverse(reverse_points.begin(), reverse_points.end()); + return calcLongitudinalOffsetPoint(reverse_points, src_point, -offset); + } + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + + return calcLongitudinalOffsetPoint(points, src_seg_idx, offset + signed_length_src_offset); +} + +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); + +/** + * @brief calculate the point offset from source point index along the trajectory (or path) (points + * container) + * @param points points of trajectory, path, ... + * @param src_idx index of source point + * @param offset length of offset from source point + * @param set_orientation_from_position_direction set orientation by spherical interpolation if + * false + * @return offset pose + */ +template +std::optional calcLongitudinalOffsetPose( + const T & points, const size_t src_idx, const double offset, + const bool set_orientation_from_position_direction = true, const bool throw_exception = false) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: %s", e.what()); + return {}; + } + + if (points.size() - 1 < src_idx) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); + autoware::universe_utils::print_backtrace(); + if (throw_exception) { + throw std::out_of_range(error_message); + } + RCLCPP_DEBUG(get_logger(), "%s", error_message.c_str()); + return {}; + } + + if (points.size() == 1) { + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: points size is one."); + return {}; + } + + if (src_idx + 1 == points.size() && offset == 0.0) { + return autoware::universe_utils::getPose(points.at(src_idx)); + } + + if (offset < 0.0) { + auto reverse_points = points; + std::reverse(reverse_points.begin(), reverse_points.end()); + + double dist_sum = 0.0; + + for (size_t i = reverse_points.size() - src_idx - 1; i < reverse_points.size() - 1; ++i) { + const auto & p_front = reverse_points.at(i); + const auto & p_back = reverse_points.at(i + 1); + + 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 autoware::universe_utils::calcInterpolatedPose( + p_back, p_front, std::abs(dist_res / dist_segment), + set_orientation_from_position_direction); + } + } + } else { + double dist_sum = 0.0; + + for (size_t i = src_idx; i < points.size() - 1; ++i) { + const auto & p_front = points.at(i); + const auto & p_back = points.at(i + 1); + + 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 autoware::universe_utils::calcInterpolatedPose( + p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), + set_orientation_from_position_direction); + } + } + } + + // not found (out of range) + return {}; +} + +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); + +/** + * @brief calculate the point offset from source point along the trajectory (or path) (points + * container) + * @param points points of trajectory, path, ... + * @param src_point source point + * @param offset length of offset from source point + * @param set_orientation_from_position_direction set orientation by spherical interpolation if + * false + * @return offset pose + */ +template +std::optional calcLongitudinalOffsetPose( + const T & points, const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + + return calcLongitudinalOffsetPose( + points, src_seg_idx, offset + signed_length_src_offset, + set_orientation_from_position_direction); +} + +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); + +/** + * @brief insert a point in points container (trajectory, path, ...) using segment id + * @param seg_idx segment index of point at beginning of length + * @param p_target point to be inserted + * @param points output points of trajectory, path, ... + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of segment id, where point is inserted + */ +template +std::optional insertTargetPoint( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points, + const double overlap_threshold = 1e-3) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + // invalid segment index + if (seg_idx + 1 >= points.size()) { + return {}; + } + + 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); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + const auto overlap_with_front = + autoware::universe_utils::calcDistance2d(p_target, p_front) < overlap_threshold; + const auto overlap_with_back = + autoware::universe_utils::calcDistance2d(p_target, p_back) < overlap_threshold; + + const auto is_driving_forward = isDrivingForward(points); + if (!is_driving_forward) { + return {}; + } + + geometry_msgs::msg::Pose target_pose; + { + const auto p_base = is_driving_forward.value() ? p_back : p_front; + 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 = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); + } + + auto p_insert = points.at(seg_idx); + 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 = autoware::universe_utils::calcElevationAngle(p_base, p_target); + const auto yaw = autoware::universe_utils::calcAzimuthAngle(p_base, p_target); + + 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()) { + autoware::universe_utils::setPose(base_pose, points.at(seg_idx)); + } else { + autoware::universe_utils::setPose(base_pose, points.at(seg_idx + 1)); + } + points.insert(points.begin() + seg_idx + 1, p_insert); + return seg_idx + 1; + } + + if (overlap_with_back) { + return seg_idx + 1; + } + + return seg_idx; +} + +extern template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); + +/** + * @brief insert a point in points container (trajectory, path, ...) using length of point to be + * inserted + * @param insert_point_length length to insert point from the beginning of the points + * @param p_target point to be inserted + * @param points output points of trajectory, path, ... + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of segment id, where point is inserted + */ +template +std::optional insertTargetPoint( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, T & points, + const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points); + + if (insert_point_length < 0.0) { + return std::nullopt; + } + + // Get Nearest segment index + std::optional segment_idx = std::nullopt; + for (size_t i = 1; i < points.size(); ++i) { + // TODO(Mamoru Sobue): find accumulated sum beforehand + const double length = calcSignedArcLength(points, 0, i); + if (insert_point_length <= length) { + segment_idx = i - 1; + break; + } + } + + if (!segment_idx) { + return std::nullopt; + } + + return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); +} + +extern template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); + +/** + * @brief insert a point in points container (trajectory, path, ...) using segment index and length + * of point to be inserted + * @param src_segment_idx source segment index on the trajectory + * @param insert_point_length length to insert point from the beginning of the points + * @param points output points of trajectory, path, ... + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of insert point + */ +template +std::optional insertTargetPoint( + const size_t src_segment_idx, const double insert_point_length, T & points, + const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points); + + if (src_segment_idx >= points.size() - 1) { + return std::nullopt; + } + + // Get Nearest segment index + std::optional segment_idx = std::nullopt; + if (0.0 <= insert_point_length) { + for (size_t i = src_segment_idx + 1; i < points.size(); ++i) { + const double length = calcSignedArcLength(points, src_segment_idx, i); + if (insert_point_length <= length) { + segment_idx = i - 1; + break; + } + } + } else { + for (int i = src_segment_idx - 1; 0 <= i; --i) { + const double length = calcSignedArcLength(points, src_segment_idx, i); + if (length <= insert_point_length) { + segment_idx = i; + break; + } + } + } + + if (!segment_idx) { + return std::nullopt; + } + + // Get Target Point + const double segment_length = calcSignedArcLength(points, *segment_idx, *segment_idx + 1); + 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 = 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); +} + +extern template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); + +/** + * @brief Insert a target point from a source pose on the trajectory + * @param src_pose source pose on the trajectory + * @param insert_point_length length to insert point from the beginning of the points + * @param points output points of trajectory, path, ... + * @param max_dist max distance, used to search for nearest segment index in points container to the + * given source pose + * @param max_yaw max yaw, used to search for nearest segment index in points container to the given + * source pose + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of insert point + */ +template +std::optional insertTargetPoint( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, T & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points); + + if (insert_point_length < 0.0) { + return std::nullopt; + } + + const auto nearest_segment_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); + if (!nearest_segment_idx) { + return std::nullopt; + } + + const double offset_length = + calcLongitudinalOffsetToSegment(points, *nearest_segment_idx, src_pose.position); + + return insertTargetPoint( + *nearest_segment_idx, insert_point_length + offset_length, points, overlap_threshold); +} + +extern template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); + +/** + * @brief Insert stop point from the source segment index + * @param src_segment_idx start segment index on the trajectory + * @param distance_to_stop_point distance to stop point from the source index + * @param points_with_twist output points of trajectory, path, ... (with velocity) + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of stop point + */ +template +std::optional insertStopPoint( + const size_t src_segment_idx, const double distance_to_stop_point, T & points_with_twist, + const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points_with_twist); + + if (distance_to_stop_point < 0.0 || src_segment_idx >= points_with_twist.size() - 1) { + return std::nullopt; + } + + const auto stop_idx = insertTargetPoint( + src_segment_idx, distance_to_stop_point, points_with_twist, overlap_threshold); + if (!stop_idx) { + return std::nullopt; + } + + for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { + autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + } + + return stop_idx; +} + +extern template std::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + +/** + * @brief Insert stop point from the front point of the path + * @param distance_to_stop_point distance to stop point from the front point of the path + * @param points_with_twist output points of trajectory, path, ... (with velocity) + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of stop point + */ +template +std::optional insertStopPoint( + const double distance_to_stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points_with_twist); + + if (distance_to_stop_point < 0.0) { + return std::nullopt; + } + + double accumulated_length = 0; + for (size_t i = 0; i < points_with_twist.size() - 1; ++i) { + 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); + } + accumulated_length += length; + } + + return std::nullopt; +} + +extern template std::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + +/** + * @brief Insert Stop point from the source pose + * @param src_pose source pose + * @param distance_to_stop_point distance to stop point from the src point + * @param points_with_twist output points of trajectory, path, ... (with velocity) + * @param max_dist max distance, used to search for nearest segment index in points container to the + * given source pose + * @param max_yaw max yaw, used to search for nearest segment index in points container to the given + * source pose + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of stop point + */ +template +std::optional insertStopPoint( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + T & points_with_twist, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points_with_twist); + + if (distance_to_stop_point < 0.0) { + return std::nullopt; + } + + const auto stop_idx = insertTargetPoint( + src_pose, distance_to_stop_point, points_with_twist, max_dist, max_yaw, overlap_threshold); + + if (!stop_idx) { + return std::nullopt; + } + + for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { + autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + } + + return stop_idx; +} + +extern template std::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); + +/** + * @brief Insert Stop point that is in the stop segment index + * @param stop_seg_idx segment index of the stop pose + * @param stop_point stop point + * @param points_with_twist output points of trajectory, path, ... (with velocity) + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of stop point + */ +template +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 = 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) { + autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + } + + return insert_idx; +} + +extern template std::optional +insertStopPoint>( + const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + +/** + * @brief Insert deceleration point from the source pose + * @param src_point source point + * @param distance_to_decel_point distance to deceleration point from the src point + * @param velocity velocity of stop point + * @param points_with_twist output points of trajectory, path, ... (with velocity) + */ +template +std::optional insertDecelPoint( + const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, + const double velocity, T & points_with_twist) +{ + const auto decel_point = + calcLongitudinalOffsetPoint(points_with_twist, src_point, distance_to_decel_point); + + if (!decel_point) { + return {}; + } + + const auto seg_idx = findNearestSegmentIndex(points_with_twist, decel_point.value()); + const auto insert_idx = insertTargetPoint(seg_idx, decel_point.value(), points_with_twist); + + if (!insert_idx) { + return {}; + } + + for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { + const auto & original_velocity = + autoware::universe_utils::getLongitudinalVelocity(points_with_twist.at(i)); + autoware::universe_utils::setLongitudinalVelocity( + std::min(original_velocity, velocity), points_with_twist.at(i)); + } + + return insert_idx; +} + +extern template std::optional +insertDecelPoint>( + const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, + const double velocity, + std::vector & points_with_twist); + +/** + * @brief Insert orientation to each point in points container (trajectory, path, ...) + * @param points points of trajectory, path, ... (input / output) + * @param is_driving_forward flag indicating the order of points is forward or backward + */ +template +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 = 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 + 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 = 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 + autoware::universe_utils::setOrientation( + autoware::universe_utils::getPose(points.at(1)).orientation, points.at(0)); + } +} + +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +extern template void insertOrientation>( + std::vector & points, + const bool is_driving_forward); +extern template void insertOrientation>( + std::vector & points, + const bool is_driving_forward); + +/** + * @brief Remove points with invalid orientation differences from a given points container + * (trajectory, path, ...). Check the difference between the angles of two points and the difference + * between the azimuth angle between the two points and the angle of the next point. + * @param points Points of trajectory, path, or other point container (input / output) + * @param max_yaw_diff Maximum acceptable yaw angle difference between two consecutive points in + * radians (default: M_PI_2) + */ +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 = 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(autoware::universe_utils::normalizeRadian(yaw1 - yaw2)) || + !autoware::universe_utils::isDrivingForward(p1, p2)) { + points.erase(std::next(itr)); + return; + } else { + itr++; + } + } +} + +/** + * @brief calculate length of 2D distance between two points, specified by start point and end + * point with their segment indices in points container + * @param points points of trajectory, path, ... + * @param src_point start point + * @param src_seg_idx index of start point segment + * @param dst_point end point + * @param dst_seg_idx index of end point segment + * @return length of distance between two points. + * Length is positive if destination point is greater that source point (i.e. after it in + * trajectory, path, ...) and negative otherwise. + */ +template +double calcSignedArcLength( + const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx) +{ + validateNonEmpty(points); + + const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_seg_idx); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + const double signed_length_dst_offset = + calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + + return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; +} + +extern template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +/** + * @brief calculate length of 2D distance between two points, specified by start point and its + * segment index in points container and end point index in points container + * @param points points of trajectory, path, ... + * @param src_point start point + * @param src_seg_idx index of start point segment + * @param dst_idx index of end point + * @return length of distance between two points + * Length is positive if destination point associated to dst_idx is greater that source point (i.e. + * after it in trajectory, path, ...) and negative otherwise. + */ +template +double calcSignedArcLength( + const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const size_t dst_idx) +{ + validateNonEmpty(points); + + const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_idx); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + + return signed_length_on_traj - signed_length_src_offset; +} + +extern template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); + +/** + * @brief calculate length of 2D distance between two points, specified by start point index in + * points container and end point and its segment index in points container + * @param points points of trajectory, path, ... + * @param src_idx index of start point start point + * @param dst_point end point + * @param dst_seg_idx index of end point segment + * @return length of distance between two points + * Length is positive if destination point is greater that source point associated to src_idx (i.e. + * after it in trajectory, path, ...) and negative otherwise. + */ +template +double calcSignedArcLength( + const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, + const size_t dst_seg_idx) +{ + validateNonEmpty(points); + + const double signed_length_on_traj = calcSignedArcLength(points, src_idx, dst_seg_idx); + const double signed_length_dst_offset = + calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + + return signed_length_on_traj + signed_length_dst_offset; +} + +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +/** + * @brief find first nearest point index through points container for a given pose with soft + * distance and yaw constraints. Finding nearest point is determined by looping through the points + * container, and finding the nearest point to the given pose in terms of squared 2D distance and + * yaw deviation. The index of the point with minimum distance and yaw deviation comparing to the + * given pose will be returned. + * @param points points of trajectory, path, ... + * @param pose given pose + * @param dist_threshold distance threshold used for searching for first nearest index to given pose + * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose + * @return index of nearest point (index or none if not found) + */ +template +size_t findFirstNearestIndexWithSoftConstraints( + const T & points, const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()) +{ + validateNonEmpty(points); + + { // with dist and yaw thresholds + const double squared_dist_threshold = dist_threshold * dist_threshold; + double min_squared_dist = std::numeric_limits::max(); + size_t min_idx = 0; + bool is_within_constraints = false; + for (size_t i = 0; i < points.size(); ++i) { + const auto squared_dist = + 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) { + break; + } + continue; + } + + if (min_squared_dist <= squared_dist) { + continue; + } + + min_squared_dist = squared_dist; + min_idx = i; + is_within_constraints = true; + } + + // nearest index is found + if (is_within_constraints) { + return min_idx; + } + } + + { // with dist threshold + const double squared_dist_threshold = dist_threshold * dist_threshold; + double min_squared_dist = std::numeric_limits::max(); + size_t min_idx = 0; + bool is_within_constraints = false; + for (size_t i = 0; i < points.size(); ++i) { + const auto squared_dist = + autoware::universe_utils::calcSquaredDistance2d(points.at(i), pose.position); + + if (squared_dist_threshold < squared_dist) { + if (is_within_constraints) { + break; + } + continue; + } + + if (min_squared_dist <= squared_dist) { + continue; + } + + min_squared_dist = squared_dist; + min_idx = i; + is_within_constraints = true; + } + + // nearest index is found + if (is_within_constraints) { + return min_idx; + } + } + + // without any threshold + return findNearestIndex(points, pose.position); +} + +extern template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + +/** + * @brief find nearest segment index to pose with soft constraints + * Segment is straight path between two continuous points of trajectory + * When pose is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 + * @param points points of trajectory, path, .. + * @param pose pose to which to find nearest segment index + * @param dist_threshold distance threshold used for searching for first nearest index to given pose + * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose + * @return nearest index + */ +template +size_t findFirstNearestSegmentIndexWithSoftConstraints( + const T & points, const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()) +{ + // find first nearest index with soft constraints (not segment index) + const size_t nearest_idx = + findFirstNearestIndexWithSoftConstraints(points, pose, dist_threshold, yaw_threshold); + + // calculate segment index + if (nearest_idx == 0) { + return 0; + } + if (nearest_idx == points.size() - 1) { + return points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, pose.position); + + if (signed_length <= 0) { + return nearest_idx - 1; + } + + return nearest_idx; +} + +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + +/** + * @brief calculate the point offset from source point along the trajectory (or path) + * @brief calculate length of 2D distance between given pose and first point in container with zero + * longitudinal velocity + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @param pose given pose to start the distance calculation from + * @param max_dist max distance, used to search for nearest segment index in points container to the + * given pose + * @param max_yaw max yaw, used to search for nearest segment index in points container to the given + * pose + * @return Length of 2D distance between given pose and first point in container with zero + * longitudinal velocity + */ +template +std::optional calcDistanceToForwardStopPoint( + const T & points_with_twist, const geometry_msgs::msg::Pose & pose, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()) +{ + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "Failed to calculate stop distance %s", e.what()); + return {}; + } + + const auto nearest_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); + + if (!nearest_segment_idx) { + return std::nullopt; + } + + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex( + points_with_twist, *nearest_segment_idx + 1, points_with_twist.size()); + + if (!stop_idx) { + return std::nullopt; + } + + const auto closest_stop_dist = + calcSignedArcLength(points_with_twist, pose.position, *nearest_segment_idx, *stop_idx); + + return std::max(0.0, closest_stop_dist); +} + +extern template std::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + +// NOTE: Points after forward length from the point will be cropped +// forward_length is assumed to be positive. +template +T cropForwardPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length) +{ + if (points.empty()) { + return T{}; + } + + double sum_length = + -autoware::motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + 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}; + } + } + + return points; +} + +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); + +// NOTE: Points before backward length from the point will be cropped +// backward_length is assumed to be positive. +template +T cropBackwardPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length) +{ + if (points.empty()) { + return T{}; + } + + double sum_length = + -autoware::motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + for (int i = target_seg_idx; 0 < i; --i) { + 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()}; + } + } + + return points; +} + +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); + +template +T cropPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length) +{ + if (points.empty()) { + return T{}; + } + + // NOTE: Cropping forward must be done first in order to keep target_seg_idx. + const auto cropped_forward_points = + cropForwardPoints(points, target_pos, target_seg_idx, forward_length); + + const size_t modified_target_seg_idx = + std::min(target_seg_idx, cropped_forward_points.size() - 2); + const auto cropped_points = cropBackwardPoints( + cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); + + if (cropped_points.size() < 2) { + RCLCPP_DEBUG(get_logger(), "Return original points since cropped_points size is less than 2."); + return points; + } + + return cropped_points; +} + +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); + +/** + * @brief Calculate the angle of the input pose with respect to the nearest trajectory segment. + * The function gets the nearest segment index between the points of the trajectory and the given + * pose's position, then calculates the azimuth angle of that segment and compares it to the yaw of + * the input pose. The segment is a straight path between two continuous points of the trajectory. + * @param points Points of the trajectory, path, ... + * @param pose Input pose with position and orientation (yaw) + * @param throw_exception Flag to enable/disable exception throwing + * @return Angle with respect to the trajectory segment (signed) in radians + */ +template +double calcYawDeviation( + const T & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false) +{ + const auto overlap_removed_points = removeOverlapPoints(points, 0); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + } + + if (overlap_removed_points.size() <= 1) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + " Given points size is less than 2. Failed to calculate yaw deviation."); + autoware::universe_utils::print_backtrace(); + if (throw_exception) { + throw std::runtime_error(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return 0 since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return 0.0; + } + + const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, pose.position); + + 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 autoware::universe_utils::normalizeRadian(pose_yaw - path_yaw); +} + +extern template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); +extern template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); +extern template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); + +/** + * @brief Check if the given target point is in front of the based pose from the trajectory. + * if the points is empty, the function returns false + * @param points Points of the trajectory, path, ... + * @param base_point Base point + * @param target_point Target point + * @param threshold threshold for judging front point + * @return true if the target pose is in front of the base pose + */ +template +bool isTargetPointFront( + const T & points, const geometry_msgs::msg::Point & base_point, + const geometry_msgs::msg::Point & target_point, const double threshold = 0.0) +{ + if (points.empty()) { + return false; + } + + const double s_base = calcSignedArcLength(points, 0, base_point); + const double s_target = calcSignedArcLength(points, 0, target_point); + + return s_target - s_base > threshold; +} + +extern template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); +extern template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); +extern template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/vehicle/vehicle_state_checker.hpp similarity index 84% rename from common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/vehicle/vehicle_state_checker.hpp index 82433d8e3c241..f1d5677d61f70 100644 --- a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/vehicle/vehicle_state_checker.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ -#define MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ +#define AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ #include -#include +#include #include #include #include -namespace motion_utils +namespace autoware::motion_utils { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::Odometry; @@ -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/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml new file mode 100644 index 0000000000000..81fec78b04812 --- /dev/null +++ b/common/autoware_motion_utils/package.xml @@ -0,0 +1,45 @@ + + + + autoware_motion_utils + 0.1.0 + The autoware_motion_utils package + Satoshi Ota + Takayuki Murooka + + Fumiya Watanabe + Kosuke Takeuchi + Taiki Tanaka + Takamasa Horibe + Tomoya Kimura + Mamoru Sobue + Apache License 2.0 + + Takayuki Murooka + Satoshi Ota + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_planning_msgs + autoware_universe_utils + autoware_vehicle_msgs + builtin_interfaces + geometry_msgs + interpolation + libboost-dev + rclcpp + tf2 + tf2_geometry_msgs + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_motion_utils/src/distance/distance.cpp b/common/autoware_motion_utils/src/distance/distance.cpp new file mode 100644 index 0000000000000..d31e7ec709810 --- /dev/null +++ b/common/autoware_motion_utils/src/distance/distance.cpp @@ -0,0 +1,272 @@ +// 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 "autoware/motion_utils/distance/distance.hpp" + +namespace autoware::motion_utils +{ +namespace +{ +bool validCheckDecelPlan( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin) +{ + const double v_min = v_target - std::abs(v_margin); + const double v_max = v_target + std::abs(v_margin); + const double a_min = a_target - std::abs(a_margin); + const double a_max = a_target + std::abs(a_margin); + + if (v_end < v_min || v_max < v_end) { + return false; + } + if (a_end < a_min || a_max < a_end) { + return false; + } + + return true; +} + +/** + * @brief update traveling distance, velocity and acceleration under constant jerk. + * @param (x) current traveling distance [m/s] + * @param (v) current velocity [m/s] + * @param (a) current acceleration [m/ss] + * @param (j) target jerk [m/sss] + * @param (t) time [s] + * @return updated traveling distance, velocity and acceleration + */ +std::tuple update( + const double x, const double v, const double a, const double j, const double t) +{ + const double a_new = a + j * t; + const double v_new = v + a * t + 0.5 * j * t * t; + const double x_new = x + v * t + 0.5 * a * t * t + (1.0 / 6.0) * j * t * t * t; + + return {x_new, v_new, a_new}; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: TRAPEZOID + * ACCELERATION PROFILE). this type of profile has ZERO JERK time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * a0 * + * |* + * ----+-*-------------------*------> t + * | * * + * | * * + * | a1 *************** + * | + * + * [JERK PROFILE] + * j ^ + * | + * | ja **** + * | * + * ----+----***************---------> t + * | * + * | * + * jd ****** + * | + * + * @param (v0) current velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (vt) target velocity [m/s] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @param (t_during_min_acc) duration of constant deceleration [s] + * @return moving distance until velocity is reached vt [m] + */ +std::optional calcDecelDistPlanType1( + const double v0, const double vt, const double a0, const double am, const double ja, + const double jd, const double t_during_min_acc) +{ + constexpr double epsilon = 1e-3; + + // negative jerk time + const double j1 = am < a0 ? jd : ja; + const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; + const auto [x1, v1, a1] = update(0.0, v0, a0, j1, t1); + + // zero jerk time + const double t2 = epsilon < t_during_min_acc ? t_during_min_acc : 0.0; + const auto [x2, v2, a2] = update(x1, v1, a1, 0.0, t2); + + // positive jerk time + const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; + const auto [x3, v3, a3] = update(x2, v2, a2, ja, t3); + + const double a_target = 0.0; + const double v_margin = 0.3; // [m/s] + const double a_margin = 0.1; // [m/s^2] + + if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x3; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: TRIANGLE + * ACCELERATION PROFILE), This type of profile do NOT have ZERO JERK time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * a0 * + * |* + * ----+-*-----*--------------------> t + * | * * + * | * * + * | a1 * + * | + * + * [JERK PROFILE] + * j ^ + * | + * | ja **** + * | * + * ----+----*-----------------------> t + * | * + * | * + * jd ****** + * | + * + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + */ +std::optional calcDecelDistPlanType2( + const double v0, const double vt, const double a0, const double ja, const double jd) +{ + constexpr double epsilon = 1e-3; + + const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); + const double a1 = -std::sqrt(a1_square); + + // negative jerk time + const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; + const auto [x1, v1, no_use_a1] = update(0.0, v0, a0, jd, t1); + + // positive jerk time + const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; + const auto [x2, v2, a2] = update(x1, v1, a1, ja, t2); + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x2; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: LINEAR ACCELERATION + * PROFILE). This type of profile has only positive jerk time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * ----+----*-----------------------> t + * | * + * | * + * | * + * |* + * a0 * + * | + * + * [JERK PROFILE] + * j ^ + * | + * ja ****** + * | * + * | * + * ----+----*-----------------------> t + * | + * + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + */ +std::optional calcDecelDistPlanType3( + const double v0, const double vt, const double a0, const double ja) +{ + constexpr double epsilon = 1e-3; + + // positive jerk time + const double t_acc = (0.0 - a0) / ja; + const double t1 = epsilon < t_acc ? t_acc : 0.0; + const auto [x1, v1, a1] = update(0.0, v0, a0, ja, t1); + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x1; +} +} // namespace + +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) +{ + if (current_vel < target_vel) { + return {}; + } + + constexpr double epsilon = 1e-3; + const double jerk_before_min_acc = acc_min < current_acc ? jerk_dec : jerk_acc; + const double t_before_min_acc = (acc_min - current_acc) / jerk_before_min_acc; + const double jerk_after_min_acc = jerk_acc; + const double t_after_min_acc = (0.0 - acc_min) / jerk_after_min_acc; + + const double t_during_min_acc = + (target_vel - current_vel - current_acc * t_before_min_acc - + 0.5 * jerk_before_min_acc * std::pow(t_before_min_acc, 2) - acc_min * t_after_min_acc - + 0.5 * jerk_after_min_acc * std::pow(t_after_min_acc, 2)) / + acc_min; + + // check if it is possible to decelerate to the target velocity + // by simply bringing the current acceleration to zero. + const auto is_decel_needed = + 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; + + if (t_during_min_acc > epsilon) { + return calcDecelDistPlanType1( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_during_min_acc); + } + if (is_decel_needed || current_acc > epsilon) { + return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); + } + + return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); +} +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp new file mode 100644 index 0000000000000..e405cdb655c02 --- /dev/null +++ b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp @@ -0,0 +1,49 @@ +// 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. + +#include +#include + +#include +#include +#include + +namespace autoware::motion_utils +{ +template +void VelocityFactorInterface::set( + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail) +{ + const auto & curr_point = curr_pose.position; + const auto & stop_point = stop_pose.position; + velocity_factor_.behavior = behavior_; + velocity_factor_.pose = stop_pose; + velocity_factor_.distance = + static_cast(autoware::motion_utils::calcSignedArcLength(points, curr_point, stop_point)); + velocity_factor_.status = status; + velocity_factor_.detail = detail; +} + +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); + +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp new file mode 100644 index 0000000000000..388c7102b825c --- /dev/null +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -0,0 +1,143 @@ +// Copyright 2021 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/motion_utils/marker/marker_helper.hpp" + +#include "autoware/universe_utils/ros/marker_helper.hpp" + +#include + +#include + +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 +{ + +inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( + const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, + const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // Virtual Wall + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(0.1, 5.0, 2.0), color); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 1.0; + + marker_array.markers.push_back(marker); + } + + // Factor Text + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 2.0; + marker.text = module_name; + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( + const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // Virtual Wall + { + auto marker = createDeletedDefaultMarker(now, ns_prefix + "virtual_wall", id); + marker_array.markers.push_back(marker); + } + + // Factor Text + { + auto marker = createDeletedDefaultMarker(now, ns_prefix + "factor_text", id); + marker_array.markers.push_back(marker); + } + + return marker_array; +} +} // namespace + +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 = 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, + createMarkerColor(1.0, 0.0, 0.0, 0.5)); +} + +visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) +{ + 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, + createMarkerColor(1.0, 1.0, 0.0, 0.5)); +} + +visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) +{ + 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, + createMarkerColor(0.0, 1.0, 0.0, 0.5)); +} + +visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( + const rclcpp::Time & now, const int32_t id) +{ + return createDeletedVirtualWallMarkerArray("stop_", now, id); +} + +visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker( + const rclcpp::Time & now, const int32_t id) +{ + return createDeletedVirtualWallMarkerArray("slow_down_", now, id); +} + +visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( + const rclcpp::Time & now, const int32_t id) +{ + return createDeletedVirtualWallMarkerArray("dead_line_", now, id); +} +} // 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/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp new file mode 100644 index 0000000000000..baf1c534a8a00 --- /dev/null +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -0,0 +1,733 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "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" + +namespace autoware::motion_utils +{ +std::vector resamplePointVector( + const std::vector & points, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, + const bool use_lerp_for_z) +{ + // validate arguments + if (!resample_utils::validate_arguments(points, resampled_arclength)) { + return points; + } + + // Input Path Information + std::vector input_arclength; + std::vector x; + std::vector y; + std::vector z; + input_arclength.reserve(points.size()); + x.reserve(points.size()); + y.reserve(points.size()); + z.reserve(points.size()); + + input_arclength.push_back(0.0); + x.push_back(points.front().x); + y.push_back(points.front().y); + z.push_back(points.front().z); + 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 = 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); + z.push_back(curr_pt.z); + } + + // Interpolate + const auto lerp = [&](const auto & input) { + return interpolation::lerp(input_arclength, input, resampled_arclength); + }; + const auto spline = [&](const auto & input) { + return interpolation::spline(input_arclength, input, resampled_arclength); + }; + const auto spline_by_akima = [&](const auto & input) { + return interpolation::splineByAkima(input_arclength, input, resampled_arclength); + }; + + const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x); + const auto interpolated_y = use_akima_spline_for_xy ? lerp(y) : spline_by_akima(y); + const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z); + + std::vector resampled_points; + resampled_points.resize(interpolated_x.size()); + + // Insert Position + for (size_t i = 0; i < resampled_points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = interpolated_x.at(i); + point.y = interpolated_y.at(i); + point.z = interpolated_z.at(i); + resampled_points.at(i) = point; + } + + return resampled_points; +} + +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 = 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 << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; + return points; + } + + // Insert terminal point + if (input_length - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { + resampling_arclength.back() = input_length; + } else { + resampling_arclength.push_back(input_length); + } + + return resamplePointVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); +} + +std::vector resamplePoseVector( + const std::vector & points_raw, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, + const bool use_lerp_for_z) +{ + // Remove overlap points for resampling + const auto points = autoware::motion_utils::removeOverlapPoints(points_raw); + + // validate arguments + if (!resample_utils::validate_arguments(points, resampled_arclength)) { + return points_raw; + } + + std::vector position(points.size()); + for (size_t i = 0; i < points.size(); ++i) { + position.at(i) = points.at(i).position; + } + const auto resampled_position = + resamplePointVector(position, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); + + std::vector resampled_points(resampled_position.size()); + + // Insert Position + for (size_t i = 0; i < resampled_position.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position.x = resampled_position.at(i).x; + pose.position.y = resampled_position.at(i).y; + pose.position.z = resampled_position.at(i).z; + resampled_points.at(i) = pose; + } + + const bool 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 + if (!is_driving_forward && resampled_arclength.front() < 1e-3) { + resampled_points.at(0).orientation = points.at(0).orientation; + } + + return resampled_points; +} + +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 = 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 << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; + return points; + } + + // Insert terminal point + if (input_length - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { + resampling_arclength.back() = input_length; + } else { + resampling_arclength.push_back(input_length); + } + + return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); +} + +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, + const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) +{ + auto resampling_arclength = resampled_arclength; + + // Add resampling_arclength to insert input points which have multiple lane_ids + for (size_t i = 0; i < input_path.points.size(); ++i) { + if (input_path.points.at(i).lane_ids.size() < 2) { + continue; + } + + const double distance_to_resampling_point = calcSignedArcLength(input_path.points, 0, i); + for (size_t j = 1; j < resampling_arclength.size(); ++j) { + if ( + resampling_arclength.at(j - 1) <= distance_to_resampling_point && + distance_to_resampling_point < resampling_arclength.at(j)) { + const double dist_to_prev_point = + 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 < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(j - 1) = distance_to_resampling_point; + } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(j) = distance_to_resampling_point; + } else { + resampling_arclength.insert( + resampling_arclength.begin() + j, distance_to_resampling_point); + } + break; + } + } + } + + // validate arguments + if (!resample_utils::validate_arguments(input_path.points, resampling_arclength)) { + return input_path; + } + + // For LaneIds, is_final + // + // ------|----|----|----|----|----|----|-------> resampled + // [0] [1] [2] [3] [4] [5] [6] + // + // ------|----------------|----------|---------> base + // [0] [1] [2] + // + // resampled[0~3] = base[0] + // resampled[4~5] = base[1] + // resampled[6] = base[2] + + // Input Path Information + std::vector input_arclength; + std::vector input_pose; + std::vector v_lon; + std::vector v_lat; + std::vector heading_rate; + std::vector is_final; + std::vector> lane_ids; + input_arclength.reserve(input_path.points.size()); + input_pose.reserve(input_path.points.size()); + v_lon.reserve(input_path.points.size()); + v_lat.reserve(input_path.points.size()); + heading_rate.reserve(input_path.points.size()); + is_final.reserve(input_path.points.size()); + lane_ids.reserve(input_path.points.size()); + + input_arclength.push_back(0.0); + input_pose.push_back(input_path.points.front().point.pose); + v_lon.push_back(input_path.points.front().point.longitudinal_velocity_mps); + v_lat.push_back(input_path.points.front().point.lateral_velocity_mps); + heading_rate.push_back(input_path.points.front().point.heading_rate_rps); + is_final.push_back(input_path.points.front().point.is_final); + lane_ids.push_back(input_path.points.front().lane_ids); + for (size_t i = 1; i < input_path.points.size(); ++i) { + const auto & prev_pt = input_path.points.at(i - 1).point; + const auto & curr_pt = input_path.points.at(i).point; + const double ds = + 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); + v_lat.push_back(curr_pt.lateral_velocity_mps); + heading_rate.push_back(curr_pt.heading_rate_rps); + is_final.push_back(curr_pt.is_final); + lane_ids.push_back(input_path.points.at(i).lane_ids); + } + + if (input_arclength.back() < resampling_arclength.back()) { + std::cerr << "[autoware_motion_utils]: resampled path length is longer than input path length" + << std::endl; + return input_path; + } + + // Interpolate + const auto lerp = [&](const auto & input) { + return interpolation::lerp(input_arclength, input, resampling_arclength); + }; + + auto closest_segment_indices = + interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); + + const auto zoh = [&](const auto & input) { + return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + }; + + const auto interpolated_pose = + resamplePoseVector(input_pose, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); + const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); + const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); + const auto interpolated_heading_rate = lerp(heading_rate); + const auto interpolated_is_final = zoh(is_final); + + // interpolate lane_ids + std::vector> interpolated_lane_ids; + interpolated_lane_ids.resize(resampling_arclength.size()); + constexpr double epsilon = 1e-6; + for (size_t i = 0; i < resampling_arclength.size(); ++i) { + const size_t seg_idx = std::min(closest_segment_indices.at(i), input_path.points.size() - 2); + const auto & prev_lane_ids = input_path.points.at(seg_idx).lane_ids; + const auto & next_lane_ids = input_path.points.at(seg_idx + 1).lane_ids; + + if (std::abs(input_arclength.at(seg_idx) - resampling_arclength.at(i)) <= epsilon) { + interpolated_lane_ids.at(i).insert( + interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end()); + } else if (std::abs(input_arclength.at(seg_idx + 1) - resampling_arclength.at(i)) <= epsilon) { + interpolated_lane_ids.at(i).insert( + interpolated_lane_ids.at(i).end(), next_lane_ids.begin(), next_lane_ids.end()); + } else { + // extract lane_ids those prev_lane_ids and next_lane_ids have in common + for (const auto target_lane_id : prev_lane_ids) { + if ( + std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != + next_lane_ids.end()) { + interpolated_lane_ids.at(i).push_back(target_lane_id); + } + } + // If there are no common lane_ids, the prev_lane_ids is assigned. + if (interpolated_lane_ids.at(i).empty()) { + interpolated_lane_ids.at(i).insert( + interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end()); + } + } + } + + if (interpolated_pose.size() != resampling_arclength.size()) { + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; + return input_path; + } + + tier4_planning_msgs::msg::PathWithLaneId resampled_path; + resampled_path.header = input_path.header; + resampled_path.left_bound = input_path.left_bound; + resampled_path.right_bound = input_path.right_bound; + resampled_path.points.resize(interpolated_pose.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + autoware_planning_msgs::msg::PathPoint path_point; + path_point.pose = interpolated_pose.at(i); + path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); + path_point.lateral_velocity_mps = interpolated_v_lat.at(i); + path_point.heading_rate_rps = interpolated_heading_rate.at(i); + path_point.is_final = interpolated_is_final.at(i); + resampled_path.points.at(i).point = path_point; + resampled_path.points.at(i).lane_ids = interpolated_lane_ids.at(i); + } + + return resampled_path; +} + +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, + const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) +{ + // validate arguments + if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { + return input_path; + } + + // transform input_path + std::vector transformed_input_path( + input_path.points.size()); + for (size_t i = 0; i < input_path.points.size(); ++i) { + transformed_input_path.at(i) = input_path.points.at(i).point; + } + // compute path length + 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 << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; + return input_path; + } + + // Insert terminal point + 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); + } + + // Insert stop point + if (resample_input_path_stop_point) { + const auto distance_to_stop_point = + 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 ( + resampling_arclength.at(i - 1) <= *distance_to_stop_point && + *distance_to_stop_point < resampling_arclength.at(i)) { + const double dist_to_prev_point = + 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 < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(i - 1) = *distance_to_stop_point; + } 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); + } + break; + } + } + } + } + + return resamplePath( + input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, + use_zero_order_hold_for_v); +} + +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, + const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) +{ + // validate arguments + if (!resample_utils::validate_arguments(input_path.points, resampled_arclength)) { + return input_path; + } + + // Input Path Information + std::vector input_arclength; + std::vector input_pose; + std::vector v_lon; + std::vector v_lat; + std::vector heading_rate; + input_arclength.reserve(input_path.points.size()); + input_pose.reserve(input_path.points.size()); + v_lon.reserve(input_path.points.size()); + v_lat.reserve(input_path.points.size()); + heading_rate.reserve(input_path.points.size()); + + input_arclength.push_back(0.0); + input_pose.push_back(input_path.points.front().pose); + v_lon.push_back(input_path.points.front().longitudinal_velocity_mps); + v_lat.push_back(input_path.points.front().lateral_velocity_mps); + heading_rate.push_back(input_path.points.front().heading_rate_rps); + for (size_t i = 1; i < input_path.points.size(); ++i) { + const auto & prev_pt = input_path.points.at(i - 1); + const auto & curr_pt = input_path.points.at(i); + const double ds = + 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); + v_lat.push_back(curr_pt.lateral_velocity_mps); + heading_rate.push_back(curr_pt.heading_rate_rps); + } + + // Interpolate + const auto lerp = [&](const auto & input) { + return interpolation::lerp(input_arclength, input, resampled_arclength); + }; + + std::vector closest_segment_indices; + if (use_zero_order_hold_for_v) { + closest_segment_indices = + interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + } + const auto zoh = [&](const auto & input) { + return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + }; + + const auto interpolated_pose = + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); + const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); + const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); + const auto interpolated_heading_rate = lerp(heading_rate); + + if (interpolated_pose.size() != resampled_arclength.size()) { + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; + return input_path; + } + + autoware_planning_msgs::msg::Path resampled_path; + resampled_path.header = input_path.header; + resampled_path.left_bound = input_path.left_bound; + resampled_path.right_bound = input_path.right_bound; + resampled_path.points.resize(interpolated_pose.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + autoware_planning_msgs::msg::PathPoint path_point; + path_point.pose = interpolated_pose.at(i); + path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); + path_point.lateral_velocity_mps = interpolated_v_lat.at(i); + path_point.heading_rate_rps = interpolated_heading_rate.at(i); + resampled_path.points.at(i) = path_point; + } + + return resampled_path; +} + +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, + const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point) +{ + // validate arguments + if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { + return input_path; + } + + 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 << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; + return input_path; + } + + // Insert terminal point + 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); + } + + // Insert stop point + if (resample_input_path_stop_point) { + const auto distance_to_stop_point = + 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 ( + resampling_arclength.at(i - 1) <= *distance_to_stop_point && + *distance_to_stop_point < resampling_arclength.at(i)) { + const double dist_to_prev_point = + 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 < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(i - 1) = *distance_to_stop_point; + } 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); + } + break; + } + } + } + } + + return resamplePath( + input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, + use_zero_order_hold_for_twist); +} + +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, + const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist) +{ + // validate arguments + if (!resample_utils::validate_arguments(input_trajectory.points, resampled_arclength)) { + return input_trajectory; + } + + // Input Trajectory Information + std::vector input_arclength; + std::vector input_pose; + std::vector v_lon; + std::vector v_lat; + std::vector heading_rate; + std::vector acceleration; + std::vector front_wheel_angle; + std::vector rear_wheel_angle; + std::vector time_from_start; + input_arclength.reserve(input_trajectory.points.size()); + input_pose.reserve(input_trajectory.points.size()); + v_lon.reserve(input_trajectory.points.size()); + v_lat.reserve(input_trajectory.points.size()); + heading_rate.reserve(input_trajectory.points.size()); + acceleration.reserve(input_trajectory.points.size()); + front_wheel_angle.reserve(input_trajectory.points.size()); + rear_wheel_angle.reserve(input_trajectory.points.size()); + time_from_start.reserve(input_trajectory.points.size()); + + input_arclength.push_back(0.0); + input_pose.push_back(input_trajectory.points.front().pose); + v_lon.push_back(input_trajectory.points.front().longitudinal_velocity_mps); + v_lat.push_back(input_trajectory.points.front().lateral_velocity_mps); + heading_rate.push_back(input_trajectory.points.front().heading_rate_rps); + acceleration.push_back(input_trajectory.points.front().acceleration_mps2); + front_wheel_angle.push_back(input_trajectory.points.front().front_wheel_angle_rad); + rear_wheel_angle.push_back(input_trajectory.points.front().rear_wheel_angle_rad); + time_from_start.push_back( + rclcpp::Duration(input_trajectory.points.front().time_from_start).seconds()); + for (size_t i = 1; i < input_trajectory.points.size(); ++i) { + const auto & prev_pt = input_trajectory.points.at(i - 1); + const auto & curr_pt = input_trajectory.points.at(i); + const double ds = + 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); + v_lat.push_back(curr_pt.lateral_velocity_mps); + heading_rate.push_back(curr_pt.heading_rate_rps); + acceleration.push_back(curr_pt.acceleration_mps2); + front_wheel_angle.push_back(curr_pt.front_wheel_angle_rad); + rear_wheel_angle.push_back(curr_pt.rear_wheel_angle_rad); + time_from_start.push_back(rclcpp::Duration(curr_pt.time_from_start).seconds()); + } + + // Interpolate + const auto lerp = [&](const auto & input) { + return interpolation::lerp(input_arclength, input, resampled_arclength); + }; + + std::vector closest_segment_indices; + if (use_zero_order_hold_for_twist) { + closest_segment_indices = + interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + } + const auto zoh = [&](const auto & input) { + return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + }; + + const auto interpolated_pose = + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); + const auto interpolated_v_lon = use_zero_order_hold_for_twist ? zoh(v_lon) : lerp(v_lon); + const auto interpolated_v_lat = use_zero_order_hold_for_twist ? zoh(v_lat) : lerp(v_lat); + const auto interpolated_heading_rate = lerp(heading_rate); + const auto interpolated_acceleration = + use_zero_order_hold_for_twist ? zoh(acceleration) : lerp(acceleration); + const auto interpolated_front_wheel_angle = lerp(front_wheel_angle); + const auto interpolated_rear_wheel_angle = lerp(rear_wheel_angle); + const auto interpolated_time_from_start = lerp(time_from_start); + + if (interpolated_pose.size() != resampled_arclength.size()) { + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; + return input_trajectory; + } + + autoware_planning_msgs::msg::Trajectory resampled_trajectory; + resampled_trajectory.header = input_trajectory.header; + resampled_trajectory.points.resize(interpolated_pose.size()); + for (size_t i = 0; i < resampled_trajectory.points.size(); ++i) { + autoware_planning_msgs::msg::TrajectoryPoint traj_point; + traj_point.pose = interpolated_pose.at(i); + traj_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); + traj_point.lateral_velocity_mps = interpolated_v_lat.at(i); + traj_point.heading_rate_rps = interpolated_heading_rate.at(i); + traj_point.acceleration_mps2 = interpolated_acceleration.at(i); + traj_point.front_wheel_angle_rad = interpolated_front_wheel_angle.at(i); + traj_point.rear_wheel_angle_rad = interpolated_rear_wheel_angle.at(i); + traj_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time_from_start.at(i)); + resampled_trajectory.points.at(i) = traj_point; + } + + return resampled_trajectory; +} + +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, + const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point) +{ + // validate arguments + if (!resample_utils::validate_arguments(input_trajectory.points, resample_interval)) { + return input_trajectory; + } + + 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 << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; + return input_trajectory; + } + + // Insert terminal point + 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); + } + + // Insert stop point + if (resample_input_trajectory_stop_point) { + const auto distance_to_stop_point = + 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 ( + resampling_arclength.at(i - 1) <= *distance_to_stop_point && + *distance_to_stop_point < resampling_arclength.at(i)) { + const double dist_to_prev_point = + 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 < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(i - 1) = *distance_to_stop_point; + } 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); + } + break; + } + } + } + } + + return resampleTrajectory( + input_trajectory, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, + use_zero_order_hold_for_twist); +} + +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/conversion.cpp b/common/autoware_motion_utils/src/trajectory/conversion.cpp new file mode 100644 index 0000000000000..368d3e0fbbb75 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory/conversion.cpp @@ -0,0 +1,54 @@ +// 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 "autoware/motion_utils/trajectory/conversion.hpp" + +#include + +namespace autoware::motion_utils +{ +/** + * @brief Convert std::vector to + * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, + const std_msgs::msg::Header & header) +{ + autoware_planning_msgs::msg::Trajectory output{}; + output.header = header; + for (const auto & pt : trajectory) output.points.push_back(pt); + return output; +} + +/** + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory) +{ + std::vector output(trajectory.points.size()); + std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); + return output; +} + +} // 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 79% rename from common/motion_utils/src/trajectory/interpolation.cpp rename to common/autoware_motion_utils/src/trajectory/interpolation.cpp index 6ee3e665f746a..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_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +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 75% 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 c7e85554dbddd..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,20 +12,20 @@ // 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( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) { size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error @@ -56,8 +56,8 @@ std::optional> getPathIndexRangeWithLaneId( } size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id) { const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); if (opt_range) { @@ -66,7 +66,7 @@ size_t findNearestIndexFromLaneId( validateNonEmpty(path.points); - const auto sub_points = std::vector{ + const auto sub_points = std::vector{ path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; validateNonEmpty(sub_points); @@ -77,8 +77,8 @@ size_t findNearestIndexFromLaneId( } size_t findNearestSegmentIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id) { const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); @@ -99,8 +99,8 @@ size_t findNearestSegmentIndexFromLaneId( } // NOTE: rear_to_cog is supposed to be positive -autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation) { auto cog_path = path; @@ -116,15 +116,15 @@ autoware_auto_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 @@ autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( return cog_path; } -} // namespace motion_utils +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp new file mode 100644 index 0000000000000..5d536c0772fea --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -0,0 +1,602 @@ +// 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 "autoware/motion_utils/trajectory/trajectory.hpp" + +namespace autoware::motion_utils +{ + +// +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); + +// +template std::optional isDrivingForward>( + const std::vector &); +template std::optional +isDrivingForward>( + const std::vector &); +template std::optional +isDrivingForward>( + const std::vector &); + +// +template std::optional +isDrivingForwardWithTwist>( + const std::vector &); +template std::optional +isDrivingForwardWithTwist>( + const std::vector &); +template std::optional +isDrivingForwardWithTwist>( + const std::vector &); + +// +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); + +// +template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx, const size_t dst_idx); + +// +template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx); + +// +template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist); + +// +template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +// +template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); + +// +template size_t findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +// +template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); + +// +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); + +// +template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); + +// +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); + +// +template std::vector calcCurvature>( + const std::vector & points); +template std::vector +calcCurvature>( + const std::vector & points); +template std::vector +calcCurvature>( + const std::vector & points); + +// +template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); + +// +template std::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const size_t src_idx); + +// +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); + +// +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); + +// +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); + +// +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); + +// +template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, const double overlap_threshold); +template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); + +// +template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, const double overlap_threshold); +template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); + +// +template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, const double overlap_threshold); +template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold); +template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold); + +// +template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); +template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); +template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); + +// +template std::optional insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +template std::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +template std::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + +// +template std::optional insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); +template std::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); +template std::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); + +// +template std::optional insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, const double max_dist, + const double max_yaw, const double overlap_threshold); +template std::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist, const double max_yaw, const double overlap_threshold); +template std::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist, const double max_yaw, const double overlap_threshold); + +// +template std::optional +insertStopPoint>( + const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, + std::vector & points_with_twist, + const double overlap_threshold); + +// +template std::optional +insertDecelPoint>( + const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, + const double velocity, + std::vector & points_with_twist); + +// +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +template void insertOrientation>( + std::vector & points, + const bool is_driving_forward); +template void insertOrientation>( + std::vector & points, + const bool is_driving_forward); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +// +template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); + +// +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); + +// +template std::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); + +// +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); + +// +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); + +// +template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); +template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); +template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); + +// +template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); +template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); +template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); + +} // namespace autoware::motion_utils diff --git a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp b/common/autoware_motion_utils/src/vehicle/vehicle_state_checker.cpp similarity index 88% rename from common/motion_utils/src/vehicle/vehicle_state_checker.cpp rename to common/autoware_motion_utils/src/vehicle/vehicle_state_checker.cpp index 900a652033f28..e2a4754a5653e 100644 --- a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp +++ b/common/autoware_motion_utils/src/vehicle/vehicle_state_checker.cpp @@ -12,13 +12,13 @@ // 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 "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include -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 96% rename from common/motion_utils/test/src/resample/test_resample.cpp rename to common/autoware_motion_utils/test/src/resample/test_resample.cpp index 373a12c5bbd76..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,15 +27,15 @@ namespace { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -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; +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_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -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); @@ -297,7 +297,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -403,7 +403,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Duplicated points in the original path { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(11); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -437,7 +437,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -470,7 +470,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -504,7 +504,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -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; { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + 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); @@ -651,7 +651,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) // change initial orientation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -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,11 +771,11 @@ 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 { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -852,7 +852,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -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); @@ -931,7 +931,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp v { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -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); @@ -1626,7 +1626,7 @@ TEST(resample_path, resample_path_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -1708,7 +1708,7 @@ TEST(resample_path, resample_path_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = @@ -1742,7 +1742,7 @@ TEST(resample_path, resample_path_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -1776,7 +1776,7 @@ TEST(resample_path, resample_path_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -1812,10 +1812,10 @@ 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_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); @@ -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); @@ -1896,13 +1896,13 @@ TEST(resample_path, resample_path_by_vector_backward) // change initial orientation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { 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,11 +1990,11 @@ 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 { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2053,7 +2053,7 @@ TEST(resample_path, resample_path_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -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); @@ -2115,7 +2115,7 @@ TEST(resample_path, resample_path_by_vector_non_default) // Lerp v { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2175,11 +2175,11 @@ 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 { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -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); @@ -2241,7 +2241,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2269,7 +2269,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without stop point but with terminal point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2314,14 +2314,14 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without stop point but with terminal point (Boundary Condition) { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); } 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); @@ -2360,7 +2360,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case with duplicated zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2390,7 +2390,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case with zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2532,7 +2532,7 @@ TEST(resample_path, resample_path_by_same_interval) { // Input path size is not enough for resample { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = @@ -2565,7 +2565,7 @@ TEST(resample_path, resample_path_by_same_interval) // Resample interval is invalid { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -2598,7 +2598,7 @@ TEST(resample_path, resample_path_by_same_interval) // Resample interval is invalid (Negative value) { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { 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); @@ -2698,7 +2698,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2787,7 +2787,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = @@ -2821,7 +2821,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2855,7 +2855,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2891,11 +2891,11 @@ 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 { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2959,7 +2959,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = generateTestTrajectoryPoint( @@ -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); @@ -3025,7 +3025,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) // Lerp twist { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3090,11 +3090,11 @@ 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 { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -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); @@ -3160,7 +3160,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3190,7 +3190,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without stop point but with terminal point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3238,7 +3238,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without stop point but with terminal point (Boundary Condition) { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -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); @@ -3287,7 +3287,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case with duplicated zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3319,7 +3319,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case with zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3470,7 +3470,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) { // Input path size is not enough for resample { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = @@ -3503,7 +3503,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Resample interval is invalid { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3536,7 +3536,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Resample interval is invalid (Negative value) { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { 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 89% rename from common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp rename to common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp index 549ca4c0ae5bb..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_auto_planning_msgs::msg::Trajectory; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware_planning_msgs::msg::Trajectory; 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 96% rename from common/motion_utils/test/src/trajectory/test_interpolation.cpp rename to common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp index 5794ed61e9e44..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,13 +27,13 @@ namespace { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -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; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -124,10 +124,10 @@ T generateTestPath( TEST(Interpolation, interpolate_path_for_trajectory) { - using motion_utils::calcInterpolatedPoint; + using autoware::motion_utils::calcInterpolatedPoint; { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -319,7 +319,7 @@ TEST(Interpolation, interpolate_path_for_trajectory) // Duplicated Points { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -348,10 +348,10 @@ TEST(Interpolation, interpolate_path_for_trajectory) TEST(Interpolation, interpolate_path_for_path) { - using motion_utils::calcInterpolatedPoint; + using autoware::motion_utils::calcInterpolatedPoint; { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -515,7 +515,7 @@ TEST(Interpolation, interpolate_path_for_path) // Duplicated Points { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -540,10 +540,10 @@ TEST(Interpolation, interpolate_path_for_path) TEST(Interpolation, interpolate_points_with_length) { - using motion_utils::calcInterpolatedPose; + using autoware::motion_utils::calcInterpolatedPose; { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -631,7 +631,7 @@ TEST(Interpolation, interpolate_points_with_length) // one point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = generateTestTrajectoryPoint( 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 89% 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 a65147a65b12d..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,16 +22,16 @@ namespace { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using tier4_autoware_utils::createPoint; +using autoware::universe_utils::createPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) { 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,8 +54,8 @@ PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { - using autoware_auto_planning_msgs::msg::PathWithLaneId; - 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 94% rename from common/motion_utils/test/src/trajectory/test_trajectory.cpp rename to common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp index eb6a06041e65d..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 @@ -26,11 +26,11 @@ namespace { -using autoware_auto_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_planning_msgs::msg::Trajectory; +using TrajectoryPointArray = std::vector; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware::universe_utils::transformPoint; constexpr double epsilon = 1e-6; @@ -69,7 +69,7 @@ TrajectoryPointArray generateTestTrajectoryPointArray( const size_t num_points, const double point_interval, const double vel = 0.0, const double init_theta = 0.0, const double delta_theta = 0.0) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; TrajectoryPointArray traj; for (size_t i = 0; i < num_points; ++i) { const double theta = init_theta + i * delta_theta; @@ -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_auto_planning_msgs::msg::TrajectoryPoint; - using motion_utils::validateNonSharpAngle; + using autoware::motion_utils::validateNonSharpAngle; + using autoware_planning_msgs::msg::TrajectoryPoint; 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_auto_planning_msgs::msg::TrajectoryPoint; - using motion_utils::validateNonSharpAngle; - using tier4_autoware_utils::pi; + using autoware::motion_utils::validateNonSharpAngle; + using autoware::universe_utils::pi; + using autoware_planning_msgs::msg::TrajectoryPoint; 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_auto_planning_msgs::msg::TrajectoryPoint; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using tier4_autoware_utils::deg2rad; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::universe_utils::deg2rad; + using autoware_planning_msgs::msg::TrajectoryPoint; Trajectory traj{}; @@ -1385,10 +1385,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using tier4_autoware_utils::deg2rad; + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::universe_utils::deg2rad; + using autoware_planning_msgs::msg::TrajectoryPoint; 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_auto_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; + 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; Trajectory traj{}; @@ -1637,12 +1637,12 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) { - using autoware_auto_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; + 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; 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); @@ -1876,40 +1876,34 @@ TEST(trajectory, insertTargetPoint) // Invalid target point(In front of begin point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(-1.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(10.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(4.0, 10.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -1934,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); @@ -1992,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); @@ -2085,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); @@ -2304,13 +2298,11 @@ TEST(trajectory, insertTargetPoint_Length) // Invalid target point(Huge lateral offset) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(4.0, 10.0, 0.0); const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -2325,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); @@ -2511,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); @@ -2741,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); @@ -2928,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); @@ -3280,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); @@ -3535,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); @@ -3732,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); @@ -4118,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); @@ -4310,40 +4302,34 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) // Invalid target point(In front of begin point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(-1.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(10.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(4.0, 10.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4368,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); @@ -4458,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); @@ -4984,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); @@ -5054,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); @@ -5133,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); @@ -5260,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, removeInvalidOrientationPoints) +TEST(Trajectory, removeFirstInvalidOrientationPoints) { - using motion_utils::insertOrientation; - using motion_utils::removeInvalidOrientationPoints; + using autoware::motion_utils::insertOrientation; + using autoware::motion_utils::removeFirstInvalidOrientationPoints; const double max_yaw_diff = M_PI_2; @@ -5365,13 +5351,13 @@ TEST(Trajectory, removeInvalidOrientationPoints) auto modified_traj = traj; insertOrientation(modified_traj.points, true); modifyTrajectory(modified_traj); - removeInvalidOrientationPoints(modified_traj.points, max_yaw_diff); + removeFirstInvalidOrientationPoints(modified_traj.points, max_yaw_diff); EXPECT_EQ(modified_traj.points.size(), expected_size); for (size_t i = 0; i < modified_traj.points.size() - 1; ++i) { 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); } }; @@ -5379,8 +5365,7 @@ TEST(Trajectory, removeInvalidOrientationPoints) 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( @@ -5388,7 +5373,7 @@ TEST(Trajectory, removeInvalidOrientationPoints) [&](Trajectory & t) { auto invalid_point = t.points.back(); invalid_point.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4)); t.points.push_back(invalid_point); }, traj.points.size()); @@ -5399,27 +5384,16 @@ TEST(Trajectory, removeInvalidOrientationPoints) [&](Trajectory & t) { auto invalid_point = t.points[4]; invalid_point.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4)); t.points.insert(t.points.begin() + 4, invalid_point); }, traj.points.size()); - - // invalid point at the beginning - testRemoveInvalidOrientationPoints( - traj, - [&](Trajectory & t) { - auto invalid_point = t.points.front(); - invalid_point.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2)); - t.points.insert(t.points.begin(), invalid_point); - }, - 1); // expected size is 1 since only the first point remains } TEST(trajectory, calcYawDeviation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using motion_utils::calcYawDeviation; + using autoware::motion_utils::calcYawDeviation; + using autoware_planning_msgs::msg::TrajectoryPoint; constexpr double tolerance = 1e-3; @@ -5445,9 +5419,9 @@ TEST(trajectory, calcYawDeviation) TEST(trajectory, isTargetPointFront) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using motion_utils::isTargetPointFront; - using tier4_autoware_utils::createPoint; + using autoware::motion_utils::isTargetPointFront; + using autoware::universe_utils::createPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; // 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 88% 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 index aa26a64cb87ce..29802e70bfd5f 100644 --- 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 @@ -15,11 +15,11 @@ #ifndef VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ #define VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ -#include -#include +#include +#include -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; inline Trajectory generateTrajectoryWithStopPoint(const geometry_msgs::msg::Pose & goal_pose) { diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt index aafc62fc90d25..95224b9967d4b 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt @@ -68,7 +68,7 @@ pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) ament_target_dependencies(${PROJECT_NAME} PUBLIC rviz_common rviz_rendering - autoware_auto_vehicle_msgs + autoware_vehicle_msgs tier4_planning_msgs autoware_perception_msgs ) diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md index 0d0def1a46997..943d566ad109c 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md @@ -15,13 +15,13 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t | Name | Type | Description | | ------------------------------------------------------- | ------------------------------------------------------- | ------------------------------------ | -| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle velocity | -| `/vehicle/status/turn_indicators_status` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | -| `/vehicle/status/hazard_status` | `autoware_auto_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | -| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic is status of gear | +| `/vehicle/status/velocity_status` | `autoware_vehicle_msgs::msg::VelocityReport` | The topic is vehicle velocity | +| `/vehicle/status/turn_indicators_status` | `autoware_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/hazard_status` | `autoware_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | +| `/vehicle/status/steering_status` | `autoware_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/vehicle/status/gear_status` | `autoware_vehicle_msgs::msg::GearReport` | The topic is status of gear | | `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | The topic is velocity limit | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | The topic is status of traffic light | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | The topic is status of traffic light | ## Parameter diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp index 3fe473d5d5123..cb7e49685d0b3 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" +#include "autoware_vehicle_msgs/msg/gear_report.hpp" #include #include @@ -37,7 +37,7 @@ class GearDisplay public: GearDisplay(); void drawGearIndicator(QPainter & painter, const QRectF & backgroundRect); - void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); private: int current_gear_; // Internal variable to store current gear diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp index ec1acb9a5dc68..0831ded468c99 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -95,29 +95,29 @@ private Q_SLOTS: std::unique_ptr traffic_display_; std::unique_ptr speed_limit_display_; - rclcpp::Subscription::SharedPtr gear_sub_; - rclcpp::Subscription::SharedPtr steering_sub_; - rclcpp::Subscription::SharedPtr speed_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr gear_sub_; + rclcpp::Subscription::SharedPtr steering_sub_; + rclcpp::Subscription::SharedPtr speed_sub_; + rclcpp::Subscription::SharedPtr turn_signals_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr hazard_lights_sub_; - rclcpp::Subscription::SharedPtr traffic_sub_; + rclcpp::Subscription::SharedPtr + traffic_sub_; rclcpp::Subscription::SharedPtr speed_limit_sub_; std::mutex property_mutex_; - void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); - void updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); void updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); void updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); void drawWidget(QImage & hud); }; } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp index 0669028dba3b2..2ae8e9a3fe0b9 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include #include @@ -37,7 +37,7 @@ class SpeedDisplay public: SpeedDisplay(); void drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: float current_speed_; // Internal variable to store current speed diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp index b59f4acf380db..fcdb293fe8c72 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include #include @@ -39,7 +39,7 @@ class SpeedLimitDisplay SpeedLimitDisplay(); void drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: float current_limit; // Internal variable to store current gear diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp index 121ee9a0a4d84..dada3cddab911 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include #include @@ -37,8 +37,7 @@ class SteeringWheelDisplay public: SteeringWheelDisplay(); void drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect); - void updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); private: float steering_angle_ = 0.0f; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp index efa30f37ccb3e..fd15f542021f1 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp @@ -23,8 +23,9 @@ #include #include -#include -#include +#include +#include +#include #include #include @@ -39,8 +40,8 @@ class TrafficDisplay TrafficDisplay(); void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg); - autoware_perception_msgs::msg::TrafficSignal current_traffic_; + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg); + autoware_perception_msgs::msg::TrafficLightGroupArray current_traffic_; private: QImage traffic_light_image_; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp index ca10c92c06a3e..022de6d8c22c9 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp @@ -23,8 +23,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -41,9 +41,9 @@ class TurnSignalsDisplay TurnSignalsDisplay(); void drawArrows(QPainter & painter, const QRectF & backgroundRect, const QColor & color); void updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); void updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); private: QImage arrowImage; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml index 4d2f53e1e27ed..73e0dbea0866e 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml @@ -11,8 +11,8 @@ BSD-3-Clause - autoware_auto_vehicle_msgs autoware_perception_msgs + autoware_vehicle_msgs boost rviz_common rviz_ogre_vendor diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp index 825c7c1620e2c..956172bef6ed6 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp @@ -49,8 +49,7 @@ GearDisplay::GearDisplay() : current_gear_(0) } } -void GearDisplay::updateGearData( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +void GearDisplay::updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) { current_gear_ = msg->report; // Assuming msg->report contains the gear information } @@ -60,19 +59,19 @@ void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroun // we deal with the different gears here std::string gearString; switch (current_gear_) { - case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: + case autoware_vehicle_msgs::msg::GearReport::NEUTRAL: gearString = "N"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::LOW: - case autoware_auto_vehicle_msgs::msg::GearReport::LOW_2: + case autoware_vehicle_msgs::msg::GearReport::LOW: + case autoware_vehicle_msgs::msg::GearReport::LOW_2: gearString = "L"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::NONE: - case autoware_auto_vehicle_msgs::msg::GearReport::PARK: + case autoware_vehicle_msgs::msg::GearReport::NONE: + case autoware_vehicle_msgs::msg::GearReport::PARK: gearString = "P"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: - case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE_2: + case autoware_vehicle_msgs::msg::GearReport::REVERSE: + case autoware_vehicle_msgs::msg::GearReport::REVERSE_2: gearString = "R"; break; // all the drive gears from DRIVE to DRIVE_16 diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp index 9add6336ece46..ccfdaa69f5d2a 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -76,31 +76,29 @@ void SignalDisplay::onInitialize() auto rviz_ros_node = context_->getRosNodeAbstraction(); gear_topic_property_ = std::make_unique( - "Gear Topic Test", "/vehicle/status/gear_status", "autoware_auto_vehicle_msgs/msg/GearReport", + "Gear Topic Test", "/vehicle/status/gear_status", "autoware_vehicle_msgs/msg/GearReport", "Topic for Gear Data", this, SLOT(topic_updated_gear())); gear_topic_property_->initialize(rviz_ros_node); turn_signals_topic_property_ = std::make_unique( "Turn Signals Topic", "/vehicle/status/turn_indicators_status", - "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, + "autoware_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, SLOT(topic_updated_turn_signals())); turn_signals_topic_property_->initialize(rviz_ros_node); speed_topic_property_ = std::make_unique( - "Speed Topic", "/vehicle/status/velocity_status", - "autoware_auto_vehicle_msgs/msg/VelocityReport", "Topic for Speed Data", this, - SLOT(topic_updated_speed())); + "Speed Topic", "/vehicle/status/velocity_status", "autoware_vehicle_msgs/msg/VelocityReport", + "Topic for Speed Data", this, SLOT(topic_updated_speed())); speed_topic_property_->initialize(rviz_ros_node); steering_topic_property_ = std::make_unique( - "Steering Topic", "/vehicle/status/steering_status", - "autoware_auto_vehicle_msgs/msg/SteeringReport", "Topic for Steering Data", this, - SLOT(topic_updated_steering())); + "Steering Topic", "/vehicle/status/steering_status", "autoware_vehicle_msgs/msg/SteeringReport", + "Topic for Steering Data", this, SLOT(topic_updated_steering())); steering_topic_property_->initialize(rviz_ros_node); hazard_lights_topic_property_ = std::make_unique( "Hazard Lights Topic", "/vehicle/status/hazard_lights_status", - "autoware_auto_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, + "autoware_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, SLOT(topic_updated_hazard_lights())); hazard_lights_topic_property_->initialize(rviz_ros_node); @@ -111,10 +109,9 @@ void SignalDisplay::onInitialize() speed_limit_topic_property_->initialize(rviz_ros_node); traffic_topic_property_ = std::make_unique( - "Traffic Topic", - "/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal", - "autoware_perception/msgs/msg/TrafficSignal", "Topic for Traffic Light Data", this, - SLOT(topic_updated_traffic())); + "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals", + "autoware_perception_msgs/msgs/msg/TrafficLightGroupArray", "Topic for Traffic Light Data", + this, SLOT(topic_updated_traffic())); traffic_topic_property_->initialize(rviz_ros_node); } @@ -192,7 +189,7 @@ void SignalDisplay::onDisable() } void SignalDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) { std::lock_guard lock(property_mutex_); @@ -213,7 +210,7 @@ void SignalDisplay::updateSpeedLimitData( } void SignalDisplay::updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -224,7 +221,7 @@ void SignalDisplay::updateHazardLightsData( } void SignalDisplay::updateGearData( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -235,7 +232,7 @@ void SignalDisplay::updateGearData( } void SignalDisplay::updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -246,7 +243,7 @@ void SignalDisplay::updateSteeringData( } void SignalDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -258,7 +255,7 @@ void SignalDisplay::updateSpeedData( } void SignalDisplay::updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -372,11 +369,10 @@ void SignalDisplay::topic_updated_gear() gear_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); gear_sub_ = - rviz_ros_node->get_raw_node()->create_subscription( - gear_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { - updateGearData(msg); - }); + rviz_ros_node->get_raw_node()->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::GearReport::SharedPtr msg) { updateGearData(msg); }); } void SignalDisplay::topic_updated_steering() @@ -384,12 +380,13 @@ void SignalDisplay::topic_updated_steering() // resubscribe to the topic steering_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - steering_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - steering_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { - updateSteeringData(msg); - }); + steering_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); } void SignalDisplay::topic_updated_speed() @@ -397,12 +394,13 @@ void SignalDisplay::topic_updated_speed() // resubscribe to the topic speed_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - speed_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - speed_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { - updateSpeedData(msg); - }); + speed_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); } void SignalDisplay::topic_updated_speed_limit() @@ -427,9 +425,10 @@ void SignalDisplay::topic_updated_turn_signals() turn_signals_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - turn_signals_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + ->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { updateTurnSignalsData(msg); }); } @@ -442,9 +441,10 @@ void SignalDisplay::topic_updated_hazard_lights() hazard_lights_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - hazard_lights_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + ->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { updateHazardLightsData(msg); }); } @@ -454,12 +454,14 @@ void SignalDisplay::topic_updated_traffic() // resubscribe to the topic traffic_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - traffic_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - traffic_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_perception_msgs::msg::TrafficSignal::SharedPtr msg) { - updateTrafficLightData(msg); - }); + traffic_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficLightGroupArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); } } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp index 5c5342259005b..b9c22ec5f53ac 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp @@ -50,7 +50,7 @@ SpeedDisplay::SpeedDisplay() : current_speed_(0.0) } void SpeedDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { try { // Assuming msg->state.longitudinal_velocity_mps is the field you're interested in @@ -64,7 +64,7 @@ void SpeedDisplay::updateSpeedData( } // void SpeedDisplay::processMessage(const -// autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) +// autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) // { // try // { diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp index 4c83b4a73c0c2..3dec6a8e7d4a0 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp @@ -57,7 +57,7 @@ void SpeedLimitDisplay::updateSpeedLimitData( } void SpeedLimitDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { try { float speed = msg->longitudinal_velocity; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp index 45ebcde086165..d2390b16e5373 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp @@ -56,7 +56,7 @@ SteeringWheelDisplay::SteeringWheelDisplay() } void SteeringWheelDisplay::updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) { try { // Assuming msg->steering_angle is the field you're interested in diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp index f6d232709a333..2dc9c23583a52 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp @@ -48,7 +48,7 @@ TrafficDisplay::TrafficDisplay() } void TrafficDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg) { current_traffic_ = *msg; } @@ -68,8 +68,8 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF backgroundRect.top() + circleRect.height() / 2)); painter.drawEllipse(circleRect); - if (!current_traffic_.elements.empty()) { - switch (current_traffic_.elements[0].color) { + if (!current_traffic_.traffic_light_groups.empty()) { + switch (current_traffic_.traffic_light_groups[0].elements[0].color) { case 1: painter.setBrush(QBrush(tl_red_)); painter.drawEllipse(circleRect); diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp index 1aaa5871015a8..b42b5d953fcc6 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp @@ -46,7 +46,7 @@ TurnSignalsDisplay::TurnSignalsDisplay() : current_turn_signal_(0) } void TurnSignalsDisplay::updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) { try { // Assuming msg->report is the field you're interested in @@ -58,7 +58,7 @@ void TurnSignalsDisplay::updateTurnSignalsData( } void TurnSignalsDisplay::updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) { try { // Assuming msg->report is the field you're interested in @@ -80,11 +80,11 @@ void TurnSignalsDisplay::drawArrows( int rightArrowXPos = backgroundRect.right() - scaledRightArrow.width() * 3 - 175; bool leftActive = - (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || - current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + (current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || + current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE); bool rightActive = - (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || - current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + (current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || + current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE); // Color the arrows based on the state of the turn signals and hazard lights by having them blink // on and off diff --git a/common/autoware_perception_rviz_plugin/CMakeLists.txt b/common/autoware_perception_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..74671d74f7397 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/CMakeLists.txt @@ -0,0 +1,62 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_perception_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED COMPONENTS Widgets) + +set(OD_PLUGIN_LIB_SRC + src/object_detection/detected_objects_display.cpp + src/object_detection/tracked_objects_display.cpp + src/object_detection/predicted_objects_display.cpp +) + +set(OD_PLUGIN_LIB_HEADERS + include/autoware_perception_rviz_plugin/visibility_control.hpp +) +set(OD_PLUGIN_LIB_HEADERS_TO_WRAP + include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp + include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp + include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp +) + +set(COMMON_HEADERS + include/autoware_perception_rviz_plugin/common/color_alpha_property.hpp + include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp + include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +) + +set(COMMON_SRC + src/common/color_alpha_property.cpp + src/object_detection/object_polygon_detail.cpp +) + +foreach(header "${OD_PLUGIN_LIB_HEADERS_TO_WRAP}") + qt5_wrap_cpp(OD_PLUGIN_LIB_HEADERS_MOC "${header}") +endforeach() + +ament_auto_add_library(${PROJECT_NAME} SHARED + ${COMMON_HEADERS} + ${COMMON_SRC} + ${OD_PLUGIN_LIB_HEADERS} + ${OD_PLUGIN_LIB_HEADERS_MOC} + ${OD_PLUGIN_LIB_SRC} +) +target_link_libraries(${PROJECT_NAME} + rviz_common::rviz_common + Qt5::Widgets +) +target_include_directories(${PROJECT_NAME} PRIVATE include) + +# Settings to improve the build as suggested on https://github.com/ros2/rviz/blob/ros2/docs/plugin_development.md +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_compile_definitions(${PROJECT_NAME} PRIVATE "OBJECT_DETECTION_PLUGINS_BUILDING_LIBRARY") + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons +) diff --git a/common/autoware_perception_rviz_plugin/README.md b/common/autoware_perception_rviz_plugin/README.md new file mode 100644 index 0000000000000..ed6f3e1675ace --- /dev/null +++ b/common/autoware_perception_rviz_plugin/README.md @@ -0,0 +1,62 @@ +# autoware_perception_rviz_plugin + +## Purpose + +It is an rviz plugin for visualizing the result from perception module. This package is based on the implementation of the rviz plugin developed by Autoware.Auto. + +See Autoware.Auto design documentation for the original design philosophy. [[1]](https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/blob/master/src/tools/visualization/autoware_rviz_plugins) + + + +## Input Types / Visualization Results + +### DetectedObjects + +#### Input Types + +| Name | Type | Description | +| ---- | ------------------------------------------------ | ---------------------- | +| | `autoware_perception_msgs::msg::DetectedObjects` | detection result array | + +#### Visualization Result + +![detected-object-visualization-description](./images/detected-object-visualization-description.jpg) + +### TrackedObjects + +#### Input Types + +| Name | Type | Description | +| ---- | ----------------------------------------------- | --------------------- | +| | `autoware_perception_msgs::msg::TrackedObjects` | tracking result array | + +#### Visualization Result + +Overwrite tracking results with detection results. + +![tracked-object-visualization-description](./images/tracked-object-visualization-description.jpg) + +### PredictedObjects + +#### Input Types + +| Name | Type | Description | +| ---- | ------------------------------------------------- | ----------------------- | +| | `autoware_perception_msgs::msg::PredictedObjects` | prediction result array | + +#### Visualization Result + +Overwrite prediction results with tracking results. + +![predicted-object-visualization-description](./images/predicted-object-visualization-description.jpg) + +## References/External links + +[1] + +## Future extensions / Unimplemented parts diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/DetectedObjects.png b/common/autoware_perception_rviz_plugin/icons/classes/DetectedObjects.png similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/icons/classes/DetectedObjects.png rename to common/autoware_perception_rviz_plugin/icons/classes/DetectedObjects.png diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/PredictedObjects.png b/common/autoware_perception_rviz_plugin/icons/classes/PredictedObjects.png similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/icons/classes/PredictedObjects.png rename to common/autoware_perception_rviz_plugin/icons/classes/PredictedObjects.png diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/TrackedObjects.png b/common/autoware_perception_rviz_plugin/icons/classes/TrackedObjects.png similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/icons/classes/TrackedObjects.png rename to common/autoware_perception_rviz_plugin/icons/classes/TrackedObjects.png diff --git a/common/autoware_auto_perception_rviz_plugin/images/detected-object-visualization-description.jpg b/common/autoware_perception_rviz_plugin/images/detected-object-visualization-description.jpg similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/images/detected-object-visualization-description.jpg rename to common/autoware_perception_rviz_plugin/images/detected-object-visualization-description.jpg diff --git a/common/autoware_auto_perception_rviz_plugin/images/predicted-object-visualization-description.jpg b/common/autoware_perception_rviz_plugin/images/predicted-object-visualization-description.jpg similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/images/predicted-object-visualization-description.jpg rename to common/autoware_perception_rviz_plugin/images/predicted-object-visualization-description.jpg diff --git a/common/autoware_auto_perception_rviz_plugin/images/tracked-object-visualization-description.jpg b/common/autoware_perception_rviz_plugin/images/tracked-object-visualization-description.jpg similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/images/tracked-object-visualization-description.jpg rename to common/autoware_perception_rviz_plugin/images/tracked-object-visualization-description.jpg diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/common/color_alpha_property.hpp similarity index 81% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/common/color_alpha_property.hpp index 10dc46e55ec70..e39faeb254add 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/common/color_alpha_property.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_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ -#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" +#include "autoware_perception_rviz_plugin/visibility_control.hpp" #include #include @@ -31,7 +31,7 @@ namespace rviz_plugins namespace common { /// \brief Class to define Color and Alpha values as plugin properties -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty { public: /// \brief Constructor @@ -56,4 +56,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp new file mode 100644 index 0000000000000..4dcc4e9ea1545 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp @@ -0,0 +1,46 @@ +// Copyright 2021 Apex.AI, 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_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ + +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" + +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +/// \brief Class defining rviz plugin to visualize DetectedObjects +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay +: public ObjectPolygonDisplayBase +{ + Q_OBJECT + +public: + using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; + + DetectedObjectsDisplay(); + +private: + void processMessage(DetectedObjects::ConstSharedPtr msg) override; +}; + +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp new file mode 100644 index 0000000000000..8d29900e1da26 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -0,0 +1,298 @@ +// Copyright 2021 Apex.AI, 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. +/// \brief This file defines some helper functions used by ObjectPolygonDisplayBase class +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ + +#include "autoware_perception_rviz_plugin/visibility_control.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace rviz_plugins +{ +namespace object_detection +{ +namespace detail +{ +// Struct to define all the configurable visual properties of an object of a particular +// classification type +struct ObjectPropertyValues +{ + // Classified type of the object + std::string label; + // Color for the type of the object + std::array color; + // Alpha values for the type of the object + float alpha{0.999F}; +}; + +// Control object marker visualization +enum class ObjectFillType { Skeleton, Fill }; + +// Map defining colors according to value of label field in ObjectClassification msg +const std::map< + autoware_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> + // Color map is based on cityscapes color + kDefaultObjectPropertyValues = { + {autoware_perception_msgs::msg::ObjectClassification::UNKNOWN, {"UNKNOWN", {255, 255, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + {"PEDESTRIAN", {255, 192, 203}}}, + {autoware_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, + {autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + {"MOTORCYCLE", {119, 11, 32}}}, + {autoware_perception_msgs::msg::ObjectClassification::TRAILER, {"TRAILER", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; + +/// \brief Convert the given polygon into a marker representing the shape in 3d +/// \param shape_msg Shape msg to be converted. Corners should be in object-local frame +/// \param centroid Centroid position of the shape in Object.header.frame_id frame +/// \param orientation Orientation of the shape in Object.header.frame_id frame +/// \param color_rgba Color and alpha values to use for the marker +/// \param line_width Line thickness around the object +/// \return Marker ptr. Id and header will have to be set by the caller +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_shape_marker_ptr( + const autoware_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available = true, + const ObjectFillType fill_type = ObjectFillType::Skeleton); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_2d_shape_marker_ptr( + const autoware_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available = true); + +/// \brief Convert the given polygon into a marker representing the shape in 3d +/// \param centroid Centroid position of the shape in Object.header.frame_id frame +/// \return Marker ptr. Id and header will have to be set by the caller +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_label_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std::string label, const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_uuid_marker_ptr( + const std::string & uuid, const geometry_msgs::msg::Point & centroid, + const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_pose_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const double & confidence_interval_coefficient); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & confidence_interval_coefficient, const double & line_width); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_velocity_text_marker_ptr( + const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, + const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_acceleration_text_marker_ptr( + const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, + const std_msgs::msg::ColorRGBA & color_rgba); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_twist_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_rate_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient, const double & line_width); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_predicted_path_marker_ptr( + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_path_confidence_marker_ptr( + const autoware_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & path_confidence_color); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( + const double start_angle, const double end_angle, const double radius, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points( + const double point_list[][3], const int point_pairs[][2], const int & num_pairs, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( + const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( + const geometry_msgs::msg::Point center, const double radius, + std::vector & points, const int n); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); + +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( + const autoware_perception_msgs::msg::PredictedPath & paths, + std::vector & points, const bool is_simple = false); + +/// \brief Convert Point32 to Point +/// \param val Point32 to be converted +/// \return Point type +inline AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Point to_point( + const geometry_msgs::msg::Point32 & val) +{ + geometry_msgs::msg::Point ret; + ret.x = static_cast(val.x); + ret.y = static_cast(val.y); + ret.z = static_cast(val.z); + return ret; +} + +/// \brief Convert to Pose from Point and Quaternion +/// \param point +/// \param orientation +/// \return Pose type +inline AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose to_pose( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Quaternion & orientation) +{ + geometry_msgs::msg::Pose ret; + ret.position = point; + ret.orientation = orientation; + return ret; +} + +inline AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose initPose() +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + return pose; +} + +/// \brief Get the best classification from the list of classifications based on max probability +/// \tparam ClassificationContainerT List type with ObjectClassificationMsg +/// \param labels List of ObjectClassificationMsg objects +/// \param logger_name Name to use for logger in case of a warning (if labels is empty) +/// \return Id of the best classification, Unknown if there is no best label +template +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC + autoware_perception_msgs::msg::ObjectClassification::_label_type + get_best_label(ClassificationContainerT labels, const std::string & logger_name) +{ + const auto best_class_label = std::max_element( + labels.begin(), labels.end(), + [](const auto & a, const auto & b) -> bool { return a.probability < b.probability; }); + if (best_class_label == labels.end()) { + RCLCPP_WARN( + rclcpp::get_logger(logger_name), + "Empty classification field. " + "Treating as unknown"); + return autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; + } + return best_class_label->label; +} + +} // namespace detail +} // namespace object_detection +} // namespace rviz_plugins +} // namespace autoware + +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp similarity index 95% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 1093f6e4f2414..b05ba5f27f551 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.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_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" -#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" +#include "autoware_perception_rviz_plugin/common/color_alpha_property.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" +#include "autoware_perception_rviz_plugin/visibility_control.hpp" #include #include @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include @@ -46,14 +46,14 @@ namespace object_detection /// classes. /// \tparam MsgT PredictedObjects or TrackedObjects or DetectedObjects type template -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase : public rviz_common::RosTopicDisplay { public: using Color = std::array; using Marker = visualization_msgs::msg::Marker; using MarkerCommon = rviz_default_plugins::displays::MarkerCommon; - using ObjectClassificationMsg = autoware_auto_perception_msgs::msg::ObjectClassification; + using ObjectClassificationMsg = autoware_perception_msgs::msg::ObjectClassification; using RosTopicDisplay = rviz_common::RosTopicDisplay; using PolygonPropertyMap = @@ -189,7 +189,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \return Marker ptr. Id and header will have to be set by the caller template std::optional get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const ClassificationContainerT & labels, const double & line_width, const bool & is_orientation_available) const @@ -212,7 +212,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase template visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available); @@ -363,8 +363,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_predicted_path_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path) const { if (m_display_predicted_paths_property.getBool()) { const std::string uuid_str = uuid_to_string(uuid); @@ -379,7 +379,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_path_confidence_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + const autoware_perception_msgs::msg::PredictedPath & predicted_path) const { if (m_display_path_confidence_property.getBool()) { const std::string uuid_str = uuid_to_string(uuid); @@ -431,7 +431,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase return ss.str(); } - std_msgs::msg::ColorRGBA AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC + std_msgs::msg::ColorRGBA AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC get_color_from_uuid(const std::string & uuid) const { int i = (static_cast(uuid.at(0)) * 4 + static_cast(uuid.at(1))) % @@ -576,4 +576,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp similarity index 85% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp index 775c18db6ba5c..1445f02e34290 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.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_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" -#include +#include #include #include @@ -38,13 +38,13 @@ namespace rviz_plugins namespace object_detection { /// \brief Class defining rviz plugin to visualize PredictedObjects -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay -: public ObjectPolygonDisplayBase +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; + using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; PredictedObjectsDisplay(); ~PredictedObjectsDisplay() @@ -153,4 +153,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp similarity index 80% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp index 4e86a5ee93fd8..9ccaa5cd150f6 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.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_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" -#include +#include #include #include @@ -33,14 +33,14 @@ namespace rviz_plugins namespace object_detection { /// \brief Class defining rviz plugin to visualize TrackedObjects -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay -: public ObjectPolygonDisplayBase +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using TrackedObject = autoware_auto_perception_msgs::msg::TrackedObject; - using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects; + using TrackedObject = autoware_perception_msgs::msg::TrackedObject; + using TrackedObjects = autoware_perception_msgs::msg::TrackedObjects; TrackedObjectsDisplay(); @@ -114,4 +114,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/visibility_control.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/visibility_control.hpp new file mode 100644 index 0000000000000..e5e7693054ec8 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/visibility_control.hpp @@ -0,0 +1,43 @@ +// Copyright 2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) +#if defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || \ + defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllexport) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#else +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllimport) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#endif +#elif defined(__linux__) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) +#else // defined(_LINUX) +#error "Unsupported Build Configuration" +#endif // defined(_WINDOWS) + +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_perception_rviz_plugin/package.xml b/common/autoware_perception_rviz_plugin/package.xml new file mode 100644 index 0000000000000..460186c33b7d8 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/package.xml @@ -0,0 +1,34 @@ + + + + autoware_perception_rviz_plugin + 1.0.0 + Contains plugins to visualize object detection outputs + Apex.AI, Inc. + Satoshi Tanaka + Taiki Tanaka + Takeshi Miura + Shunsuke Miura + Yoshi Ri + Apache 2.0 + + ament_cmake + autoware_cmake + + libboost-dev + qtbase5-dev + + autoware_perception_msgs + rviz_common + rviz_default_plugins + + libqt5-widgets + rviz2 + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_perception_rviz_plugin/plugins_description.xml b/common/autoware_perception_rviz_plugin/plugins_description.xml new file mode 100644 index 0000000000000..710e3aa2bfa26 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/plugins_description.xml @@ -0,0 +1,26 @@ + + + + + + + Convert a PredictedObjects to markers and display them. + + autoware_perception_msgs/msg/PredictedObjects + + + + + Convert a TrackedObjects to markers and display them. + + autoware_perception_msgs/msg/TrackedObjects + + + + + Convert a DetectedObjects to markers and display them. + + autoware_perception_msgs/msg/DetectedObjects + + + diff --git a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp b/common/autoware_perception_rviz_plugin/src/common/color_alpha_property.cpp similarity index 95% rename from common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp rename to common/autoware_perception_rviz_plugin/src/common/color_alpha_property.cpp index b3e542a02243b..9b8ac2e38b740 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp +++ b/common/autoware_perception_rviz_plugin/src/common/color_alpha_property.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" +#include "autoware_perception_rviz_plugin/common/color_alpha_property.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp similarity index 97% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 53e935fa1850a..9dd0b2923f09f 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp" +#include "autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp" #include @@ -40,7 +40,7 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) object.kinematics.pose_with_covariance.pose.orientation, object.classification, get_line_width(), object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp similarity index 96% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 7631acffafdf9..1d06454582a2f 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License.. -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" #include #include @@ -50,7 +50,7 @@ namespace detail using Marker = visualization_msgs::msg::Marker; visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color) { auto marker_ptr = std::make_shared(); @@ -70,8 +70,8 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple) { auto marker_ptr = std::make_shared(); @@ -492,7 +492,7 @@ visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available, const ObjectFillType fill_type) @@ -502,7 +502,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( marker_ptr->color = color_rgba; marker_ptr->scale.x = line_width; - using autoware_auto_perception_msgs::msg::Shape; + using autoware_perception_msgs::msg::Shape; if (shape_msg.type == Shape::BOUNDING_BOX) { if (fill_type == ObjectFillType::Skeleton) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; @@ -542,7 +542,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available) @@ -550,7 +550,7 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); - using autoware_auto_perception_msgs::msg::Shape; + using autoware_perception_msgs::msg::Shape; if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); @@ -597,7 +597,7 @@ void calc_line_list_from_points( } void calc_bounding_box_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -620,7 +620,7 @@ void calc_bounding_box_line_list( } void calc_bounding_box_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { // direction triangle @@ -646,7 +646,7 @@ void calc_bounding_box_direction_line_list( } void calc_bounding_box_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -671,7 +671,7 @@ void calc_bounding_box_orientation_line_list( } void calc_2d_bounding_box_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -697,7 +697,7 @@ void calc_2d_bounding_box_bottom_line_list( } void calc_2d_bounding_box_bottom_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -721,7 +721,7 @@ void calc_2d_bounding_box_bottom_direction_line_list( } void calc_2d_bounding_box_bottom_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -746,7 +746,7 @@ void calc_2d_bounding_box_bottom_orientation_line_list( } void calc_cylinder_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double radius = shape.dimensions.x * 0.5; @@ -789,7 +789,7 @@ void calc_cylinder_line_list( } void calc_2d_cylinder_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double radius = shape.dimensions.x * 0.5; @@ -837,7 +837,7 @@ void calc_circle_line_list( } void calc_polygon_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { if (shape.footprint.points.size() < 2) { @@ -890,7 +890,7 @@ void calc_polygon_line_list( } void calc_2d_polygon_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { if (shape.footprint.points.size() < 2) { @@ -917,7 +917,7 @@ void calc_2d_polygon_bottom_line_list( } void calc_path_line_list( - const autoware_auto_perception_msgs::msg::PredictedPath & paths, + const autoware_perception_msgs::msg::PredictedPath & paths, std::vector & points, const bool is_simple) { const int circle_line_num = is_simple ? 5 : 10; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp similarity index 98% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index b42ffe1804246..d11aba912854d 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp" +#include "autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp" #include #include @@ -295,7 +295,7 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) lock.unlock(); - ObjectPolygonDisplayBase::update( + ObjectPolygonDisplayBase::update( wall_dt, ros_dt); } diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp similarity index 98% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 504b51f850444..214ed9ce70e63 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp" +#include "autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp" #include @@ -61,7 +61,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) object.kinematics.pose_with_covariance.pose.orientation, object.classification, get_line_width(), object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; diff --git a/common/autoware_point_types/README.md b/common/autoware_point_types/README.md new file mode 100644 index 0000000000000..92f19d2bc353a --- /dev/null +++ b/common/autoware_point_types/README.md @@ -0,0 +1 @@ +# Autoware Point Types diff --git a/common/autoware_test_utils/CMakeLists.txt b/common/autoware_test_utils/CMakeLists.txt new file mode 100644 index 0000000000000..91adf935445b4 --- /dev/null +++ b/common/autoware_test_utils/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_test_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_test_utils SHARED + src/autoware_test_utils.cpp) + +ament_auto_add_library(mock_data_parser SHARED + src/mock_data_parser.cpp) + +target_link_libraries(mock_data_parser + yaml-cpp +) + +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) +endif() + +ament_auto_package(INSTALL_TO_SHARE + config + test_map + test_data +) diff --git a/common/autoware_test_utils/README.md b/common/autoware_test_utils/README.md new file mode 100644 index 0000000000000..a656f35a90ce4 --- /dev/null +++ b/common/autoware_test_utils/README.md @@ -0,0 +1,40 @@ +# Test Utils + +## Background + +Several Autoware's components and modules have already adopted unit testing, so a common library to ease the process of writing unit tests is necessary. + +## Purpose + +The objective of the `test_utils` is to develop a unit testing library for the Autoware components. This library will include + +- commonly used functions +- input/mock data parser +- maps for testing +- common routes and mock data for testing. + +## Available Maps + +The following maps are available [here](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/autoware_test_utils/test_map) + +### Common + +The common map contains multiple types of usable inputs, including shoulder lanes, intersections, and some regulatory elements. The common map is named `lanelet2_map.osm` in the folder. + +![common](./images/common.png) + +### 2 km Straight + +The 2 km straight lanelet map consists of two lanes that run in the same direction. The map is named `2km_test.osm`. + +![two_km](./images/2km-test.png) + +The following illustrates the design of the map. + +![straight_diagram](./images/2km-test.svg) + +## Example use cases + +### Autoware Planning Test Manager + +The goal of the [Autoware Planning Test Manager](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_planning_test_manager/) is to test planning module nodes. The `PlanningInterfaceTestManager` class ([source code](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp)) creates wrapper functions based on the `test_utils` functions. diff --git a/planning/planning_test_utils/config/path_with_lane_id_data.yaml b/common/autoware_test_utils/config/path_with_lane_id_data.yaml similarity index 100% rename from planning/planning_test_utils/config/path_with_lane_id_data.yaml rename to common/autoware_test_utils/config/path_with_lane_id_data.yaml diff --git a/planning/planning_test_utils/config/test_common.param.yaml b/common/autoware_test_utils/config/test_common.param.yaml similarity index 100% rename from planning/planning_test_utils/config/test_common.param.yaml rename to common/autoware_test_utils/config/test_common.param.yaml diff --git a/control/trajectory_follower_node/test/test_nearest_search.param.yaml b/common/autoware_test_utils/config/test_nearest_search.param.yaml similarity index 100% rename from control/trajectory_follower_node/test/test_nearest_search.param.yaml rename to common/autoware_test_utils/config/test_nearest_search.param.yaml diff --git a/control/trajectory_follower_node/test/test_vehicle_info.param.yaml b/common/autoware_test_utils/config/test_vehicle_info.param.yaml similarity index 100% rename from control/trajectory_follower_node/test/test_vehicle_info.param.yaml rename to common/autoware_test_utils/config/test_vehicle_info.param.yaml diff --git a/common/autoware_test_utils/images/2km-test.png b/common/autoware_test_utils/images/2km-test.png new file mode 100644 index 0000000000000..297dc5a43865e Binary files /dev/null and b/common/autoware_test_utils/images/2km-test.png differ diff --git a/common/autoware_test_utils/images/2km-test.svg b/common/autoware_test_utils/images/2km-test.svg new file mode 100644 index 0000000000000..26632cdde9118 --- /dev/null +++ b/common/autoware_test_utils/images/2km-test.svg @@ -0,0 +1,505 @@ + + + + + + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 3.5 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2000.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 1.75 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (0.0, 0.0) +
+
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ 1000.0 meter +
+
+
+ + + + + + + + + + + + + + + + + +
+
+
+ 7.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + +
+
+
+ pose +
+ (1000.0, -3.5) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (1000.0, 3.5) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (-1000.0, 3.5) +
+
+
+
+
+ + + + + + + diff --git a/common/autoware_test_utils/images/common.png b/common/autoware_test_utils/images/common.png new file mode 100644 index 0000000000000..340437b53c0f5 Binary files /dev/null and b/common/autoware_test_utils/images/common.png differ 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 new file mode 100644 index 0000000000000..49d2c848ab508 --- /dev/null +++ b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp @@ -0,0 +1,517 @@ +// 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 AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_ +#define AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::test_utils +{ +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::LaneletPrimitive; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::Path; +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; +using geometry_msgs::msg::TransformStamped; +using nav_msgs::msg::OccupancyGrid; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; +using tf2_msgs::msg::TFMessage; +using tier4_planning_msgs::msg::Scenario; +using unique_identifier_msgs::msg::UUID; + +/** + * @brief Creates a Pose message with the specified position and orientation. + * + * This function initializes a geometry_msgs::msg::Pose message with the + * given position (x, y, z) and orientation (roll, pitch, yaw). + * + * @param x The x-coordinate of the position. + * @param y The y-coordinate of the position. + * @param z The z-coordinate of the position. + * @param roll The roll component of the orientation (in radians). + * @param pitch The pitch component of the orientation (in radians). + * @param yaw The yaw component of the orientation (in radians). + * @return A geometry_msgs::msg::Pose message with the specified position + * and orientation. + */ +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw); + +/** + * @brief Creates a Pose message from a 4-element array representing a 3D pose. + * + * This function initializes a geometry_msgs::msg::Pose message using a + * std::array of four doubles, where the first three elements represent the + * position (x, y, z) and the fourth element represents the yaw orientation. + * The roll and pitch orientations are assumed to be zero. + * + * @param pose3d A std::array of four doubles representing the 3D pose. The + * first element is the x-coordinate, the second is the y-coordinate, the + * third is the z-coordinate, and the fourth is the yaw orientation. + * @return A geometry_msgs::msg::Pose message with the specified position + * and yaw orientation, with roll and pitch set to zero. + */ +geometry_msgs::msg::Pose createPose(const std::array & pose3d); + +/** + * @brief Creates a LaneletSegment with the specified ID. + * + * Initializes a LaneletSegment containing a single LaneletPrimitive with the + * given ID and a primitive type of "lane". + * + * @param id The ID for the LaneletPrimitive and preferred primitive. + * @return A LaneletSegment with the specified ID. + */ +LaneletSegment createLaneletSegment(int id); + +/** + * @brief Loads a Lanelet map from a specified file. + * + * This function loads a Lanelet2 map using the given filename. It uses the MGRS + * projector and checks for any errors during the loading process. If errors + * are found, they are logged, and the function returns nullptr. + * + * @param lanelet2_filename The filename of the Lanelet2 map to load. + * @return A LaneletMapPtr to the loaded map, or nullptr if there were errors. + */ +lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename); + +/** + * @brief Converts a Lanelet map to a LaneletMapBin message. + * + * This function converts a given Lanelet map to a LaneletMapBin message. It also + * parses the format and map versions from the specified filename and includes + * them in the message. The timestamp for the message is set to the provided time. + * + * @param map The Lanelet map to convert. + * @param lanelet2_filename The filename of the Lanelet2 map, used to parse format and map versions. + * @param now The current time to set in the message header. + * @return A LaneletMapBin message containing the converted map data. + */ +LaneletMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, + const rclcpp::Time & now); + +/** + * @brief Creates a normal Lanelet route with predefined start and goal poses. + * + * This function initializes a LaneletRoute with a predefined start and goal pose. + * + * @return A LaneletRoute with the specified start and goal poses. + */ +LaneletRoute makeNormalRoute(); + +/** + * @brief Creates an OccupancyGrid message with the specified dimensions and resolution. + * + * This function initializes an OccupancyGrid message with given width, height, and resolution. + * All cells in the grid are initialized to zero. + * + * @param width The width of the occupancy grid. + * @param height The height of the occupancy grid. + * @param resolution The resolution of the occupancy grid. + * @return An OccupancyGrid message with the specified dimensions and resolution. + */ +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. + * + * This function loads a Lanelet map from the given file, overwrites the + * centerline with the specified resolution, and converts the map to a LaneletMapBin message. + * + * @param absolute_path The absolute path to the Lanelet2 map file. + * @param center_line_resolution The resolution for the centerline. + * @return A LaneletMapBin message containing the map data. + */ +LaneletMapBin make_map_bin_msg( + const std::string & absolute_path, const double center_line_resolution = 5.0); + +/** + * @brief Creates a LaneletMapBin message using a predefined Lanelet2 map file. + * + * This function loads a lanelet2_map.osm from the test_map folder in the + * autoware_test_utils package, overwrites the centerline with a resolution of 5.0, + * and converts the map to a LaneletMapBin message. + * + * @return A LaneletMapBin message containing the map data. + */ +LaneletMapBin makeMapBinMsg(); + +/** + * @brief Creates an Odometry message with a specified shift. + * + * This function initializes an Odometry message with a pose shifted by the given amount at y + * direction. x pose, z pose, and yaw angle remains zero. + * + * @param shift The amount by which to shift the pose. + * @return An Odometry message with the specified shift. + */ +Odometry makeOdometry(const double shift = 0.0); + +/** + * @brief Creates an initial Odometry message with a specified shift. + * + * This function initializes an Odometry message with a pose shifted by the given amount, + * accounting for a specific yaw angle. + * + * @param shift The amount by which to shift the pose. + * @return An Odometry message with the specified shift. + */ +Odometry makeInitialPose(const double shift = 0.0); + +/** + * @brief Creates a TFMessage with the specified frame IDs. + * + * This function initializes a TFMessage with a transform containing the given frame IDs. + * The transform includes a specific translation and rotation. + * + * @param target_node The node to use for obtaining the current time. + * @param frame_id The ID of the parent frame. + * @param child_frame_id The ID of the child frame. + * @return A TFMessage containing the transform. + */ +TFMessage makeTFMsg( + rclcpp::Node::SharedPtr target_node, std::string && frame_id = "", + std::string && child_frame_id = ""); + +/** + * @brief Creates a Scenario message with the specified scenario. + * + * This function initializes a Scenario message with the current scenario and a list of activating + * scenarios. + * + * @param scenario The name of the current scenario. + * @return A Scenario message with the specified scenario. + */ +Scenario makeScenarioMsg(const std::string & scenario); + +/** + * @brief Combines two sets of consecutive RouteSections. + * + * This function combines two sets of RouteSections, removing the overlapping end section from the + * first set. + * + * @param route_sections1 The first set of RouteSections. + * @param route_sections2 The second set of RouteSections. + * @return A combined set of RouteSections. + */ +RouteSections combineConsecutiveRouteSections( + const RouteSections & route_sections1, const RouteSections & route_sections2); + +/** + * @brief Creates a predefined behavior Lanelet route. + * + * This function initializes a LaneletRoute with predefined start and goal poses, + * a list of lanelet segment IDs, and a fixed UUID. + * this is for the test lanelet2_map.osm + * file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 + * + * @return A LaneletRoute with the specified attributes. + */ +LaneletRoute makeBehaviorNormalRoute(); + +/** + * @brief Spins multiple ROS nodes a specified number of times. + * + * This function spins the given test and target nodes for the specified number of iterations. + * + * @param test_node The test node to spin. + * @param target_node The target node to spin. + * @param repeat_count The number of times to spin the nodes. + */ +void spinSomeNodes( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, + const int repeat_count = 1); + +/** + * @brief Updates node options with parameter files. + * + * This function updates the given node options to include the specified parameter files. + * + * @param node_options The node options to update. + * @param params_files A vector of parameter file paths to add to the node options. + */ +void updateNodeOptions( + rclcpp::NodeOptions & node_options, const std::vector & params_files); + +/** + * @brief Loads a PathWithLaneId message from a YAML file. + * + * This function loads a PathWithLaneId message from a YAML file located in the + * autoware_test_utils package. + * + * @return A PathWithLaneId message containing the loaded data. + */ +PathWithLaneId loadPathWithLaneIdInYaml(); + +/** + * @brief Generates a trajectory with specified parameters. + * + * This function generates a trajectory of type T with a given number of points, + * point interval, velocity, initial theta, delta theta, and optionally an + * overlapping point index. + * + * @tparam T The type of the trajectory. + * @param num_points The number of points in the trajectory. + * @param point_interval The distance between consecutive points. + * @param velocity The longitudinal velocity for each point. + * @param init_theta The initial theta angle. + * @param delta_theta The change in theta per point. + * @param overlapping_point_index The index of the point to overlap (default is max size_t value). + * @return A trajectory of type T with the specified parameters. + */ +template +T generateTrajectory( + const size_t num_points, const double point_interval, const double velocity = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0, + const size_t overlapping_point_index = std::numeric_limits::max()) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + Point p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = velocity; + traj.points.push_back(p); + + if (i == overlapping_point_index) { + Point value_to_insert = traj.points[overlapping_point_index]; + traj.points.insert(traj.points.begin() + overlapping_point_index + 1, value_to_insert); + } + } + + return traj; +} + +/** + * @brief Creates a publisher with appropriate QoS settings. + * + * This function creates a publisher for a given topic name and message type with appropriate + * QoS settings, depending on the message type. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param topic_name The name of the topic to publish to. + * @param publisher A reference to the publisher to be created. + */ +template +void createPublisherWithQoS( + rclcpp::Node::SharedPtr test_node, std::string topic_name, + std::shared_ptr> & publisher) +{ + if constexpr ( + std::is_same_v || std::is_same_v || + std::is_same_v) { + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.reliable(); + qos.transient_local(); + publisher = rclcpp::create_publisher(test_node, topic_name, qos); + } else { + publisher = rclcpp::create_publisher(test_node, topic_name, 1); + } +} + +/** + * @brief Sets up a publisher for a given topic. + * + * This function sets up a publisher for a given topic using createPublisherWithQoS. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param topic_name The name of the topic to publish to. + * @param publisher A reference to the publisher to be set. + */ +template +void setPublisher( + rclcpp::Node::SharedPtr test_node, std::string topic_name, + std::shared_ptr> & publisher) +{ + createPublisherWithQoS(test_node, topic_name, publisher); +} + +/** + * @brief Creates a subscription with appropriate QoS settings. + * + * This function creates a subscription for a given topic name and message type with appropriate + * QoS settings, depending on the message type. + * + * @tparam T The type of the message to subscribe to. + * @param test_node The node to create the subscription on. + * @param topic_name The name of the topic to subscribe to. + * @param callback The callback function to call when a message is received. + * @param subscriber A reference to the subscription to be created. + */ +template +void createSubscription( + rclcpp::Node::SharedPtr test_node, std::string topic_name, + std::function callback, + std::shared_ptr> & subscriber) +{ + if constexpr (std::is_same_v) { + subscriber = test_node->create_subscription(topic_name, rclcpp::QoS{1}, callback); + } else { + subscriber = test_node->create_subscription(topic_name, 10, callback); + } +} + +/** + * @brief Sets up a subscriber for a given topic. + * + * This function sets up a subscriber for a given topic using createSubscription. + * + * @tparam T The type of the message to subscribe to. + * @param test_node The node to create the subscription on. + * @param topic_name The name of the topic to subscribe to. + * @param subscriber A reference to the subscription to be set. + * @param count A reference to a counter that increments on message receipt. + */ +template +void setSubscriber( + rclcpp::Node::SharedPtr test_node, std::string topic_name, + std::shared_ptr> & subscriber, size_t & count) +{ + createSubscription( + test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber); +} + +/** + * @brief Publishes data to a target node. + * + * This function publishes data to a target node on a given topic, ensuring that the topic has + * subscribers and retrying a specified number of times. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param target_node The target node to publish the message to. + * @param topic_name The name of the topic to publish to. + * @param publisher A shared pointer to the publisher. + * @param data The data to publish. + * @param repeat_count The number of times to retry publishing. + */ +template +void publishToTargetNode( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, std::string topic_name, + typename rclcpp::Publisher::SharedPtr publisher, T data, const int repeat_count = 3) +{ + if (topic_name.empty()) { + int status = 1; + char * demangled_name = abi::__cxa_demangle(typeid(data).name(), nullptr, nullptr, &status); + if (status == 0) { + throw std::runtime_error(std::string("Topic name for ") + demangled_name + " is empty"); + } + throw std::runtime_error(std::string("Topic name for ") + typeid(data).name() + " is empty"); + } + + autoware::test_utils::setPublisher(test_node, topic_name, publisher); + publisher->publish(data); + + if (target_node->count_subscribers(topic_name) == 0) { + throw std::runtime_error("No subscriber for " + topic_name); + } + autoware::test_utils::spinSomeNodes(test_node, target_node, repeat_count); +} + +} // namespace autoware::test_utils + +#endif // AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_ diff --git a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp new file mode 100644 index 0000000000000..47284da447133 --- /dev/null +++ b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp @@ -0,0 +1,46 @@ +// 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. + +#ifndef AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_ +#define AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include + +namespace autoware::test_utils +{ +using autoware_planning_msgs::msg::LaneletPrimitive; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::LaneletSegment; +using geometry_msgs::msg::Pose; + +Pose parse_pose(const YAML::Node & node); + +LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node); + +std::vector parse_lanelet_primitives(const YAML::Node & node); + +std::vector parse_segments(const YAML::Node & node); + +LaneletRoute parse_lanelet_route_file(const std::string & filename); +} // namespace autoware::test_utils + +#endif // AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_ diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml new file mode 100644 index 0000000000000..778b9a42ca207 --- /dev/null +++ b/common/autoware_test_utils/package.xml @@ -0,0 +1,45 @@ + + + + autoware_test_utils + 0.1.0 + ROS 2 node for testing interface of the nodes in planning module + Kyoichi Sugahara + Takamasa Horibe + Zulfaqar Azmi + Mamoru Sobue + Apache License 2.0 + + Kyoichi Sugahara + + ament_cmake_auto + autoware_cmake + + autoware_control_msgs + autoware_map_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_universe_utils + autoware_vehicle_msgs + component_interface_specs + component_interface_utils + lanelet2_extension + lanelet2_io + nav_msgs + rclcpp + tf2_msgs + tf2_ros + tier4_api_msgs + tier4_planning_msgs + tier4_v2x_msgs + unique_identifier_msgs + yaml_cpp_vendor + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp new file mode 100644 index 0000000000000..563350dbe53cc --- /dev/null +++ b/common/autoware_test_utils/src/autoware_test_utils.cpp @@ -0,0 +1,338 @@ +// 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 +#include +#include +#include +#include +#include + +#include + +namespace autoware::test_utils +{ + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = createPoint(x, y, z); + p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + return p; +} + +geometry_msgs::msg::Pose createPose(const std::array & pose3d) +{ + return createPose(pose3d[0], pose3d[1], pose3d[2], 0.0, 0.0, pose3d[3]); +} + +LaneletSegment createLaneletSegment(int id) +{ + LaneletPrimitive primitive; + primitive.id = id; + primitive.primitive_type = "lane"; + LaneletSegment segment; + segment.preferred_primitive.id = id; + segment.primitives.push_back(primitive); + return segment; +} + +lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (errors.empty()) { + return map; + } + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + return nullptr; +} + +LaneletMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) +{ + std::string format_version{}; + std::string map_version{}; + lanelet::io_handlers::AutowareOsmParser::parseVersions( + lanelet2_filename, &format_version, &map_version); + + LaneletMapBin map_bin_msg; + map_bin_msg.header.stamp = now; + map_bin_msg.header.frame_id = "map"; + map_bin_msg.version_map_format = format_version; + map_bin_msg.version_map = map_version; + lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); + + return map_bin_msg; +} + +LaneletRoute makeNormalRoute() +{ + const std::array start_pose{5.5, 4., 0., M_PI_2}; + const std::array goal_pose{8.0, 26.3, 0, 0}; + LaneletRoute route; + route.header.frame_id = "map"; + route.start_pose = createPose(start_pose); + route.goal_pose = createPose(goal_pose); + return route; +} + +OccupancyGrid makeCostMapMsg(size_t width, size_t height, double resolution) +{ + nav_msgs::msg::OccupancyGrid costmap_msg; + + // create info + costmap_msg.header.frame_id = "map"; + costmap_msg.info.width = width; + costmap_msg.info.height = height; + costmap_msg.info.resolution = static_cast(resolution); + + // create data + const size_t n_elem = width * height; + for (size_t i = 0; i < n_elem; ++i) { + costmap_msg.data.push_back(0.0); + } + 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) +{ + const auto map = loadMap(absolute_path); + if (!map) { + return autoware_map_msgs::msg::LaneletMapBin_>{}; + } + + // overwrite centerline + lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + + // create map bin msg + const auto map_bin_msg = + convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now()); + return map_bin_msg; +} + +LaneletMapBin makeMapBinMsg() +{ + return make_map_bin_msg( + get_absolute_path_to_lanelet_map("autoware_test_utils", "lanelet2_map.osm")); +} + +Odometry makeOdometry(const double shift) +{ + Odometry odometry; + const std::array start_pose{0.0, shift, 0.0, 0.0}; + odometry.pose.pose = createPose(start_pose); + odometry.header.frame_id = "map"; + return odometry; +} + +Odometry makeInitialPose(const double shift) +{ + Odometry current_odometry; + const auto yaw = 0.9724497591854532; + const auto shift_x = shift * std::sin(yaw); + const auto shift_y = shift * std::cos(yaw); + const std::array start_pose{ + 3722.16015625 + shift_x, 73723.515625 + shift_y, 0.233112560494183, yaw}; + current_odometry.pose.pose = autoware::test_utils::createPose(start_pose); + current_odometry.header.frame_id = "map"; + return current_odometry; +} + +TFMessage makeTFMsg( + rclcpp::Node::SharedPtr target_node, std::string && frame_id, std::string && child_frame_id) +{ + TFMessage tf_msg; + geometry_msgs::msg::Quaternion quaternion; + quaternion.x = 0.; + quaternion.y = 0.; + quaternion.z = 0.23311256049418302; + quaternion.w = 0.9724497591854532; + + TransformStamped tf; + tf.header.stamp = target_node->get_clock()->now(); + tf.header.frame_id = frame_id; + tf.child_frame_id = child_frame_id; + tf.transform.translation.x = 3722.16015625; + tf.transform.translation.y = 73723.515625; + tf.transform.translation.z = 0; + tf.transform.rotation = quaternion; + + tf_msg.transforms.emplace_back(std::move(tf)); + return tf_msg; +} + +Scenario makeScenarioMsg(const std::string & scenario) +{ + Scenario scenario_msg; + scenario_msg.current_scenario = scenario; + scenario_msg.activating_scenarios = {scenario}; + return scenario_msg; +} + +RouteSections combineConsecutiveRouteSections( + 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; +} + +// this is for the test lanelet2_map.osm +// file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 +LaneletRoute makeBehaviorNormalRoute() +{ + LaneletRoute route; + route.header.frame_id = "map"; + route.start_pose = + createPose({3722.16015625, 73723.515625, 0.233112560494183, 0.9724497591854532}); + route.goal_pose = + createPose({3778.362060546875, 73721.2734375, -0.5107480274693206, 0.8597304533609347}); + + std::vector primitive_ids = {9102, 9178, 54, 112}; + for (int id : primitive_ids) { + route.segments.push_back(createLaneletSegment(id)); + } + + std::array uuid_bytes{210, 87, 16, 126, 98, 151, 58, 28, + 252, 221, 230, 92, 122, 170, 46, 6}; + route.uuid.uuid = uuid_bytes; + + route.allow_modification = false; + return route; +} + +void spinSomeNodes( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, const int repeat_count) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node); + executor.add_node(target_node); + for (int i = 0; i < repeat_count; i++) { + executor.spin_some(std::chrono::milliseconds(100)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } +} + +void updateNodeOptions( + rclcpp::NodeOptions & node_options, const std::vector & params_files) +{ + std::vector arguments; + arguments.push_back("--ros-args"); + arguments.push_back("--params-file"); + for (const auto & param_file : params_files) { + arguments.push_back(param_file.c_str()); + arguments.push_back("--params-file"); + } + arguments.pop_back(); + + node_options.arguments(std::vector{arguments.begin(), arguments.end()}); +} + +PathWithLaneId loadPathWithLaneIdInYaml() +{ + 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; + + // Convert YAML data to PathWithLaneId message + // Fill the header + path_msg.header.stamp.sec = yaml_node["header"]["stamp"]["sec"].as(); + path_msg.header.stamp.nanosec = yaml_node["header"]["stamp"]["nanosec"].as(); + path_msg.header.frame_id = yaml_node["header"]["frame_id"].as(); + + // Fill the points + for (const auto & point_node : yaml_node["points"]) { + PathPointWithLaneId point; + // Fill the PathPoint data + point.point.pose.position.x = point_node["point"]["pose"]["position"]["x"].as(); + point.point.pose.position.y = point_node["point"]["pose"]["position"]["y"].as(); + point.point.pose.position.z = point_node["point"]["pose"]["position"]["z"].as(); + point.point.pose.orientation.x = point_node["point"]["pose"]["orientation"]["x"].as(); + point.point.pose.orientation.y = point_node["point"]["pose"]["orientation"]["y"].as(); + point.point.pose.orientation.z = point_node["point"]["pose"]["orientation"]["z"].as(); + point.point.pose.orientation.w = point_node["point"]["pose"]["orientation"]["w"].as(); + point.point.longitudinal_velocity_mps = + point_node["point"]["longitudinal_velocity_mps"].as(); + point.point.lateral_velocity_mps = point_node["point"]["lateral_velocity_mps"].as(); + point.point.heading_rate_rps = point_node["point"]["heading_rate_rps"].as(); + point.point.is_final = point_node["point"]["is_final"].as(); + // Fill the lane_ids + for (const auto & lane_id_node : point_node["lane_ids"]) { + point.lane_ids.push_back(lane_id_node.as()); + } + + path_msg.points.push_back(point); + } + + // Fill the left_bound + for (const auto & point_node : yaml_node["left_bound"]) { + Point point; + // Fill the Point data (left_bound) + point.x = point_node["x"].as(); + point.y = point_node["y"].as(); + point.z = point_node["z"].as(); + + path_msg.left_bound.push_back(point); + } + + // Fill the right_bound + for (const auto & point_node : yaml_node["right_bound"]) { + Point point; + // Fill the Point data + point.x = point_node["x"].as(); + point.y = point_node["y"].as(); + point.z = point_node["z"].as(); + + path_msg.right_bound.push_back(point); + } + return path_msg; +} + +} // namespace autoware::test_utils diff --git a/common/autoware_test_utils/src/mock_data_parser.cpp b/common/autoware_test_utils/src/mock_data_parser.cpp new file mode 100644 index 0000000000000..a8f2a8c6433df --- /dev/null +++ b/common/autoware_test_utils/src/mock_data_parser.cpp @@ -0,0 +1,92 @@ +// 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/mock_data_parser.hpp" + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::test_utils +{ +Pose parse_pose(const YAML::Node & node) +{ + Pose pose; + pose.position.x = node["position"]["x"].as(); + pose.position.y = node["position"]["y"].as(); + pose.position.z = node["position"]["z"].as(); + pose.orientation.x = node["orientation"]["x"].as(); + pose.orientation.y = node["orientation"]["y"].as(); + pose.orientation.z = node["orientation"]["z"].as(); + pose.orientation.w = node["orientation"]["w"].as(); + return pose; +} + +LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node) +{ + LaneletPrimitive primitive; + primitive.id = node["id"].as(); + primitive.primitive_type = node["primitive_type"].as(); + + return primitive; +} + +std::vector parse_lanelet_primitives(const YAML::Node & node) +{ + std::vector primitives; + primitives.reserve(node.size()); + std::transform(node.begin(), node.end(), std::back_inserter(primitives), [&](const auto & p) { + return parse_lanelet_primitive(p); + }); + + return primitives; +} + +std::vector parse_segments(const YAML::Node & node) +{ + std::vector segments; + std::transform(node.begin(), node.end(), std::back_inserter(segments), [&](const auto & input) { + LaneletSegment segment; + segment.preferred_primitive = parse_lanelet_primitive(input["preferred_primitive"]); + segment.primitives = parse_lanelet_primitives(input["primitives"]); + return segment; + }); + + return segments; +} + +LaneletRoute parse_lanelet_route_file(const std::string & filename) +{ + LaneletRoute lanelet_route; + try { + YAML::Node config = YAML::LoadFile(filename); + + lanelet_route.start_pose = (config["start_pose"]) ? parse_pose(config["start_pose"]) : Pose(); + lanelet_route.goal_pose = (config["goal_pose"]) ? parse_pose(config["goal_pose"]) : Pose(); + lanelet_route.segments = parse_segments(config["segments"]); + } catch (const std::exception & e) { + RCLCPP_DEBUG(rclcpp::get_logger("autoware_test_utils"), "Exception caught: %s", e.what()); + } + return lanelet_route; +} +} // namespace autoware::test_utils diff --git a/common/autoware_test_utils/test/test_mock_data_parser.cpp b/common/autoware_test_utils/test/test_mock_data_parser.cpp new file mode 100644 index 0000000000000..f9e37a521c42c --- /dev/null +++ b/common/autoware_test_utils/test/test_mock_data_parser.cpp @@ -0,0 +1,135 @@ +// 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 + +// Assuming the parseRouteFile function is in 'RouteHandler.h' +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "autoware_test_utils/mock_data_parser.hpp" + +namespace autoware::test_utils +{ +// Example YAML structure as a string for testing +const char g_complete_yaml[] = R"( +start_pose: + position: + x: 1.0 + y: 2.0 + z: 3.0 + orientation: + x: 0.1 + y: 0.2 + z: 0.3 + w: 0.4 +goal_pose: + position: + x: 4.0 + y: 5.0 + z: 6.0 + orientation: + x: 0.5 + y: 0.6 + z: 0.7 + w: 0.8 +segments: +- preferred_primitive: + id: 11 + primitive_type: '' + primitives: + - id: 22 + primitive_type: lane + - id: 33 + primitive_type: lane +)"; + +TEST(ParseFunctions, CompleteYAMLTest) +{ + YAML::Node config = YAML::Load(g_complete_yaml); + + // Test parsing of start_pose and goal_pose + Pose start_pose = parse_pose(config["start_pose"]); + Pose goal_pose = parse_pose(config["goal_pose"]); + + EXPECT_DOUBLE_EQ(start_pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(start_pose.position.y, 2.0); + EXPECT_DOUBLE_EQ(start_pose.position.z, 3.0); + + EXPECT_DOUBLE_EQ(start_pose.orientation.x, 0.1); + EXPECT_DOUBLE_EQ(start_pose.orientation.y, 0.2); + EXPECT_DOUBLE_EQ(start_pose.orientation.z, 0.3); + EXPECT_DOUBLE_EQ(start_pose.orientation.w, 0.4); + + EXPECT_DOUBLE_EQ(goal_pose.position.x, 4.0); + EXPECT_DOUBLE_EQ(goal_pose.position.y, 5.0); + EXPECT_DOUBLE_EQ(goal_pose.position.z, 6.0); + EXPECT_DOUBLE_EQ(goal_pose.orientation.x, 0.5); + EXPECT_DOUBLE_EQ(goal_pose.orientation.y, 0.6); + EXPECT_DOUBLE_EQ(goal_pose.orientation.z, 0.7); + EXPECT_DOUBLE_EQ(goal_pose.orientation.w, 0.8); + + // Test parsing of segments + std::vector segments = parse_segments(config["segments"]); + ASSERT_EQ( + segments.size(), uint64_t(1)); // Assuming only one segment in the provided YAML for this test + + const auto & segment0 = segments[0]; + EXPECT_EQ(segment0.preferred_primitive.id, 11); + EXPECT_EQ(segment0.primitives.size(), uint64_t(2)); + EXPECT_EQ(segment0.primitives[0].id, 22); + EXPECT_EQ(segment0.primitives[0].primitive_type, "lane"); + EXPECT_EQ(segment0.primitives[1].id, 33); + EXPECT_EQ(segment0.primitives[1].primitive_type, "lane"); +} + +TEST(ParseFunction, CompleteFromFilename) +{ + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto parser_test_route = + autoware_test_utils_dir + "/test_data/lanelet_route_parser_test.yaml"; + + const auto lanelet_route = parse_lanelet_route_file(parser_test_route); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.y, 2.0); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.z, 3.0); + + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.x, 0.1); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.y, 0.2); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.z, 0.3); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.w, 0.4); + + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.x, 4.0); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.y, 5.0); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.z, 6.0); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.x, 0.5); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.y, 0.6); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.z, 0.7); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.w, 0.8); + + ASSERT_EQ( + lanelet_route.segments.size(), + uint64_t(2)); // Assuming only one segment in the provided YAML for this test + const auto & segment1 = lanelet_route.segments[1]; + EXPECT_EQ(segment1.preferred_primitive.id, 44); + EXPECT_EQ(segment1.primitives.size(), uint64_t(4)); + EXPECT_EQ(segment1.primitives[0].id, 55); + EXPECT_EQ(segment1.primitives[0].primitive_type, "lane"); + EXPECT_EQ(segment1.primitives[1].id, 66); + EXPECT_EQ(segment1.primitives[1].primitive_type, "lane"); + EXPECT_EQ(segment1.primitives[2].id, 77); + EXPECT_EQ(segment1.primitives[2].primitive_type, "lane"); + EXPECT_EQ(segment1.primitives[3].id, 88); + EXPECT_EQ(segment1.primitives[3].primitive_type, "lane"); +} +} // namespace autoware::test_utils diff --git a/common/autoware_test_utils/test_data/lanelet_route_parser_test.yaml b/common/autoware_test_utils/test_data/lanelet_route_parser_test.yaml new file mode 100644 index 0000000000000..06ed15d20d3c9 --- /dev/null +++ b/common/autoware_test_utils/test_data/lanelet_route_parser_test.yaml @@ -0,0 +1,41 @@ +start_pose: + position: + x: 1.0 + y: 2.0 + z: 3.0 + orientation: + x: 0.1 + y: 0.2 + z: 0.3 + w: 0.4 +goal_pose: + position: + x: 4.0 + y: 5.0 + z: 6.0 + orientation: + x: 0.5 + y: 0.6 + z: 0.7 + w: 0.8 +segments: + - preferred_primitive: + id: 11 + primitive_type: "" + primitives: + - id: 22 + primitive_type: lane + - id: 33 + primitive_type: lane + - preferred_primitive: + id: 44 + primitive_type: "" + primitives: + - id: 55 + primitive_type: lane + - id: 66 + primitive_type: lane + - id: 77 + primitive_type: lane + - id: 88 + primitive_type: lane diff --git a/common/autoware_test_utils/test_map/2km_test.osm b/common/autoware_test_utils/test_map/2km_test.osm new file mode 100644 index 0000000000000..43c22f8de6bb1 --- /dev/null +++ b/common/autoware_test_utils/test_map/2km_test.osm @@ -0,0 +1,33390 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/planning_test_utils/test_map/lanelet2_map.osm b/common/autoware_test_utils/test_map/lanelet2_map.osm similarity index 100% rename from planning/planning_test_utils/test_map/lanelet2_map.osm rename to common/autoware_test_utils/test_map/lanelet2_map.osm 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/planning/planning_test_utils/test_map/pointcloud_map.pcd b/common/autoware_test_utils/test_map/pointcloud_map.pcd similarity index 100% rename from planning/planning_test_utils/test_map/pointcloud_map.pcd rename to common/autoware_test_utils/test_map/pointcloud_map.pcd diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt new file mode 100644 index 0000000000000..9e86ddeb60692 --- /dev/null +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_universe_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Boost REQUIRED) + +ament_auto_add_library(autoware_universe_utils SHARED + src/geometry/geometry.cpp + src/geometry/pose_deviation.cpp + src/geometry/boost_polygon_utils.cpp + src/math/sin_table.cpp + src/math/trigonometry.cpp + src/ros/msg_operation.cpp + src/ros/marker_helper.cpp + src/ros/logger_level_configure.cpp + src/system/backtrace.cpp +) + +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + + file(GLOB_RECURSE test_files test/**/*.cpp) + + ament_add_ros_isolated_gtest(test_autoware_universe_utils ${test_files}) + + target_link_libraries(test_autoware_universe_utils + autoware_universe_utils + ) +endif() + +ament_auto_package() 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/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp new file mode 100644 index 0000000000000..1313ec558fe01 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/boost_polygon_utils.hpp @@ -0,0 +1,52 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ + +#include "autoware/universe_utils/geometry/boost_geometry.hpp" + +#include +#include +#include +#include + +#include + +namespace autoware::universe_utils +{ +bool isClockwise(const Polygon2d & polygon); +Polygon2d inverseClockwise(const Polygon2d & polygon); +geometry_msgs::msg::Polygon rotatePolygon( + const geometry_msgs::msg::Polygon & polygon, const double & angle); +/// @brief rotate a polygon by some angle around the origin +/// @param[in] polygon input polygon +/// @param[in] angle angle of rotation [rad] +/// @return rotated polygon +Polygon2d rotatePolygon(const Polygon2d & polygon, const double angle); +Polygon2d toPolygon2d( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape); +Polygon2d toPolygon2d( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::DetectedObject & object); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::TrackedObject & object); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::PredictedObject & object); +Polygon2d toFootprint( + const geometry_msgs::msg::Pose & base_link_pose, const double base_to_front, + 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 autoware::universe_utils + +#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 89% 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 63cf6305e8158..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 @@ -27,9 +27,8 @@ #define EIGEN_MPL2_ONLY #include -#include -#include -#include +#include +#include #include #include #include @@ -39,6 +38,7 @@ #include #include #include +#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) @@ -129,21 +129,19 @@ inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseWithCova } template <> -inline geometry_msgs::msg::Point getPoint(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline geometry_msgs::msg::Point getPoint(const autoware_planning_msgs::msg::PathPoint & p) { return p.pose.position; } template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Point getPoint(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose.position; } template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline geometry_msgs::msg::Point getPoint(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.pose.position; } @@ -168,20 +166,19 @@ inline geometry_msgs::msg::Pose getPose(const geometry_msgs::msg::PoseStamped & } template <> -inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline geometry_msgs::msg::Pose getPose(const autoware_planning_msgs::msg::PathPoint & p) { return p.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Pose getPose(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose; } template <> -inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline geometry_msgs::msg::Pose getPose(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.pose; } @@ -194,20 +191,19 @@ double getLongitudinalVelocity([[maybe_unused]] const T & p) } template <> -inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline double getLongitudinalVelocity(const autoware_planning_msgs::msg::PathPoint & p) { return p.longitudinal_velocity_mps; } template <> -inline double getLongitudinalVelocity( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline double getLongitudinalVelocity(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.longitudinal_velocity_mps; } template <> -inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline double getLongitudinalVelocity(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.longitudinal_velocity_mps; } @@ -233,21 +229,21 @@ inline void setPose(const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::P template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPoint & p) + const geometry_msgs::msg::Pose & pose, autoware_planning_msgs::msg::PathPoint & p) { p.pose = pose; } template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) + const geometry_msgs::msg::Pose & pose, tier4_planning_msgs::msg::PathPointWithLaneId & p) { p.point.pose = pose; } template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) + const geometry_msgs::msg::Pose & pose, autoware_planning_msgs::msg::TrajectoryPoint & p) { p.pose = pose; } @@ -269,21 +265,21 @@ void setLongitudinalVelocity([[maybe_unused]] const float velocity, [[maybe_unus template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) + const float velocity, autoware_planning_msgs::msg::TrajectoryPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::PathPoint & p) + const float velocity, autoware_planning_msgs::msg::PathPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) + const float velocity, tier4_planning_msgs::msg::PathPointWithLaneId & p) { p.point.longitudinal_velocity_mps = velocity; } @@ -581,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/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp new file mode 100644 index 0000000000000..f06770c2925c7 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/constants.hpp @@ -0,0 +1,24 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ + +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 autoware::universe_utils + +#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/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp new file mode 100644 index 0000000000000..19a59523c7f08 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/trigonometry.hpp @@ -0,0 +1,31 @@ +// 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 AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_ + +#include + +namespace autoware::universe_utils +{ + +float sin(float radian); + +float cos(float radian); + +std::pair sin_and_cos(float radian); + +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__TRIGONOMETRY_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp new file mode 100644 index 0000000000000..36ce8e9f39514 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/unit_conversion.hpp @@ -0,0 +1,41 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__UNIT_CONVERSION_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__UNIT_CONVERSION_HPP_ + +#include "autoware/universe_utils/math/constants.hpp" + +namespace autoware::universe_utils +{ +constexpr double deg2rad(const double deg) +{ + return deg * pi / 180.0; +} +constexpr double rad2deg(const double rad) +{ + return rad * 180.0 / pi; +} + +constexpr double kmph2mps(const double kmph) +{ + return kmph * 1000.0 / 3600.0; +} +constexpr double mps2kmph(const double mps) +{ + return mps * 3600.0 / 1000.0; +} +} // namespace autoware::universe_utils + +#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/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp new file mode 100644 index 0000000000000..6991962c420d1 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/marker_helper.hpp @@ -0,0 +1,81 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__MARKER_HELPER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__MARKER_HELPER_HPP_ + +#include + +#include + +#include +#include + +namespace autoware::universe_utils +{ +inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) +{ + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + point.z = z; + return point; +} + +inline geometry_msgs::msg::Quaternion createMarkerOrientation( + double x, double y, double z, double w) +{ + geometry_msgs::msg::Quaternion quaternion; + quaternion.x = x; + quaternion.y = y; + quaternion.z = z; + quaternion.w = w; + return quaternion; +} + +inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +visualization_msgs::msg::Marker createDefaultMarker( + const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, + const int32_t type, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color); + +visualization_msgs::msg::Marker createDeletedDefaultMarker( + const rclcpp::Time & now, const std::string & ns, const int32_t id); + +void appendMarkerArray( + const visualization_msgs::msg::MarkerArray & additional_marker_array, + visualization_msgs::msg::MarkerArray * marker_array, + const std::optional & current_time = {}); + +} // namespace autoware::universe_utils + +#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/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp new file mode 100644 index 0000000000000..1b7ea0bd69951 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp @@ -0,0 +1,164 @@ +// 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__UNIVERSE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ + +#include + +#include +#include +#include +#include +#include + +namespace autoware::universe_utils +{ + +inline rclcpp::SensorDataQoS SingleDepthSensorQoS() +{ + rclcpp::SensorDataQoS qos; + qos.get_rmw_qos_profile().depth = 1; + return qos; +} + +template +class InterProcessPollingSubscriber; + +template +class InterProcessPollingSubscriber::type> +{ +public: + using SharedPtr = + std::shared_ptr::type>>; + static SharedPtr create_subscription( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) + { + return std::make_shared>(node, topic_name, qos); + } + +private: + typename rclcpp::Subscription::SharedPtr subscriber_; + typename T::SharedPtr data_; + +public: + explicit InterProcessPollingSubscriber( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) + { + auto noexec_callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto noexec_subscription_options = rclcpp::SubscriptionOptions(); + noexec_subscription_options.callback_group = noexec_callback_group; + + subscriber_ = node->create_subscription( + topic_name, qos, + [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); }, + noexec_subscription_options); + if (qos.get_rmw_qos_profile().depth > 1) { + throw std::invalid_argument( + "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient " + "serialization while updateLatestData()"); + } + }; + /* + * @brief take and return the latest data from DDS queue if such data existed, otherwise return + * previous taken data("stale" data) + * @note if you want to ignore "stale" data, you should use takeNewData() + * instead + */ + typename T::ConstSharedPtr takeData() + { + auto new_data = std::make_shared(); + rclcpp::MessageInfo message_info; + const bool success = subscriber_->take(*new_data, message_info); + if (success) { + data_ = new_data; + } + + return data_; + }; + + /* + * @brief take and return the latest data from DDS queue if such data existed, otherwise return + * nullptr instead + * @note this API allows you to avoid redundant computation on the taken data which is unchanged + * since the previous cycle + */ + typename T::ConstSharedPtr takeNewData() + { + auto new_data = std::make_shared(); + rclcpp::MessageInfo message_info; + const bool success = subscriber_->take(*new_data, message_info); + if (success) { + data_ = new_data; + return data_; + } else { + return nullptr; + } + } +}; + +template +class InterProcessPollingSubscriber= 2)>::type> +{ +public: + using SharedPtr = + std::shared_ptr= 2)>::type>>; + static SharedPtr create_subscription( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{N}) + { + return std::make_shared>(node, topic_name, qos); + } + +private: + typename rclcpp::Subscription::SharedPtr subscriber_; + +public: + explicit InterProcessPollingSubscriber( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{N}) + { + auto noexec_callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto noexec_subscription_options = rclcpp::SubscriptionOptions(); + noexec_subscription_options.callback_group = noexec_callback_group; + + subscriber_ = node->create_subscription( + topic_name, qos, + [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); }, + noexec_subscription_options); + if (qos.get_rmw_qos_profile().depth < N) { + throw std::invalid_argument( + "InterProcessPollingSubscriber will be used with depth == " + std::to_string(N) + + ", which may cause inefficient serialization while updateLatestData()"); + } + }; + std::vector takeData() + { + std::vector data; + rclcpp::MessageInfo message_info; + for (int i = 0; i < N; ++i) { + auto datum = std::make_shared(); + if (subscriber_->take(*datum, message_info)) { + data.push_back(datum); + } else { + break; + } + } + return data; + }; +}; + +} // namespace autoware::universe_utils + +#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 80% 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 ffc845575a76f..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 { @@ -48,8 +48,9 @@ class TransformListener try { tf = tf_buffer_->lookupTransform(from, to, tf2::TimePointZero); } catch (tf2::TransformException & ex) { - RCLCPP_WARN( - logger_, "failed to get transform from %s to %s: %s", from.c_str(), to.c_str(), ex.what()); + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, "failed to get transform from %s to %s: %s", from.c_str(), + to.c_str(), ex.what()); return {}; } @@ -64,8 +65,9 @@ class TransformListener try { tf = tf_buffer_->lookupTransform(from, to, time, duration); } catch (tf2::TransformException & ex) { - RCLCPP_WARN( - logger_, "failed to get transform from %s to %s: %s", from.c_str(), to.c_str(), ex.what()); + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, "failed to get transform from %s to %s: %s", from.c_str(), + to.c_str(), ex.what()); return {}; } @@ -80,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/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp new file mode 100644 index 0000000000000..0b025160bacf8 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/backtrace.hpp @@ -0,0 +1,25 @@ +// 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 AUTOWARE__UNIVERSE_UTILS__SYSTEM__BACKTRACE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__BACKTRACE_HPP_ + +namespace autoware::universe_utils +{ + +void print_backtrace(); + +} // namespace autoware::universe_utils + +#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/autoware_universe_utils/package.xml b/common/autoware_universe_utils/package.xml new file mode 100644 index 0000000000000..de0461b5a841d --- /dev/null +++ b/common/autoware_universe_utils/package.xml @@ -0,0 +1,40 @@ + + + + autoware_universe_utils + 0.1.0 + The autoware_universe_utils package + Takamasa Horibe + Takayuki Murooka + Mamoru Sobue + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_internal_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_vehicle_msgs + builtin_interfaces + diagnostic_msgs + geometry_msgs + logging_demo + pcl_conversions + pcl_ros + rclcpp + tf2 + tf2_geometry_msgs + tier4_debug_msgs + tier4_planning_msgs + unique_identifier_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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 77% 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 79c9f9937cd4d..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) { @@ -118,21 +118,21 @@ Polygon2d rotatePolygon(const Polygon2d & polygon, const double angle) } Polygon2d toPolygon2d( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape) + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape) { Polygon2d polygon; - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - const auto point0 = tier4_autoware_utils::calcOffsetPose( + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + 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; @@ -140,7 +140,7 @@ Polygon2d toPolygon2d( appendPointToPolygon(polygon, point1); appendPointToPolygon(polygon, point2); appendPointToPolygon(polygon, point3); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { const double radius = shape.dimensions.x / 2.0; constexpr int circle_discrete_num = 6; for (int i = 0; i < circle_discrete_num; ++i) { @@ -157,7 +157,7 @@ Polygon2d toPolygon2d( pose.position.y; appendPointToPolygon(polygon, point); } - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { const double poly_yaw = tf2::getYaw(pose.orientation); const auto rotated_footprint = rotatePolygon(shape.footprint, poly_yaw); for (const auto rel_point : rotated_footprint.points) { @@ -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( - const autoware_auto_perception_msgs::msg::DetectedObject & object) +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( - const autoware_auto_perception_msgs::msg::TrackedObject & object) +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( - const autoware_auto_perception_msgs::msg::PredictedObject & object) +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); @@ -223,17 +227,17 @@ Polygon2d toFootprint( return isClockwise(polygon) ? polygon : inverseClockwise(polygon); } -double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) +double getArea(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return getRectangleArea(shape.dimensions); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return getCircleArea(shape.dimensions); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { 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/autoware_universe_utils/src/math/trigonometry.cpp b/common/autoware_universe_utils/src/math/trigonometry.cpp new file mode 100644 index 0000000000000..586b9075ba6d5 --- /dev/null +++ b/common/autoware_universe_utils/src/math/trigonometry.cpp @@ -0,0 +1,75 @@ +// 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 "autoware/universe_utils/math/trigonometry.hpp" + +#include "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/sin_table.hpp" + +#include + +namespace autoware::universe_utils +{ + +float sin(float radian) +{ + 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) % + discrete_arcs_num_360; + + float mul = 1.f; + if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) { + idx = 2 * discrete_arcs_num_90 - idx; + } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) { + mul = -1.f; + idx = idx - 2 * discrete_arcs_num_90; + } else if (3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90) { + mul = -1.f; + idx = 4 * discrete_arcs_num_90 - idx; + } + + return mul * g_sin_table[idx]; +} + +float cos(float radian) +{ + 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(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) % + discrete_arcs_num_360; + + if (idx < discrete_arcs_num_90) { + return {g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; + } else if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) { + idx = 2 * discrete_arcs_num_90 - idx; + return {g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]}; + } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) { + idx = idx - 2 * discrete_arcs_num_90; + return {-g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]}; + } else { // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90 + idx = 4 * discrete_arcs_num_90 - idx; + return {-g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; + } +} + +} // 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/autoware_universe_utils/src/ros/marker_helper.cpp b/common/autoware_universe_utils/src/ros/marker_helper.cpp new file mode 100644 index 0000000000000..507be41dea7bb --- /dev/null +++ b/common/autoware_universe_utils/src/ros/marker_helper.cpp @@ -0,0 +1,71 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/universe_utils/ros/marker_helper.hpp" + +namespace autoware::universe_utils +{ + +visualization_msgs::msg::Marker createDefaultMarker( + const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, + const int32_t type, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = frame_id; + marker.header.stamp = now; + marker.ns = ns; + marker.id = id; + marker.type = type; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + + marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); + marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); + marker.scale = scale; + marker.color = color; + marker.frame_locked = true; + + return marker; +} + +visualization_msgs::msg::Marker createDeletedDefaultMarker( + const rclcpp::Time & now, const std::string & ns, const int32_t id) +{ + visualization_msgs::msg::Marker marker; + + marker.header.stamp = now; + marker.ns = ns; + marker.id = id; + marker.action = visualization_msgs::msg::Marker::DELETE; + + return marker; +} + +void appendMarkerArray( + const visualization_msgs::msg::MarkerArray & additional_marker_array, + visualization_msgs::msg::MarkerArray * marker_array, + const std::optional & current_time) +{ + for (const auto & marker : additional_marker_array.markers) { + marker_array->markers.push_back(marker); + + if (current_time) { + marker_array->markers.back().header.stamp = current_time.value(); + } + } +} + +} // 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 88% 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 7370c3b650887..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,15 +130,15 @@ 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; const double y = 1.0; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; shape.dimensions.x = x; shape.dimensions.y = y; @@ -159,8 +159,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double diameter = 1.0; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; shape.dimensions.x = diameter; const auto poly = toPolygon2d(pose, shape); @@ -183,8 +183,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double y = 0.5; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::POLYGON; shape.footprint.points.push_back(createPoint32(-x, -y)); shape.footprint.points.push_back(createPoint32(-x, y)); shape.footprint.points.push_back(createPoint32(x, y)); @@ -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,14 +234,14 @@ 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; const double y = 2.0; - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; shape.dimensions.x = x; shape.dimensions.y = y; @@ -252,8 +252,8 @@ TEST(boost_geometry, boost_getArea) { // cylinder const double diameter = 1.0; - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; shape.dimensions.x = diameter; const double area = getArea(shape); @@ -265,8 +265,8 @@ TEST(boost_geometry, boost_getArea) const double y = 2.0; // clock wise - autoware_auto_perception_msgs::msg::Shape clock_wise_shape; - clock_wise_shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape clock_wise_shape; + clock_wise_shape.type = autoware_perception_msgs::msg::Shape::POLYGON; clock_wise_shape.footprint.points.push_back(createPoint32(0.0, 0.0)); clock_wise_shape.footprint.points.push_back(createPoint32(0.0, y)); clock_wise_shape.footprint.points.push_back(createPoint32(x, y)); @@ -276,8 +276,8 @@ TEST(boost_geometry, boost_getArea) EXPECT_DOUBLE_EQ(clock_wise_area, -x * y); // anti clock wise - autoware_auto_perception_msgs::msg::Shape anti_clock_wise_shape; - anti_clock_wise_shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape anti_clock_wise_shape; + anti_clock_wise_shape.type = autoware_perception_msgs::msg::Shape::POLYGON; anti_clock_wise_shape.footprint.points.push_back(createPoint32(0.0, 0.0)); anti_clock_wise_shape.footprint.points.push_back(createPoint32(x, 0.0)); anti_clock_wise_shape.footprint.points.push_back(createPoint32(x, y)); @@ -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/autoware_universe_utils/test/src/geometry/test_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp new file mode 100644 index 0000000000000..194cd7d503d12 --- /dev/null +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -0,0 +1,1831 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" + +#include + +#include + +#include + +constexpr double epsilon = 1e-6; + +TEST(geometry, getPoint) +{ + using autoware::universe_utils::getPoint; + + const double x_ans = 1.0; + const double y_ans = 2.0; + const double z_ans = 3.0; + + { + struct AnyPoint + { + double x; + double y; + double z; + }; + const AnyPoint p{x_ans, y_ans, z_ans}; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + geometry_msgs::msg::Point p; + p.x = x_ans; + p.y = y_ans; + p.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + geometry_msgs::msg::Pose p; + p.position.x = x_ans; + p.position.y = y_ans; + p.position.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + geometry_msgs::msg::PoseStamped p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose.pose.position.x = x_ans; + p.pose.pose.position.y = y_ans; + p.pose.pose.position.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + autoware_planning_msgs::msg::PathPoint p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } + + { + autoware_planning_msgs::msg::TrajectoryPoint p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); + } +} + +TEST(geometry, getPose) +{ + using autoware::universe_utils::getPose; + + const double x_ans = 1.0; + const double y_ans = 2.0; + const double z_ans = 3.0; + const double q_x_ans = 0.1; + const double q_y_ans = 0.2; + const double q_z_ans = 0.3; + const double q_w_ans = 0.4; + + { + geometry_msgs::msg::Pose p; + p.position.x = x_ans; + p.position.y = y_ans; + p.position.z = z_ans; + p.orientation.x = q_x_ans; + p.orientation.y = q_y_ans; + p.orientation.z = q_z_ans; + p.orientation.w = q_w_ans; + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } + + { + geometry_msgs::msg::PoseStamped p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + p.pose.orientation.x = q_x_ans; + p.pose.orientation.y = q_y_ans; + p.pose.orientation.z = q_z_ans; + p.pose.orientation.w = q_w_ans; + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } + + { + autoware_planning_msgs::msg::PathPoint p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + p.pose.orientation.x = q_x_ans; + p.pose.orientation.y = q_y_ans; + p.pose.orientation.z = q_z_ans; + p.pose.orientation.w = q_w_ans; + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } + + { + autoware_planning_msgs::msg::TrajectoryPoint p; + p.pose.position.x = x_ans; + p.pose.position.y = y_ans; + p.pose.position.z = z_ans; + p.pose.orientation.x = q_x_ans; + p.pose.orientation.y = q_y_ans; + p.pose.orientation.z = q_z_ans; + p.pose.orientation.w = q_w_ans; + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } +} + +TEST(geometry, getLongitudinalVelocity) +{ + using autoware::universe_utils::getLongitudinalVelocity; + + const double velocity = 1.0; + + { + autoware_planning_msgs::msg::PathPoint p; + p.longitudinal_velocity_mps = velocity; + EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); + } + + { + autoware_planning_msgs::msg::TrajectoryPoint p; + p.longitudinal_velocity_mps = velocity; + EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); + } +} + +TEST(geometry, setPose) +{ + using autoware::universe_utils::setPose; + + const double x_ans = 1.0; + const double y_ans = 2.0; + const double z_ans = 3.0; + const double q_x_ans = 0.1; + const double q_y_ans = 0.2; + const double q_z_ans = 0.3; + const double q_w_ans = 0.4; + + geometry_msgs::msg::Pose p; + p.position.x = x_ans; + p.position.y = y_ans; + p.position.z = z_ans; + p.orientation.x = q_x_ans; + p.orientation.y = q_y_ans; + p.orientation.z = q_z_ans; + p.orientation.w = q_w_ans; + + { + geometry_msgs::msg::Pose p_out{}; + setPose(p, p_out); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } + + { + geometry_msgs::msg::PoseStamped p_out{}; + setPose(p, p_out); + EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans); + } + + { + autoware_planning_msgs::msg::PathPoint p_out{}; + setPose(p, p_out); + EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans); + } + + { + autoware_planning_msgs::msg::TrajectoryPoint p_out{}; + setPose(p, p_out); + EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans); + } +} + +TEST(geometry, 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)); + setOrientation(orientation, p); + + EXPECT_DOUBLE_EQ(p.orientation.x, orientation.x); + EXPECT_DOUBLE_EQ(p.orientation.y, orientation.y); + EXPECT_DOUBLE_EQ(p.orientation.z, orientation.z); + EXPECT_DOUBLE_EQ(p.orientation.w, orientation.w); +} + +TEST(geometry, setLongitudinalVelocity) +{ + using autoware::universe_utils::setLongitudinalVelocity; + + const double velocity = 1.0; + + { + autoware_planning_msgs::msg::PathPoint p{}; + setLongitudinalVelocity(velocity, p); + EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity); + } + + { + autoware_planning_msgs::msg::TrajectoryPoint p{}; + setLongitudinalVelocity(velocity, p); + EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity); + } +} + +TEST(geometry, 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); + EXPECT_DOUBLE_EQ(p_out.y, 2.0); + EXPECT_DOUBLE_EQ(p_out.z, 3.0); +} + +TEST(geometry, 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 = + createQuaternion(0.18257419, 0.36514837, 0.54772256, 0.73029674); + EXPECT_DOUBLE_EQ(q_out.x, 0.18257419); + EXPECT_DOUBLE_EQ(q_out.y, 0.36514837); + EXPECT_DOUBLE_EQ(q_out.z, 0.54772256); + EXPECT_DOUBLE_EQ(q_out.w, 0.73029674); +} + +TEST(geometry, 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); + EXPECT_DOUBLE_EQ(v_out.y, 2.0); + EXPECT_DOUBLE_EQ(v_out.z, 3.0); +} + +TEST(geometry, createQuaternionFromRPY) +{ + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + + { + const auto q_out = createQuaternionFromRPY(0, 0, 0); + EXPECT_DOUBLE_EQ(q_out.x, 0.0); + EXPECT_DOUBLE_EQ(q_out.y, 0.0); + EXPECT_DOUBLE_EQ(q_out.z, 0.0); + EXPECT_DOUBLE_EQ(q_out.w, 1.0); + } + + { + const auto q_out = createQuaternionFromRPY(0, 0, deg2rad(90)); + EXPECT_DOUBLE_EQ(q_out.x, 0.0); + EXPECT_DOUBLE_EQ(q_out.y, 0.0); + EXPECT_DOUBLE_EQ(q_out.z, 0.70710678118654757); + EXPECT_DOUBLE_EQ(q_out.w, 0.70710678118654757); + } + + { + const auto q_out = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + EXPECT_DOUBLE_EQ(q_out.x, 0.17677669529663687); + EXPECT_DOUBLE_EQ(q_out.y, 0.30618621784789724); + EXPECT_DOUBLE_EQ(q_out.z, 0.17677669529663692); + EXPECT_DOUBLE_EQ(q_out.w, 0.91855865354369193); + } +} + +TEST(geometry, createQuaternionFromYaw) +{ + using autoware::universe_utils::createQuaternionFromYaw; + using autoware::universe_utils::deg2rad; + + { + const auto q_out = createQuaternionFromYaw(0); + EXPECT_DOUBLE_EQ(q_out.x, 0.0); + EXPECT_DOUBLE_EQ(q_out.y, 0.0); + EXPECT_DOUBLE_EQ(q_out.z, 0.0); + EXPECT_DOUBLE_EQ(q_out.w, 1.0); + } + + { + const auto q_out = createQuaternionFromYaw(deg2rad(90)); + EXPECT_DOUBLE_EQ(q_out.x, 0.0); + EXPECT_DOUBLE_EQ(q_out.y, 0.0); + EXPECT_DOUBLE_EQ(q_out.z, 0.70710678118654757); + EXPECT_DOUBLE_EQ(q_out.w, 0.70710678118654757); + } + + { + const auto q_out = createQuaternionFromYaw(deg2rad(30)); + EXPECT_DOUBLE_EQ(q_out.x, 0.0); + EXPECT_DOUBLE_EQ(q_out.y, 0.0); + EXPECT_DOUBLE_EQ(q_out.z, 0.25881904510252074); + EXPECT_DOUBLE_EQ(q_out.w, 0.96592582628906831); + } +} + +TEST(geometry, calcElevationAngle) +{ + 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); + const auto p2 = createPoint(1.0, 1.0, -10.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(-90.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, -std::sqrt(3.0)); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(-60.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, -1.0); + const auto p2 = createPoint(0.0, 1.0, -2.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(-45.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 1.0); + const auto p2 = createPoint(1.0, 1.0, 1.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(0.0), epsilon); + } + { + const auto p1 = createPoint(-100, -100, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(0.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 1.0); + const auto p2 = createPoint(0.0, 1.0, 2.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(45.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, std::sqrt(3.0)); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(60.0), epsilon); + } + { + const auto p1 = createPoint(1.0, 1.0, 1.0); + const auto p2 = createPoint(1.0, 1.0, 10.0); + EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(90.0), epsilon); + } +} + +TEST(geometry, calcAzimuthAngle) +{ + 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); + const auto p2 = createPoint(-100, -epsilon, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-180.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 2.0); + const auto p2 = createPoint(-1.0, -1.0, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-135.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 10.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 6.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-90.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, -1.0, 4.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-45.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 1.0, 3.3); + const auto p2 = createPoint(10.0, 1.0, -10.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(0.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 2.0); + const auto p2 = createPoint(1.0, 1.0, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(45.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 10.0); + const auto p2 = createPoint(0.0, 10.0, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(90.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 2.0); + const auto p2 = createPoint(-1.0, 1.0, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(135.0), epsilon); + } + { + const auto p1 = createPoint(0.0, 0.0, 9.0); + const auto p2 = createPoint(-100, epsilon, 0.0); + EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(180.0), epsilon); + } +} + +TEST(geometry, calcDistance2d) +{ + using autoware::universe_utils::calcDistance2d; + + geometry_msgs::msg::Point point; + point.x = 1.0; + point.y = 2.0; + point.z = 3.0; + + geometry_msgs::msg::Pose pose; + pose.position.x = 5.0; + pose.position.y = 5.0; + pose.position.z = 4.0; + + EXPECT_DOUBLE_EQ(calcDistance2d(point, pose), 5.0); +} + +TEST(geometry, calcSquaredDistance2d) +{ + using autoware::universe_utils::calcSquaredDistance2d; + + geometry_msgs::msg::Point point; + point.x = 1.0; + point.y = 2.0; + point.z = 3.0; + + geometry_msgs::msg::Pose pose; + pose.position.x = 5.0; + pose.position.y = 5.0; + pose.position.z = 4.0; + + EXPECT_DOUBLE_EQ(calcSquaredDistance2d(point, pose), 25.0); +} + +TEST(geometry, calcDistance3d) +{ + using autoware::universe_utils::calcDistance3d; + + geometry_msgs::msg::Point point; + point.x = 1.0; + point.y = 2.0; + point.z = 3.0; + + geometry_msgs::msg::Pose pose; + pose.position.x = 3.0; + pose.position.y = 4.0; + pose.position.z = 4.0; + + EXPECT_DOUBLE_EQ(calcDistance3d(point, pose), 3.0); +} + +TEST(geometry, getRPY) +{ + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getRPY; + + { + const double ans_roll = deg2rad(5); + const double ans_pitch = deg2rad(10); + const double ans_yaw = deg2rad(15); + const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); + const auto rpy = getRPY(quat); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } + { + const double ans_roll = deg2rad(0); + const double ans_pitch = deg2rad(5); + const double ans_yaw = deg2rad(-10); + const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); + const auto rpy = getRPY(quat); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } + { + const double ans_roll = deg2rad(30); + const double ans_pitch = deg2rad(-20); + const double ans_yaw = deg2rad(0); + const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); + const auto rpy = getRPY(quat); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } +} + +TEST(geometry, getRPY_wrapper) +{ + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::getRPY; + + { + const double ans_roll = deg2rad(45); + const double ans_pitch = deg2rad(25); + const double ans_yaw = deg2rad(-5); + const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); + + // geometry_msgs::Pose + { + geometry_msgs::msg::Pose pose{}; + pose.orientation = quat; + const auto rpy = getRPY(pose); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } + // geometry_msgs::PoseStamped + { + geometry_msgs::msg::PoseStamped pose{}; + pose.pose.orientation = quat; + const auto rpy = getRPY(pose); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } + // geometry_msgs::PoseWithCovarianceStamped + { + geometry_msgs::msg::PoseWithCovarianceStamped pose{}; + pose.pose.pose.orientation = quat; + const auto rpy = getRPY(pose); + EXPECT_NEAR(rpy.x, ans_roll, epsilon); + EXPECT_NEAR(rpy.y, ans_pitch, epsilon); + EXPECT_NEAR(rpy.z, ans_yaw, epsilon); + } + } +} + +TEST(geometry, transform2pose) +{ + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::transform2pose; + + { + geometry_msgs::msg::Transform transform; + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.translation.z = 3.0; + transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::Pose pose = transform2pose(transform); + + EXPECT_DOUBLE_EQ(transform.translation.x, pose.position.x); + EXPECT_DOUBLE_EQ(transform.translation.y, pose.position.y); + EXPECT_DOUBLE_EQ(transform.translation.z, pose.position.z); + EXPECT_DOUBLE_EQ(transform.rotation.x, pose.orientation.x); + EXPECT_DOUBLE_EQ(transform.rotation.y, pose.orientation.y); + EXPECT_DOUBLE_EQ(transform.rotation.z, pose.orientation.z); + EXPECT_DOUBLE_EQ(transform.rotation.w, pose.orientation.w); + } + + { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.frame_id = "test"; + transform_stamped.header.stamp = rclcpp::Time(2.0); + transform_stamped.transform.translation.x = 1.0; + transform_stamped.transform.translation.y = 2.0; + transform_stamped.transform.translation.z = 3.0; + transform_stamped.transform.rotation = + createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::PoseStamped pose_stamped = transform2pose(transform_stamped); + + EXPECT_EQ(transform_stamped.header.frame_id, pose_stamped.header.frame_id); + EXPECT_DOUBLE_EQ( + rclcpp::Time(transform_stamped.header.stamp).seconds(), + rclcpp::Time(pose_stamped.header.stamp).seconds()); + + EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.x, pose_stamped.pose.position.x); + EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.y, pose_stamped.pose.position.y); + EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.z, pose_stamped.pose.position.z); + EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.x, pose_stamped.pose.orientation.x); + EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.y, pose_stamped.pose.orientation.y); + EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.z, pose_stamped.pose.orientation.z); + EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.w, pose_stamped.pose.orientation.w); + } +} + +TEST(geometry, pose2transform) +{ + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::pose2transform; + + { + geometry_msgs::msg::Pose pose; + pose.position.x = 1.0; + pose.position.y = 2.0; + pose.position.z = 3.0; + pose.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::Transform transform = pose2transform(pose); + + EXPECT_DOUBLE_EQ(pose.position.x, transform.translation.x); + EXPECT_DOUBLE_EQ(pose.position.y, transform.translation.y); + EXPECT_DOUBLE_EQ(pose.position.z, transform.translation.z); + EXPECT_DOUBLE_EQ(pose.orientation.x, transform.rotation.x); + EXPECT_DOUBLE_EQ(pose.orientation.y, transform.rotation.y); + EXPECT_DOUBLE_EQ(pose.orientation.z, transform.rotation.z); + EXPECT_DOUBLE_EQ(pose.orientation.w, transform.rotation.w); + } + + { + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.frame_id = "test"; + pose_stamped.header.stamp = rclcpp::Time(2.0); + pose_stamped.pose.position.x = 1.0; + pose_stamped.pose.position.y = 2.0; + pose_stamped.pose.position.z = 3.0; + pose_stamped.pose.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + const std::string child_frame_id = "child"; + + const geometry_msgs::msg::TransformStamped transform_stamped = + pose2transform(pose_stamped, child_frame_id); + + EXPECT_EQ(pose_stamped.header.frame_id, transform_stamped.header.frame_id); + EXPECT_EQ(child_frame_id, transform_stamped.child_frame_id); + EXPECT_DOUBLE_EQ( + rclcpp::Time(pose_stamped.header.stamp).seconds(), + rclcpp::Time(transform_stamped.header.stamp).seconds()); + + EXPECT_DOUBLE_EQ(pose_stamped.pose.position.x, transform_stamped.transform.translation.x); + EXPECT_DOUBLE_EQ(pose_stamped.pose.position.y, transform_stamped.transform.translation.y); + EXPECT_DOUBLE_EQ(pose_stamped.pose.position.z, transform_stamped.transform.translation.z); + EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.x, transform_stamped.transform.rotation.x); + EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.y, transform_stamped.transform.rotation.y); + EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.z, transform_stamped.transform.rotation.z); + EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.w, transform_stamped.transform.rotation.w); + } +} + +TEST(geometry, point2tfVector) +{ + using autoware::universe_utils::createQuaternionFromRPY; + using autoware::universe_utils::deg2rad; + using autoware::universe_utils::point2tfVector; + + // Point + { + geometry_msgs::msg::Point src; + src.x = 1.0; + src.y = 2.0; + src.z = 3.0; + + geometry_msgs::msg::Point dst; + dst.x = 10.0; + dst.y = 5.0; + dst.z = -5.0; + + const auto vec = point2tfVector(src, dst); + + EXPECT_DOUBLE_EQ(vec.x(), 9.0); + EXPECT_DOUBLE_EQ(vec.y(), 3.0); + EXPECT_DOUBLE_EQ(vec.z(), -8.0); + } + + // Pose + { + geometry_msgs::msg::Pose src; + src.position.x = 1.0; + src.position.y = 2.0; + src.position.z = 3.0; + src.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + geometry_msgs::msg::Pose dst; + dst.position.x = 10.0; + dst.position.y = 5.0; + dst.position.z = -5.0; + dst.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(10), deg2rad(10)); + + const auto vec = point2tfVector(src, dst); + + EXPECT_DOUBLE_EQ(vec.x(), 9.0); + EXPECT_DOUBLE_EQ(vec.y(), 3.0); + EXPECT_DOUBLE_EQ(vec.z(), -8.0); + } + + // Point and Pose + { + geometry_msgs::msg::Point src; + src.x = 1.0; + src.y = 2.0; + src.z = 3.0; + + geometry_msgs::msg::Pose dst; + dst.position.x = 10.0; + dst.position.y = 5.0; + dst.position.z = -5.0; + dst.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(10), deg2rad(10)); + + const auto vec = point2tfVector(src, dst); + + EXPECT_DOUBLE_EQ(vec.x(), 9.0); + EXPECT_DOUBLE_EQ(vec.y(), 3.0); + EXPECT_DOUBLE_EQ(vec.z(), -8.0); + } +} + +TEST(geometry, 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); + + geometry_msgs::msg::Transform transform; + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.rotation = createQuaternionFromRPY(0, 0, deg2rad(30)); + + const Point2d p_transformed = transformPoint(p, transform); + + EXPECT_DOUBLE_EQ(p_transformed.x(), 0.86602540378443882); + EXPECT_DOUBLE_EQ(p_transformed.y(), 4.2320508075688767); + } + + { + const Point3d p(1.0, 2.0, 3.0); + + geometry_msgs::msg::Transform transform; + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.translation.z = 3.0; + transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const Point3d p_transformed = transformPoint(p, transform); + + EXPECT_DOUBLE_EQ(p_transformed.x(), 3.1919872981077804); + EXPECT_DOUBLE_EQ(p_transformed.y(), 3.5334936490538906); + EXPECT_DOUBLE_EQ(p_transformed.z(), 5.6160254037844393); + } + + { + const Eigen::Vector3d p(1.0, 2.0, 3.0); + + geometry_msgs::msg::Pose pose_transform; + pose_transform.position.x = 1.0; + pose_transform.position.y = 2.0; + pose_transform.position.z = 3.0; + pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const Eigen::Vector3d p_transformed = transformPoint(p, pose_transform); + + EXPECT_DOUBLE_EQ(p_transformed.x(), 3.1919872981077804); + EXPECT_DOUBLE_EQ(p_transformed.y(), 3.5334936490538906); + EXPECT_DOUBLE_EQ(p_transformed.z(), 5.6160254037844393); + } + + { + geometry_msgs::msg::Point p; + p.x = 1.0; + p.y = 2.0; + p.z = 3.0; + + geometry_msgs::msg::Pose pose_transform; + pose_transform.position.x = 1.0; + pose_transform.position.y = 2.0; + pose_transform.position.z = 3.0; + pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::Point p_transformed = transformPoint(p, pose_transform); + + EXPECT_DOUBLE_EQ(p_transformed.x, 3.1919872981077804); + EXPECT_DOUBLE_EQ(p_transformed.y, 3.5334936490538906); + EXPECT_DOUBLE_EQ(p_transformed.z, 5.6160254037844393); + } + + { + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = 2.0; + p.z = 3.0; + + geometry_msgs::msg::Pose pose_transform; + pose_transform.position.x = 1.0; + pose_transform.position.y = 2.0; + pose_transform.position.z = 3.0; + pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::Point32 p_transformed = transformPoint(p, pose_transform); + + EXPECT_DOUBLE_EQ(p_transformed.x, 3.1919872760772705); + EXPECT_DOUBLE_EQ(p_transformed.y, 3.5334937572479248); + EXPECT_DOUBLE_EQ(p_transformed.z, 5.616025447845459); + } +} + +TEST(geometry, 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; + pose.position.y = 4.0; + pose.position.z = 6.0; + pose.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(20), deg2rad(30)); + + // with transform + { + geometry_msgs::msg::Transform transform; + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.translation.z = 3.0; + transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::Pose pose_transformed = transformPose(pose, transform); + + EXPECT_NEAR(pose_transformed.position.x, 5.3839745962155598, epsilon); + EXPECT_NEAR(pose_transformed.position.y, 5.0669872981077804, epsilon); + EXPECT_NEAR(pose_transformed.position.z, 8.2320508075688785, epsilon); + EXPECT_NEAR(pose_transformed.orientation.x, 0.24304508436548405, epsilon); + EXPECT_NEAR(pose_transformed.orientation.y, 0.4296803495383052, epsilon); + EXPECT_NEAR(pose_transformed.orientation.z, 0.40981009820187703, epsilon); + EXPECT_NEAR(pose_transformed.orientation.w, 0.76704600096616271, epsilon); + } + + // with pose_transform + { + geometry_msgs::msg::Pose pose_transform; + pose_transform.position.x = 1.0; + pose_transform.position.y = 2.0; + pose_transform.position.z = 3.0; + pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::Pose pose_transformed = transformPose(pose, pose_transform); + + EXPECT_NEAR(pose_transformed.position.x, 5.3839745962155598, epsilon); + EXPECT_NEAR(pose_transformed.position.y, 5.0669872981077804, epsilon); + EXPECT_NEAR(pose_transformed.position.z, 8.2320508075688785, epsilon); + EXPECT_NEAR(pose_transformed.orientation.x, 0.24304508436548405, epsilon); + EXPECT_NEAR(pose_transformed.orientation.y, 0.4296803495383052, epsilon); + EXPECT_NEAR(pose_transformed.orientation.z, 0.40981009820187703, epsilon); + EXPECT_NEAR(pose_transformed.orientation.w, 0.76704600096616271, epsilon); + } +} + +TEST(geometry, 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; + pose.position.y = 4.0; + pose.position.z = 6.0; + pose.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(20), deg2rad(30)); + + // with transform + { + geometry_msgs::msg::Transform transform; + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.translation.z = 3.0; + transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::Pose pose_transformed = inverseTransformPose(pose, transform); + + EXPECT_NEAR(pose_transformed.position.x, 0.11602540378443926, epsilon); + EXPECT_NEAR(pose_transformed.position.y, 2.8325317547305482, epsilon); + EXPECT_NEAR(pose_transformed.position.z, 2.4419872981077804, epsilon); + EXPECT_NEAR(pose_transformed.orientation.x, -0.17298739392508941, epsilon); + EXPECT_NEAR(pose_transformed.orientation.y, -0.08189960831908924, epsilon); + EXPECT_NEAR(pose_transformed.orientation.z, 0.029809019626209146, epsilon); + EXPECT_NEAR(pose_transformed.orientation.w, 0.98106026219040698, epsilon); + } + + // with pose_transform + { + geometry_msgs::msg::Pose pose_transform; + pose_transform.position.x = 1.0; + pose_transform.position.y = 2.0; + pose_transform.position.z = 3.0; + pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const geometry_msgs::msg::Pose pose_transformed = inverseTransformPose(pose, pose_transform); + + EXPECT_NEAR(pose_transformed.position.x, 0.11602540378443926, epsilon); + EXPECT_NEAR(pose_transformed.position.y, 2.8325317547305482, epsilon); + EXPECT_NEAR(pose_transformed.position.z, 2.4419872981077804, epsilon); + EXPECT_NEAR(pose_transformed.orientation.x, -0.17298739392508941, epsilon); + EXPECT_NEAR(pose_transformed.orientation.y, -0.08189960831908924, epsilon); + EXPECT_NEAR(pose_transformed.orientation.z, 0.029809019626209146, epsilon); + EXPECT_NEAR(pose_transformed.orientation.w, 0.98106026219040698, epsilon); + } +} + +TEST(geometry, inverseTransformPoint) +{ + 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; + pose_transform.position.y = 2.0; + pose_transform.position.z = 3.0; + pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + // calc expected values + geometry_msgs::msg::Pose pose; + pose.position.x = 2.0; + pose.position.y = 4.0; + pose.position.z = 6.0; + pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + const geometry_msgs::msg::Pose pose_transformed = inverseTransformPose(pose, pose_transform); + const geometry_msgs::msg::Point expected_p = pose_transformed.position; + + geometry_msgs::msg::Point p; + p.x = 2.0; + p.y = 4.0; + p.z = 6.0; + const geometry_msgs::msg::Point p_transformed = inverseTransformPoint(p, pose_transform); + EXPECT_NEAR(p_transformed.x, expected_p.x, epsilon); + EXPECT_NEAR(p_transformed.y, expected_p.y, epsilon); + EXPECT_NEAR(p_transformed.z, expected_p.z, epsilon); +} + +TEST(geometry, 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}}; + + geometry_msgs::msg::Transform transform; + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.translation.z = 3.0; + transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + const MultiPoint3d ps_transformed = transformVector(ps, transform); + + EXPECT_DOUBLE_EQ(ps_transformed.at(0).x(), 3.1919872981077804); + EXPECT_DOUBLE_EQ(ps_transformed.at(0).y(), 3.5334936490538906); + EXPECT_DOUBLE_EQ(ps_transformed.at(0).z(), 5.6160254037844393); + + EXPECT_DOUBLE_EQ(ps_transformed.at(1).x(), 4.350480947161671); + EXPECT_DOUBLE_EQ(ps_transformed.at(1).y(), 4.625); + EXPECT_DOUBLE_EQ(ps_transformed.at(1).z(), 6.299038105676658); + } +} + +TEST(geometry, calcCurvature) +{ + using autoware::universe_utils::calcCurvature; + // Straight Line + { + 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 = 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 = 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 = 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 = 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 = 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)); + ASSERT_ANY_THROW(calcCurvature(p1, p2, p2)); + } +} + +TEST(geometry, calcOffsetPose) +{ + 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 + { + geometry_msgs::msg::Pose p_in; + p_in.position = createPoint(1.0, 2.0, 3.0); + p_in.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + + const auto p_out = calcOffsetPose(p_in, 1.0, 1.0, 1.0); + + EXPECT_DOUBLE_EQ(p_out.position.x, 2.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 3.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 4.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0); + } + + { + geometry_msgs::msg::Pose p_in; + p_in.position = createPoint(2.0, 3.0, 1.0); + p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(90)); + + const auto p_out = calcOffsetPose(p_in, 2.0, 1.0, 3.0); + + EXPECT_DOUBLE_EQ(p_out.position.x, 1.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 5.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 4.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.70710678118654757); + EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.70710678118654757); + } + + { + geometry_msgs::msg::Pose p_in; + p_in.position = createPoint(2.0, 1.0, 1.0); + p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30)); + + const auto p_out = calcOffsetPose(p_in, 2.0, 0.0, -1.0); + + EXPECT_DOUBLE_EQ(p_out.position.x, 3.73205080756887729); + EXPECT_DOUBLE_EQ(p_out.position.y, 2.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.25881904510252068); + EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.96592582628906831); + } + + // Only rotation + { + geometry_msgs::msg::Pose p_in; + p_in.position = createPoint(2.0, 1.0, 1.0); + p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30)); + + const auto p_out = calcOffsetPose(p_in, 0.0, 0.0, 0.0, deg2rad(20)); + + EXPECT_DOUBLE_EQ(p_out.position.x, 2.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 1.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 1.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon); + EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon); + } + + // Both translation and rotation + { + geometry_msgs::msg::Pose p_in; + p_in.position = createPoint(2.0, 1.0, 1.0); + p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30)); + + const auto p_out = calcOffsetPose(p_in, 2.0, 0.0, -1.0, deg2rad(20)); + + EXPECT_DOUBLE_EQ(p_out.position.x, 3.73205080756887729); + EXPECT_DOUBLE_EQ(p_out.position.y, 2.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon); + EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon); + } +} + +TEST(geometry, 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; + + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(3.0, 0.0, 0.0); + dst_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + + EXPECT_TRUE(isDrivingForward(src_pose, dst_pose)); + } + + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(180)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(3.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(180)); + + EXPECT_FALSE(isDrivingForward(src_pose, dst_pose)); + } + + // Boundary Condition + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(3.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90)); + + EXPECT_TRUE(isDrivingForward(src_pose, dst_pose)); + } + + // Boundary Condition + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90 + epsilon)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(3.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90 + epsilon)); + + EXPECT_FALSE(isDrivingForward(src_pose, dst_pose)); + } +} + +TEST(geometry, calcInterpolatedPoint) +{ + using autoware::universe_utils::calcInterpolatedPoint; + using autoware::universe_utils::createPoint; + + { + const auto src_point = createPoint(0.0, 0.0, 0.0); + const auto dst_point = createPoint(3.0, 0.0, 0.0); + + const double epsilon = 1e-3; + for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPoint(src_point, dst_point, ratio); + + EXPECT_DOUBLE_EQ(p_out.x, 3.0 * ratio); + EXPECT_DOUBLE_EQ(p_out.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.z, 0.0); + } + } + + // Same points are given + { + const auto src_point = createPoint(0.0, 0.0, 0.0); + const auto dst_point = createPoint(0.0, 0.0, 0.0); + + const double epsilon = 1e-3; + for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPoint(src_point, dst_point, ratio); + + EXPECT_DOUBLE_EQ(p_out.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.z, 0.0); + } + } + + // Boundary Condition (Negative Ratio) + { + const auto src_point = createPoint(0.0, 0.0, 0.0); + const auto dst_point = createPoint(3.0, 0.0, 0.0); + + const auto p_out = calcInterpolatedPoint(src_point, dst_point, -10.0); + EXPECT_DOUBLE_EQ(p_out.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.z, 0.0); + } + + // Boundary Condition (Positive Ratio larger than 1.0) + { + const auto src_point = createPoint(0.0, 0.0, 0.0); + const auto dst_point = createPoint(3.0, 0.0, 0.0); + + const auto p_out = calcInterpolatedPoint(src_point, dst_point, 10.0); + EXPECT_DOUBLE_EQ(p_out.x, 3.0); + EXPECT_DOUBLE_EQ(p_out.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.z, 0.0); + } +} + +TEST(geometry, calcInterpolatedPose) +{ + 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 + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(3.0, 0.0, 0.0); + dst_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + + for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); + + EXPECT_DOUBLE_EQ(p_out.position.x, 3.0 * ratio); + EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0); + } + } + + // Boundary Condition (Negative Ratio) + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(1.0, 1.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); + + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, -10.0); + + const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45)); + EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); + EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); + EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); + EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); + } + + // Boundary Condition (Positive Ratio larger than 1.0) + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(1.0, 1.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); + + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, 10.0); + + const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); + EXPECT_DOUBLE_EQ(p_out.position.x, 1.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 1.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); + EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); + EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); + EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); + } + + // Quaternion Interpolation + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(1.0, 1.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); + + for (double ratio = 0.0; ratio < 1.0 - epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); + + const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45)); + EXPECT_DOUBLE_EQ(p_out.position.x, 1.0 * ratio); + EXPECT_DOUBLE_EQ(p_out.position.y, 1.0 * ratio); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); + EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); + EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); + EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); + } + + // Boundary Condition (ratio = 1.0) + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, 1.0); + + const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); + EXPECT_DOUBLE_EQ(p_out.position.x, 1.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 1.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); + EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); + EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); + EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); + } + + // Same poses are given + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(0.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); + + EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0); + } + } + + // Same points are given (different orientation) + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(0.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45)); + + for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); + + const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45)); + EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); + EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); + EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); + EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); + } + } + + // Driving Backward + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(5.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180)); + + for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); + + const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180)); + EXPECT_DOUBLE_EQ(p_out.position.x, 5.0 * ratio); + EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); + EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); + EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); + EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); + } + } +} + +TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation) +{ + 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 + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(3.0, 0.0, 0.0); + dst_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + + for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false); + + EXPECT_DOUBLE_EQ(p_out.position.x, 3.0 * ratio); + EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0); + } + } + + // Boundary Condition (Negative Ratio) + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(1.0, 1.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); + + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, -10.0, false); + + const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); + EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); + EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); + EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); + } + + // Boundary Condition (Positive Ratio larger than 1.0) + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(1.0, 1.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); + + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, 10.0, false); + + const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); + EXPECT_DOUBLE_EQ(p_out.position.x, 1.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 1.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); + EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); + EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); + EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); + } + + // Quaternion Interpolation1 + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(0.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(90)); + + for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false); + + const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(90 * ratio)); + EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); + EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); + EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); + EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); + } + } + + // Quaternion Interpolation2 + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(1.0, 1.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); + + for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false); + + const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60 * ratio)); + EXPECT_DOUBLE_EQ(p_out.position.x, 1.0 * ratio); + EXPECT_DOUBLE_EQ(p_out.position.y, 1.0 * ratio); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); + EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); + EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); + EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); + } + } + + // Same points are given + { + geometry_msgs::msg::Pose src_pose; + src_pose.position = createPoint(0.0, 0.0, 0.0); + src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + geometry_msgs::msg::Pose dst_pose; + dst_pose.position = createPoint(0.0, 0.0, 0.0); + dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); + + for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { + const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false); + + EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0); + } + } +} + +TEST(geometry, getTwist) +{ + 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); + + geometry_msgs::msg::Twist twist = geometry_msgs::build() + .linear(createVector3(1.0, 2.0, 3.0)) + .angular(createVector3(0.1, 0.2, 0.3)); + + 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)); + + // getTwist + { + 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); + EXPECT_DOUBLE_EQ(t_out.angular.x, twist.angular.x); + EXPECT_DOUBLE_EQ(t_out.angular.y, twist.angular.y); + EXPECT_DOUBLE_EQ(t_out.angular.z, twist.angular.z); + } +} + +TEST(geometry, getTwistNorm) +{ + 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(autoware::universe_utils::calcNorm(twist_with_covariance.twist.linear), 5.0, epsilon); +} + +TEST(geometry, isTwistCovarianceValid) +{ + 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(autoware::universe_utils::isTwistCovarianceValid(twist_with_covariance), false); + + twist_with_covariance.covariance.at(0) = 1.0; + EXPECT_EQ(autoware::universe_utils::isTwistCovarianceValid(twist_with_covariance), true); +} + +TEST(geometry, intersect) +{ + using autoware::universe_utils::createPoint; + using autoware::universe_utils::intersect; + + { // Normally crossing + const auto p1 = createPoint(0.0, -1.0, 0.0); + const auto p2 = createPoint(0.0, 1.0, 0.0); + const auto p3 = createPoint(-1.0, 0.0, 0.0); + const auto p4 = createPoint(1.0, 0.0, 0.0); + const auto result = intersect(p1, p2, p3, p4); + + EXPECT_TRUE(result); + EXPECT_NEAR(result->x, 0.0, epsilon); + EXPECT_NEAR(result->y, 0.0, epsilon); + EXPECT_NEAR(result->z, 0.0, epsilon); + } + + { // No crossing + const auto p1 = createPoint(0.0, -1.0, 0.0); + const auto p2 = createPoint(0.0, 1.0, 0.0); + const auto p3 = createPoint(1.0, 0.0, 0.0); + const auto p4 = createPoint(3.0, 0.0, 0.0); + const auto result = intersect(p1, p2, p3, p4); + + EXPECT_FALSE(result); + } + + { // One segment is the point on the other's segment + const auto p1 = createPoint(0.0, -1.0, 0.0); + const auto p2 = createPoint(0.0, 1.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 0.0, 0.0); + const auto result = intersect(p1, p2, p3, p4); + + EXPECT_FALSE(result); + } + + { // One segment is the point not on the other's segment + const auto p1 = createPoint(0.0, -1.0, 0.0); + const auto p2 = createPoint(0.0, 1.0, 0.0); + const auto p3 = createPoint(1.0, 0.0, 0.0); + const auto p4 = createPoint(1.0, 0.0, 0.0); + const auto result = intersect(p1, p2, p3, p4); + + EXPECT_FALSE(result); + } + + { // Both segments are the points which are the same position + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 0.0, 0.0); + const auto result = intersect(p1, p2, p3, p4); + + EXPECT_FALSE(result); + } + + { // Both segments are the points which are different position + const auto p1 = createPoint(0.0, 1.0, 0.0); + const auto p2 = createPoint(0.0, 1.0, 0.0); + const auto p3 = createPoint(1.0, 0.0, 0.0); + const auto p4 = createPoint(1.0, 0.0, 0.0); + const auto result = intersect(p1, p2, p3, p4); + + EXPECT_FALSE(result); + } + + { // Segments are the same + const auto p1 = createPoint(0.0, -1.0, 0.0); + const auto p2 = createPoint(0.0, 1.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + const auto result = intersect(p1, p2, p3, p4); + + EXPECT_FALSE(result); + } + + { // One's edge is on the other's segment + const auto p1 = createPoint(0.0, -1.0, 0.0); + const auto p2 = createPoint(0.0, 1.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(1.0, 0.0, 0.0); + const auto result = intersect(p1, p2, p3, p4); + + EXPECT_TRUE(result); + EXPECT_NEAR(result->x, 0.0, epsilon); + EXPECT_NEAR(result->y, 0.0, epsilon); + EXPECT_NEAR(result->z, 0.0, epsilon); + } + + { // One's edge is the same as the other's edge. + const auto p1 = createPoint(0.0, -1.0, 0.0); + const auto p2 = createPoint(0.0, 1.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(2.0, -1.0, 0.0); + const auto result = intersect(p1, p2, p3, p4); + + EXPECT_TRUE(result); + EXPECT_NEAR(result->x, 0.0, epsilon); + EXPECT_NEAR(result->y, -1.0, epsilon); + EXPECT_NEAR(result->z, 0.0, epsilon); + } +} 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 84% 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 a8179f3b3cd60..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,19 +12,19 @@ // 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; const double z_ans = 3.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -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; @@ -46,7 +46,7 @@ TEST(geometry, getPose_PathWithLaneId) const double q_z_ans = 0.3; const double q_w_ans = 0.4; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -66,18 +66,18 @@ TEST(geometry, getPose_PathWithLaneId) TEST(geometry, getLongitudinalVelocity_PathWithLaneId) { - using tier4_autoware_utils::getLongitudinalVelocity; + using autoware::universe_utils::getLongitudinalVelocity; const double velocity = 1.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } 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; @@ -96,7 +96,7 @@ TEST(geometry, setPose_PathWithLaneId) p.orientation.z = q_z_ans; p.orientation.w = q_w_ans; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p_out{}; + tier4_planning_msgs::msg::PathPointWithLaneId p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.point.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.point.pose.position.y, y_ans); @@ -109,11 +109,11 @@ TEST(geometry, setPose_PathWithLaneId) TEST(geometry, setLongitudinalVelocity_PathWithLaneId) { - using tier4_autoware_utils::setLongitudinalVelocity; + using autoware::universe_utils::setLongitudinalVelocity; const double velocity = 1.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p{}; + tier4_planning_msgs::msg::PathPointWithLaneId p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.point.longitudinal_velocity_mps, velocity); } 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/autoware_universe_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp new file mode 100644 index 0000000000000..b55b27a34a6ac --- /dev/null +++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp @@ -0,0 +1,52 @@ +// 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 "autoware/universe_utils/math/constants.hpp" +#include "autoware/universe_utils/math/trigonometry.hpp" + +#include + +#include + +TEST(trigonometry, sin) +{ + 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)) - + autoware::universe_utils::sin(x * static_cast(i))) < 10e-5); + } +} + +TEST(trigonometry, cos) +{ + 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)) - + autoware::universe_utils::cos(x * static_cast(i))) < 10e-5); + } +} + +TEST(trigonometry, sin_and_cos) +{ + float x = 4.f * autoware::universe_utils::pi / 128.f; + for (int i = 0; i < 128; 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/autoware_universe_utils/test/src/transform/test_transform.cpp b/common/autoware_universe_utils/test/src/transform/test_transform.cpp new file mode 100644 index 0000000000000..2935600c9f446 --- /dev/null +++ b/common/autoware_universe_utils/test/src/transform/test_transform.cpp @@ -0,0 +1,60 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/universe_utils/transform/transforms.hpp" + +#include +#include +#include + +#include + +TEST(system, transform_point_cloud) +{ + pcl::PointCloud cloud; + cloud.push_back(pcl::PointXYZI(10.055880, -42.758892, -10.636949, 4)); + cloud.push_back(pcl::PointXYZI(23.282284, -29.485722, -11.468469, 5)); + + Eigen::Matrix transform; + transform << 0.834513, -0.550923, -0.008474, 89571.148438, 0.550986, 0.834372, 0.015428, + 42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000; + + pcl::PointCloud cloud_transformed; + autoware::universe_utils::transformPointCloud(cloud, cloud_transformed, transform); + + pcl::PointXYZI pt1_gt(89603.187500, 42270.878906, -13.056946, 4); + + constexpr float float_error = 0.0001; + EXPECT_NEAR(cloud_transformed[0].x, pt1_gt.x, float_error); + EXPECT_NEAR(cloud_transformed[0].y, pt1_gt.y, float_error); + EXPECT_NEAR(cloud_transformed[0].z, pt1_gt.z, float_error); + EXPECT_EQ(cloud_transformed[0].intensity, pt1_gt.intensity); +} + +TEST(system, empty_point_cloud) +{ + pcl::PointCloud cloud; + + Eigen::Matrix transform; + transform << 0.834513, -0.550923, -0.008474, 89571.148438, 0.550986, 0.834372, 0.015428, + 42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000; + + pcl::PointCloud cloud_transformed; + + EXPECT_NO_THROW( + autoware::universe_utils::transformPointCloud(cloud, cloud_transformed, transform)); + EXPECT_NO_FATAL_FAILURE( + autoware::universe_utils::transformPointCloud(cloud, cloud_transformed, transform)); + EXPECT_EQ(cloud_transformed.size(), 0ul); +} diff --git a/common/component_interface_specs/include/component_interface_specs/localization.hpp b/common/component_interface_specs/include/component_interface_specs/localization.hpp index 70c590c837927..e00d8cef1840d 100644 --- a/common/component_interface_specs/include/component_interface_specs/localization.hpp +++ b/common/component_interface_specs/include/component_interface_specs/localization.hpp @@ -18,16 +18,16 @@ #include #include -#include #include #include +#include namespace localization_interface { struct Initialize { - using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization; + using Service = tier4_localization_msgs::srv::InitializeLocalization; static constexpr char name[] = "/localization/initialize"; }; diff --git a/common/component_interface_specs/include/component_interface_specs/perception.hpp b/common/component_interface_specs/include/component_interface_specs/perception.hpp index 8665b9c35157d..0c72dbdb12078 100644 --- a/common/component_interface_specs/include/component_interface_specs/perception.hpp +++ b/common/component_interface_specs/include/component_interface_specs/perception.hpp @@ -17,14 +17,14 @@ #include -#include +#include namespace perception_interface { struct ObjectRecognition { - using Message = autoware_auto_perception_msgs::msg::PredictedObjects; + using Message = autoware_perception_msgs::msg::PredictedObjects; static constexpr char name[] = "/perception/object_recognition/objects"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index 9efd8c871f68f..58aba53d8ac8a 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -17,8 +17,8 @@ #include -#include #include +#include #include #include #include @@ -66,7 +66,7 @@ struct LaneletRoute struct Trajectory { - using Message = autoware_auto_planning_msgs::msg::Trajectory; + using Message = autoware_planning_msgs::msg::Trajectory; static constexpr char name[] = "/planning/scenario_planning/trajectory"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/include/component_interface_specs/vehicle.hpp b/common/component_interface_specs/include/component_interface_specs/vehicle.hpp index 6358b35c3c674..e7ce2c5212a25 100644 --- a/common/component_interface_specs/include/component_interface_specs/vehicle.hpp +++ b/common/component_interface_specs/include/component_interface_specs/vehicle.hpp @@ -20,10 +20,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include namespace vehicle_interface @@ -31,7 +31,7 @@ namespace vehicle_interface struct SteeringStatus { - using Message = autoware_auto_vehicle_msgs::msg::SteeringReport; + using Message = autoware_vehicle_msgs::msg::SteeringReport; static constexpr char name[] = "/vehicle/status/steering_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -40,7 +40,7 @@ struct SteeringStatus struct GearStatus { - using Message = autoware_auto_vehicle_msgs::msg::GearReport; + using Message = autoware_vehicle_msgs::msg::GearReport; static constexpr char name[] = "/vehicle/status/gear_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -49,7 +49,7 @@ struct GearStatus struct TurnIndicatorStatus { - using Message = autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; + using Message = autoware_vehicle_msgs::msg::TurnIndicatorsReport; static constexpr char name[] = "/vehicle/status/turn_indicators_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -58,7 +58,7 @@ struct TurnIndicatorStatus struct HazardLightStatus { - using Message = autoware_auto_vehicle_msgs::msg::HazardLightsReport; + using Message = autoware_vehicle_msgs::msg::HazardLightsReport; static constexpr char name[] = "/vehicle/status/hazard_lights_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 1ad5f410a814a..fa57bb1641eed 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -12,15 +12,15 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_msgs nav_msgs rcl rclcpp rosidl_runtime_cpp tier4_control_msgs + tier4_localization_msgs tier4_map_msgs tier4_planning_msgs tier4_system_msgs diff --git a/common/component_interface_tools/CMakeLists.txt b/common/component_interface_tools/CMakeLists.txt index a5ebc29463bec..2b896a951b8ed 100644 --- a/common/component_interface_tools/CMakeLists.txt +++ b/common/component_interface_tools/CMakeLists.txt @@ -3,5 +3,14 @@ project(component_interface_tools) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(service_log_checker src/service_log_checker.cpp) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/service_log_checker.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "ServiceLogChecker" + EXECUTABLE service_log_checker_node +) + ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/common/component_interface_tools/launch/service_log_checker.launch.xml b/common/component_interface_tools/launch/service_log_checker.launch.xml index f3099b3238096..c7845198955c1 100644 --- a/common/component_interface_tools/launch/service_log_checker.launch.xml +++ b/common/component_interface_tools/launch/service_log_checker.launch.xml @@ -1,3 +1,3 @@ - + diff --git a/common/component_interface_tools/package.xml b/common/component_interface_tools/package.xml index cff1829473e86..6df07af8729ff 100644 --- a/common/component_interface_tools/package.xml +++ b/common/component_interface_tools/package.xml @@ -13,6 +13,7 @@ diagnostic_updater fmt rclcpp + rclcpp_components tier4_system_msgs yaml_cpp_vendor diff --git a/common/component_interface_tools/src/service_log_checker.cpp b/common/component_interface_tools/src/service_log_checker.cpp index ce89573356412..18f90af5737d2 100644 --- a/common/component_interface_tools/src/service_log_checker.cpp +++ b/common/component_interface_tools/src/service_log_checker.cpp @@ -22,7 +22,8 @@ #define FMT_HEADER_ONLY #include -ServiceLogChecker::ServiceLogChecker() : Node("service_log_checker"), diagnostics_(this) +ServiceLogChecker::ServiceLogChecker(const rclcpp::NodeOptions & options) +: Node("service_log_checker", options), diagnostics_(this) { sub_ = create_subscription( "/service_log", 50, std::bind(&ServiceLogChecker::on_service_log, this, std::placeholders::_1)); @@ -98,13 +99,5 @@ void ServiceLogChecker::update_diagnostics(diagnostic_updater::DiagnosticStatusW } } -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ServiceLogChecker) diff --git a/common/component_interface_tools/src/service_log_checker.hpp b/common/component_interface_tools/src/service_log_checker.hpp index 32c7f02e757c6..9579753dfd900 100644 --- a/common/component_interface_tools/src/service_log_checker.hpp +++ b/common/component_interface_tools/src/service_log_checker.hpp @@ -26,7 +26,7 @@ class ServiceLogChecker : public rclcpp::Node { public: - ServiceLogChecker(); + explicit ServiceLogChecker(const rclcpp::NodeOptions & options); private: using ServiceLog = tier4_system_msgs::msg::ServiceLog; 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/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index e065f332b75e4..163bd761407c5 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -17,7 +17,6 @@ /// \copyright Copyright 2021 Apex.AI, Inc. /// All rights reserved. -#include #include #include @@ -29,7 +28,7 @@ #include #include -using autoware::common::types::bool8_t; +using bool8_t = bool; using FakeNodeFixture = autoware::tools::testing::FakeTestNode; using FakeNodeFixtureParametrized = autoware::tools::testing::FakeTestNodeParametrized; diff --git a/common/global_parameter_loader/launch/global_params.launch.py b/common/global_parameter_loader/launch/global_params.launch.py index 06f807aaa4609..8f9092b10fa37 100644 --- a/common/global_parameter_loader/launch/global_params.launch.py +++ b/common/global_parameter_loader/launch/global_params.launch.py @@ -33,7 +33,7 @@ def launch_setup(context, *args, **kwargs): load_vehicle_info = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("vehicle_info_util"), "/launch/vehicle_info.launch.py"] + [FindPackageShare("autoware_vehicle_info_utils"), "/launch/vehicle_info.launch.py"] ), launch_arguments={ "vehicle_info_param_file": [vehicle_description_pkg, "/config/vehicle_info.param.yaml"] diff --git a/common/global_parameter_loader/package.xml b/common/global_parameter_loader/package.xml index 4c2568b9aec98..472ef0c063430 100644 --- a/common/global_parameter_loader/package.xml +++ b/common/global_parameter_loader/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - vehicle_info_util + autoware_vehicle_info_utils ament_lint_auto autoware_lint_common diff --git a/common/goal_distance_calculator/Readme.md b/common/goal_distance_calculator/Readme.md index 3a25e147cf8e6..062b23baabe47 100644 --- a/common/goal_distance_calculator/Readme.md +++ b/common/goal_distance_calculator/Readme.md @@ -10,10 +10,10 @@ This node publishes deviation of self-pose from goal pose. ### Input -| Name | Type | Description | -| ---------------------------------- | ----------------------------------------- | --------------------- | -| `/planning/mission_planning/route` | `autoware_auto_planning_msgs::msg::Route` | Used to get goal pose | -| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | +| Name | Type | Description | +| ---------------------------------- | ------------------------------------ | --------------------- | +| `/planning/mission_planning/route` | `autoware_planning_msgs::msg::Route` | Used to get goal pose | +| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | ### Output 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 b4a98d90c74e4..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,17 +15,17 @@ #ifndef GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_ #define GOAL_DISTANCE_CALCULATOR__GOAL_DISTANCE_CALCULATOR_HPP_ +#include #include -#include -#include +#include #include #include namespace goal_distance_calculator { -using tier4_autoware_utils::PoseDeviation; +using autoware::universe_utils::PoseDeviation; struct Param { @@ -34,7 +34,7 @@ struct Param struct Input { geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose; - autoware_auto_planning_msgs::msg::Route::ConstSharedPtr route; + autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route; }; struct Output 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 50051d928b6c1..037841a505017 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,11 +17,11 @@ #include "goal_distance_calculator/goal_distance_calculator.hpp" +#include +#include #include -#include -#include -#include +#include #include #include @@ -45,18 +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_; + autoware::universe_utils::SelfPoseListener self_pose_listener_; + rclcpp::Subscription::SharedPtr sub_route_; // Data Buffer geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - autoware_auto_planning_msgs::msg::Route::SharedPtr route_; + autoware_planning_msgs::msg::LaneletRoute::SharedPtr route_; // Callback - void onRoute(const autoware_auto_planning_msgs::msg::Route::ConstSharedPtr & msg); + void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & msg); // Publisher - tier4_autoware_utils::DebugPublisher debug_publisher_; + autoware::universe_utils::DebugPublisher debug_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/common/goal_distance_calculator/package.xml b/common/goal_distance_calculator/package.xml index 49cb674f0a256..04ea8a37a5053 100644 --- a/common/goal_distance_calculator/package.xml +++ b/common/goal_distance_calculator/package.xml @@ -10,11 +10,11 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs + 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 1405d5bd62f7d..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_point.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 c6062d2a156ee..8e4e4c90bc470 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 @@ -48,9 +48,9 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions goal_distance_calculator_->setParam(param_); // Subscriber - sub_route_ = create_subscription( + sub_route_ = create_subscription( "/planning/mission_planning/route", queue_size, - [&](const autoware_auto_planning_msgs::msg::Route::SharedPtr msg_ptr) { route_ = msg_ptr; }); + [&](const autoware_planning_msgs::msg::LaneletRoute::SharedPtr msg_ptr) { route_ = msg_ptr; }); // Wait for first self pose self_pose_listener_.waitForFirstPose(); @@ -107,7 +107,7 @@ void GoalDistanceCalculatorNode::onTimer() output_ = goal_distance_calculator_->update(input_); { - using tier4_autoware_utils::rad2deg; + using autoware::universe_utils::rad2deg; const auto & deviation = output_.goal_deviation; debug_publisher_.publish( diff --git a/common/grid_map_utils/CMakeLists.txt b/common/grid_map_utils/CMakeLists.txt deleted file mode 100644 index 0fbb34e94e688..0000000000000 --- a/common/grid_map_utils/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(grid_map_utils) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(autoware_cmake REQUIRED) -autoware_package() -find_package(ament_cmake REQUIRED) - -ament_auto_add_library(${PROJECT_NAME} SHARED - DIRECTORY src -) - -target_link_libraries(${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - - ament_add_gtest(test_${PROJECT_NAME} - test/test_polygon_iterator.cpp - ) - target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME} - ) - - find_package(OpenCV REQUIRED) - add_executable(benchmark test/benchmark.cpp) - target_link_libraries(benchmark - ${PROJECT_NAME} - ${OpenCV_LIBS} - ) -endif() - -ament_auto_package() diff --git a/common/grid_map_utils/README.md b/common/grid_map_utils/README.md deleted file mode 100644 index f3c6ae5d879be..0000000000000 --- a/common/grid_map_utils/README.md +++ /dev/null @@ -1,51 +0,0 @@ -# Grid Map Utils - -## Overview - -This packages contains a re-implementation of the `grid_map::PolygonIterator` used to iterate over -all cells of a grid map contained inside some polygon. - -## Algorithm - -This implementation uses the [scan line algorithm](https://en.wikipedia.org/wiki/Scanline_rendering), -a common algorithm used to draw polygons on a rasterized image. -The main idea of the algorithm adapted to a grid map is as follow: - -- calculate intersections between rows of the grid map and the edges of the polygon edges; -- calculate for each row the column between each pair of intersections; -- the resulting `(row, column)` indexes are inside of the polygon. - -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). - -## 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. -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`. - -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 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. - -![Runtime comparison](media/runtime_comparison.png) - -## Future improvements - -There exists variations of the scan line algorithm for multiple polygons. -These can be implemented if we want to iterate over the cells contained in at least one of multiple polygons. - -The current implementation imitate the behavior of the original `grid_map::PolygonIterator` where a cell is selected if its center position is inside the polygon. -This behavior could be changed for example to only return all cells overlapped by the polygon. - -## References - -- -- diff --git a/common/grid_map_utils/package.xml b/common/grid_map_utils/package.xml deleted file mode 100644 index 6e63271d56750..0000000000000 --- a/common/grid_map_utils/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - grid_map_utils - 0.0.0 - Utilities for the grid_map library - Maxime CLEMENT - Apache License 2.0 - - autoware_cmake - eigen3_cmake_module - - grid_map_core - grid_map_cv - libopencv-dev - tier4_autoware_utils - - ament_cmake_gtest - ament_lint_auto - autoware_lint_common - - - ament_cmake - - 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 a7d7012c19e22..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_auto_planning_msgs::msg::TrajectoryPoint; - using tier4_autoware_utils::createPoint; + using autoware::universe_utils::createPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; std::vector points; points.push_back(createPoint(-2.0, -10.0, 0.0)); diff --git a/common/mission_planner_rviz_plugin/CMakeLists.txt b/common/mission_planner_rviz_plugin/CMakeLists.txt deleted file mode 100644 index 2b06e6db3e70d..0000000000000 --- a/common/mission_planner_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(mission_planner_rviz_plugin) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) -set(CMAKE_AUTOMOC ON) -set(CMAKE_INCLUDE_CURRENT_DIR ON) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/mrm_goal.cpp - src/route_selector_panel.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${QT_LIBRARIES} -) - -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package() diff --git a/common/mission_planner_rviz_plugin/README.md b/common/mission_planner_rviz_plugin/README.md deleted file mode 100644 index 59d36a0a2f840..0000000000000 --- a/common/mission_planner_rviz_plugin/README.md +++ /dev/null @@ -1,18 +0,0 @@ -# mission_planner_rviz_plugin - -## MrmGoalTool - -This is a copy of `rviz_default_plugins::tools::GoalTool`. Used together with the RouteSelectorPanel to set the MRM route. -After adding the tool, change the topic name to `/rviz/route_selector/mrm/goal` from the topic property panel in rviz. - -## RouteSelectorPanel - -This panel shows the main and mrm route state in the route_selector and the route states in the mission_planner. -Additionally, it provides clear and set functions for each main route and mrm route. - -| Trigger | Action | -| -------------------------------------- | ------------------------------------------------------------------------ | -| main route clear button | call `/planning/mission_planning/route_selector/main/clear_route` | -| mrm route clear button | call `/planning/mission_planning/route_selector/mrm/clear_route` | -| `/rviz/route_selector/main/goal` topic | call `/planning/mission_planning/route_selector/main/set_waypoint_route` | -| `/rviz/route_selector/mrm/goal` topic | call `/planning/mission_planning/route_selector/mrm/set_waypoint_route` | diff --git a/common/mission_planner_rviz_plugin/package.xml b/common/mission_planner_rviz_plugin/package.xml deleted file mode 100644 index e45cf2739f260..0000000000000 --- a/common/mission_planner_rviz_plugin/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - mission_planner_rviz_plugin - 0.0.0 - The mission_planner_rviz_plugin package - Takagi, Isamu - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - geometry_msgs - libqt5-core - libqt5-gui - libqt5-widgets - rclcpp - rviz_common - rviz_default_plugins - tier4_planning_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/common/mission_planner_rviz_plugin/plugins/plugin_description.xml b/common/mission_planner_rviz_plugin/plugins/plugin_description.xml deleted file mode 100644 index c8285fcc604d7..0000000000000 --- a/common/mission_planner_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - MrmGoalTool - - - RouteSelectorPanel - - diff --git a/common/mission_planner_rviz_plugin/src/mrm_goal.cpp b/common/mission_planner_rviz_plugin/src/mrm_goal.cpp deleted file mode 100644 index ef5529abfb4a7..0000000000000 --- a/common/mission_planner_rviz_plugin/src/mrm_goal.cpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2024 The Autoware Contributors -// -// 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 "mrm_goal.hpp" - -namespace rviz_plugins -{ - -MrmGoalTool::MrmGoalTool() -{ - shortcut_key_ = 'm'; -} - -void MrmGoalTool::onInitialize() -{ - GoalTool::onInitialize(); - setName("MRM Goal Pose"); -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::MrmGoalTool, rviz_common::Tool) diff --git a/common/mission_planner_rviz_plugin/src/mrm_goal.hpp b/common/mission_planner_rviz_plugin/src/mrm_goal.hpp deleted file mode 100644 index e16b0abf0bab3..0000000000000 --- a/common/mission_planner_rviz_plugin/src/mrm_goal.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2024 The Autoware Contributors -// -// 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 MRM_GOAL_HPP_ -#define MRM_GOAL_HPP_ - -#include - -namespace rviz_plugins -{ - -class MrmGoalTool : public rviz_default_plugins::tools::GoalTool -{ - Q_OBJECT - -public: - MrmGoalTool(); - void onInitialize() override; -}; - -} // namespace rviz_plugins - -#endif // MRM_GOAL_HPP_ diff --git a/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp b/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp deleted file mode 100644 index b4b376b1e76ec..0000000000000 --- a/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp +++ /dev/null @@ -1,148 +0,0 @@ -// Copyright 2024 The Autoware Contributors -// -// 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 "route_selector_panel.hpp" - -#include -#include - -#include -#include - -namespace rviz_plugins -{ - -QString to_string_state(const RouteState & state) -{ - // clang-format off - switch (state.state) { - case RouteState::UNKNOWN: return "unknown"; - case RouteState::INITIALIZING: return "initializing"; - case RouteState::UNSET: return "unset"; - case RouteState::ROUTING: return "routing"; - case RouteState::SET: return "set"; - case RouteState::REROUTING: return "rerouting"; - case RouteState::ARRIVED: return "arrived"; - case RouteState::ABORTED: return "aborted"; - case RouteState::INTERRUPTED: return "interrupted"; - default: return "-----"; - } - // clang-format on -} - -RouteSelectorPanel::RouteSelectorPanel(QWidget * parent) : rviz_common::Panel(parent) -{ - main_state_ = new QLabel("unknown"); - main_clear_ = new QPushButton("clear"); - mrm_state_ = new QLabel("unknown"); - mrm_clear_ = new QPushButton("clear"); - planner_state_ = new QLabel("unknown"); - - connect(main_clear_, &QPushButton::clicked, this, &RouteSelectorPanel::onMainClear); - connect(mrm_clear_, &QPushButton::clicked, this, &RouteSelectorPanel::onMrmClear); - - const auto layout = new QGridLayout(); - setLayout(layout); - layout->addWidget(new QLabel("main"), 0, 0); - layout->addWidget(main_state_, 0, 1); - layout->addWidget(main_clear_, 0, 2); - layout->addWidget(new QLabel("mrm"), 1, 0); - layout->addWidget(mrm_state_, 1, 1); - layout->addWidget(mrm_clear_, 1, 2); - layout->addWidget(new QLabel("planner"), 2, 0); - layout->addWidget(planner_state_, 2, 1); -} - -void RouteSelectorPanel::onInitialize() -{ - auto lock = getDisplayContext()->getRosNodeAbstraction().lock(); - auto node = lock->get_raw_node(); - - const auto durable_qos = rclcpp::QoS(1).transient_local(); - - sub_main_goal_ = node->create_subscription( - "/rviz/route_selector/main/goal", rclcpp::QoS(1), - std::bind(&RouteSelectorPanel::onMainGoal, this, std::placeholders::_1)); - sub_mrm_goal_ = node->create_subscription( - "/rviz/route_selector/mrm/goal", rclcpp::QoS(1), - std::bind(&RouteSelectorPanel::onMrmGoal, this, std::placeholders::_1)); - sub_main_state_ = node->create_subscription( - "/planning/mission_planning/route_selector/main/state", durable_qos, - std::bind(&RouteSelectorPanel::onMainState, this, std::placeholders::_1)); - sub_mrm_state_ = node->create_subscription( - "/planning/mission_planning/route_selector/mrm/state", durable_qos, - std::bind(&RouteSelectorPanel::onMrmState, this, std::placeholders::_1)); - sub_planner_state_ = node->create_subscription( - "/planning/mission_planning/state", durable_qos, - std::bind(&RouteSelectorPanel::onPlannerState, this, std::placeholders::_1)); - - cli_main_clear_ = - node->create_client("/planning/mission_planning/route_selector/main/clear_route"); - cli_mrm_clear_ = - node->create_client("/planning/mission_planning/route_selector/mrm/clear_route"); - cli_main_set_waypoint_ = node->create_client( - "/planning/mission_planning/route_selector/main/set_waypoint_route"); - cli_mrm_set_waypoint_ = node->create_client( - "/planning/mission_planning/route_selector/mrm/set_waypoint_route"); -} - -void RouteSelectorPanel::onMainGoal(const PoseStamped::ConstSharedPtr msg) -{ - const auto req = std::make_shared(); - req->header = msg->header; - req->goal_pose = msg->pose; - req->allow_modification = true; - cli_main_set_waypoint_->async_send_request(req); -} - -void RouteSelectorPanel::onMrmGoal(const PoseStamped::ConstSharedPtr msg) -{ - const auto req = std::make_shared(); - req->header = msg->header; - req->goal_pose = msg->pose; - req->allow_modification = true; - cli_mrm_set_waypoint_->async_send_request(req); -} - -void RouteSelectorPanel::onMainState(RouteState::ConstSharedPtr msg) -{ - main_state_->setText(to_string_state(*msg)); -} - -void RouteSelectorPanel::onMrmState(RouteState::ConstSharedPtr msg) -{ - mrm_state_->setText(to_string_state(*msg)); -} - -void RouteSelectorPanel::onPlannerState(RouteState::ConstSharedPtr msg) -{ - planner_state_->setText(to_string_state(*msg)); -} - -void RouteSelectorPanel::onMainClear() -{ - const auto req = std::make_shared(); - cli_main_clear_->async_send_request(req); -} - -void RouteSelectorPanel::onMrmClear() -{ - const auto req = std::make_shared(); - cli_mrm_clear_->async_send_request(req); -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::RouteSelectorPanel, rviz_common::Panel) diff --git a/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp b/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp deleted file mode 100644 index 99101730661e0..0000000000000 --- a/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2024 The Autoware Contributors -// -// 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 ROUTE_SELECTOR_PANEL_HPP_ -#define ROUTE_SELECTOR_PANEL_HPP_ - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace rviz_plugins -{ - -using geometry_msgs::msg::PoseStamped; -using tier4_planning_msgs::msg::RouteState; -using tier4_planning_msgs::srv::ClearRoute; -using tier4_planning_msgs::srv::SetWaypointRoute; - -class RouteSelectorPanel : public rviz_common::Panel -{ - Q_OBJECT - -public: - explicit RouteSelectorPanel(QWidget * parent = nullptr); - void onInitialize() override; - -private: - QLabel * main_state_; - QLabel * mrm_state_; - QLabel * planner_state_; - QPushButton * main_clear_; - QPushButton * mrm_clear_; - - rclcpp::Subscription::SharedPtr sub_main_goal_; - rclcpp::Subscription::SharedPtr sub_mrm_goal_; - void onMainGoal(const PoseStamped::ConstSharedPtr msg); - void onMrmGoal(const PoseStamped::ConstSharedPtr msg); - - rclcpp::Subscription::SharedPtr sub_main_state_; - rclcpp::Subscription::SharedPtr sub_mrm_state_; - rclcpp::Subscription::SharedPtr sub_planner_state_; - void onMainState(RouteState::ConstSharedPtr msg); - void onMrmState(RouteState::ConstSharedPtr msg); - void onPlannerState(RouteState::ConstSharedPtr msg); - - rclcpp::Client::SharedPtr cli_main_clear_; - rclcpp::Client::SharedPtr cli_mrm_clear_; - rclcpp::Client::SharedPtr cli_main_set_waypoint_; - rclcpp::Client::SharedPtr cli_mrm_set_waypoint_; - -private Q_SLOTS: - void onMainClear(); - void onMrmClear(); -}; - -} // namespace rviz_plugins - -#endif // ROUTE_SELECTOR_PANEL_HPP_ diff --git a/common/motion_utils/CMakeLists.txt b/common/motion_utils/CMakeLists.txt deleted file mode 100644 index f42deaa7f8c41..0000000000000 --- a/common/motion_utils/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(motion_utils) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Boost REQUIRED) - -ament_auto_add_library(motion_utils SHARED - src/distance/distance.cpp - src/marker/marker_helper.cpp - src/marker/virtual_wall_marker_creator.cpp - src/resample/resample.cpp - src/trajectory/trajectory.cpp - src/trajectory/interpolation.cpp - src/trajectory/path_with_lane_id.cpp - src/trajectory/conversion.cpp - src/vehicle/vehicle_state_checker.cpp -) - -if(BUILD_TESTING) - find_package(ament_cmake_ros REQUIRED) - - file(GLOB_RECURSE test_files test/**/*.cpp) - - ament_add_ros_isolated_gtest(test_motion_utils ${test_files}) - - target_link_libraries(test_motion_utils - motion_utils - ) -endif() - -ament_auto_package() diff --git a/common/motion_utils/README.md b/common/motion_utils/README.md deleted file mode 100644 index 925ee5b162db3..0000000000000 --- a/common/motion_utils/README.md +++ /dev/null @@ -1,117 +0,0 @@ -# Motion Utils package - -## Definition of terms - -### Segment - -`Segment` in Autoware is the line segment between two successive points as follows. - -![segment](./media/segment.svg){: style="width:600px"} - -The nearest segment index and nearest point index to a certain position is not always th same. -Therefore, we prepare two different utility functions to calculate a nearest index for points and segments. - -## Nearest index search - -In this section, the nearest index and nearest segment index search is explained. - -We have the same functions for the nearest index search and nearest segment index search. -Taking for the example the nearest index search, we have two types of functions. - -The first function finds the nearest index with distance and yaw thresholds. - -```cpp -template -size_t findFirstNearestIndexWithSoftConstraints( - const T & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); -``` - -This function finds the first local solution within thresholds. -The reason to find the first local one is to deal with some edge cases explained in the next subsection. - -There are default parameters for thresholds arguments so that you can decide which thresholds to pass to the function. - -1. When both the distance and yaw thresholds are given. - - First, try to find the nearest index with both the distance and yaw thresholds. - - If not found, try to find again with only the distance threshold. - - If not found, find without any thresholds. -2. When only distance are given. - - First, try to find the nearest index the distance threshold. - - If not found, find without any thresholds. -3. When no thresholds are given. - - Find the nearest index. - -The second function finds the nearest index in the lane whose id is `lane_id`. - -```cpp -size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); -``` - -### Application to various object - -Many node packages often calculate the nearest index of objects. -We will explain the recommended method to calculate it. - -#### Nearest index for the ego - -Assuming that the path length before the ego is short enough, we expect to find the correct nearest index in the following edge cases by `findFirstNearestIndexWithSoftConstraints` with both distance and yaw thresholds. -Blue circles describes the distance threshold from the base link position and two blue lines describe the yaw threshold against the base link orientation. -Among points in these cases, the correct nearest point which is red can be found. - -![ego_nearest_search](./media/ego_nearest_search.svg) - -Therefore, the implementation is as follows. - -```cpp -const size_t ego_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); -const size_t ego_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); -``` - -#### Nearest index for dynamic objects - -For the ego nearest index, the orientation is considered in addition to the position since the ego is supposed to follow the points. -However, for the dynamic objects (e.g., predicted object), sometimes its orientation may be different from the points order, e.g. the dynamic object driving backward although the ego is driving forward. - -Therefore, the yaw threshold should not be considered for the dynamic object. -The implementation is as follows. - -```cpp -const size_t dynamic_obj_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold); -const size_t dynamic_obj_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold); -``` - -#### Nearest index for traffic objects - -In lanelet maps, traffic objects belong to the specific lane. -With this specific lane's id, the correct nearest index can be found. - -The implementation is as follows. - -```cpp -// first extract `lane_id` which the traffic object belong to. -const size_t traffic_obj_nearest_idx = findNearestIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id); -const size_t traffic_obj_nearest_seg_idx = findNearestSegmentIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id); -``` - -## Path/Trajectory length calculation between designated points - -Based on the discussion so far, the nearest index search algorithm is different depending on the object type. -Therefore, we recommended using the wrapper utility functions which require the nearest index search (e.g., calculating the path length) with each nearest index search. - -For example, when we want to calculate the path length between the ego and the dynamic object, the implementation is as follows. - -```cpp -const size_t ego_nearest_seg_idx = findFirstNearestSegmentIndex(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); -const size_t dyn_obj_nearest_seg_idx = findFirstNearestSegmentIndex(points, dyn_obj_pose, dyn_obj_nearest_dist_threshold); -const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_nearest_seg_idx, dyn_obj_pose, dyn_obj_nearest_seg_idx); -``` - -## For developers - -Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_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. diff --git a/common/motion_utils/include/motion_utils/constants.hpp b/common/motion_utils/include/motion_utils/constants.hpp deleted file mode 100644 index 63a3439a34b8a..0000000000000 --- a/common/motion_utils/include/motion_utils/constants.hpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MOTION_UTILS__CONSTANTS_HPP_ -#define MOTION_UTILS__CONSTANTS_HPP_ - -namespace motion_utils -{ -constexpr double overlap_threshold = 0.1; -} // namespace motion_utils - -#endif // MOTION_UTILS__CONSTANTS_HPP_ diff --git a/common/motion_utils/include/motion_utils/distance/distance.hpp b/common/motion_utils/include/motion_utils/distance/distance.hpp deleted file mode 100644 index 0a0d00fc9f260..0000000000000 --- a/common/motion_utils/include/motion_utils/distance/distance.hpp +++ /dev/null @@ -1,33 +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 MOTION_UTILS__DISTANCE__DISTANCE_HPP_ -#define MOTION_UTILS__DISTANCE__DISTANCE_HPP_ - -#include -#include -#include -#include -#include -#include - -namespace 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 - -#endif // MOTION_UTILS__DISTANCE__DISTANCE_HPP_ diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp deleted file mode 100644 index 0604eee65372a..0000000000000 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2021 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 MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ -#define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ - -#include - -#include - -#include - -namespace motion_utils -{ -using geometry_msgs::msg::Pose; - -visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( - const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", - const bool is_driving_forward = true); - -visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( - const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", - const bool is_driving_forward = true); - -visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( - const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", - const bool is_driving_forward = true); - -visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( - const rclcpp::Time & now, const int32_t id); - -visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker( - const rclcpp::Time & now, const int32_t id); - -visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( - const rclcpp::Time & now, const int32_t id); -} // namespace motion_utils - -#endif // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp deleted file mode 100644 index 258386ca236a3..0000000000000 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ /dev/null @@ -1,240 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ - -#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" - -#include - -namespace motion_utils -{ -/** - * @brief A resampling function for a path(points). Note that in a default setting, position xy are - * resampled by spline interpolation, position z are resampled by linear interpolation, and - * orientation of the resampled path are calculated by a forward difference method - * based on the interpolated position x and y. - * @param input_path input path(point) to resample - * @param resampled_arclength arclength that contains length of each resampling points from initial - * point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @return resampled path(poses) - */ -std::vector resamplePointVector( - const std::vector & points, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true); - -/** - * @brief A resampling function for a path(position). Note that in a default setting, position xy - * are resampled by spline interpolation, position z are resampled by linear interpolation, and - * orientation of the resampled path are calculated by a forward difference method - * based on the interpolated position x and y. - * @param input_path input path(position) to resample - * @param resample_interval resampling interval - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @return resampled path(poses) - */ -std::vector resamplePointVector( - const std::vector & points, const double resample_interval, - const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); - -/** - * @brief A resampling function for a path(poses). Note that in a default setting, position xy are - * resampled by spline interpolation, position z are resampled by linear interpolation, and - * orientation of the resampled path are calculated by a forward difference method - * based on the interpolated position x and y. - * @param input_path input path(poses) to resample - * @param resampled_arclength arclength that contains length of each resampling points from initial - * point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @return resampled path(poses) - */ -std::vector resamplePoseVector( - const std::vector & points, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true); - -/** - * @brief A resampling function for a path(poses). Note that in a default setting, position xy are - * resampled by spline interpolation, position z are resampled by linear interpolation, and - * orientation of the resampled path are calculated by a forward difference method - * based on the interpolated position x and y. - * @param input_path input path(poses) to resample - * @param resample_interval resampling interval - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @return resampled path(poses) - */ -std::vector resamplePoseVector( - const std::vector & points, const double resample_interval, - const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); - -/** - * @brief A resampling function for a path with lane id. Note that in a default setting, position xy - * are resampled by spline interpolation, position z are resampled by linear interpolation, - * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is - * resampled by linear interpolation. Orientation of the resampled path are calculated by a - * forward difference method based on the interpolated position x and y. Moreover, lane_ids - * and is_final are also interpolated by zero order hold - * @param input_path input path to resample - * @param resampled_arclength arclength that contains length of each resampling points from initial - * point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample - * longitudinal and lateral velocity. Otherwise, it uses linear interpolation - * @return resampled path - */ -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); - -/** - * @brief A resampling function for a path with lane id. Note that in a default setting, position xy - * are resampled by spline interpolation, position z are resampled by linear interpolation, - * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is - * resampled by linear interpolation. Orientation of the resampled path are calculated by a - * forward difference method based on the interpolated position x and y. Moreover, lane_ids - * and is_final are also interpolated by zero order hold - * @param input_path input path to resample - * @param resampled_interval resampling interval point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample - * longitudinal and lateral velocity. Otherwise, it uses linear interpolation - * @param resample_input_path_stop_point If true, resample closest stop point in input path - * @return resampled path - */ -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, - const bool resample_input_path_stop_point = true); - -/** - * @brief A resampling function for a path. Note that in a default setting, position xy are - * resampled by spline interpolation, position z are resampled by linear interpolation, - * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is - * resampled by linear interpolation. Orientation of the resampled path are calculated by a - * forward difference method based on the interpolated position x and y. - * @param input_path input path to resample - * @param resampled_arclength arclength that contains length of each resampling points from initial - * point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample - * longitudinal and lateral velocity. Otherwise, it uses linear interpolation - * @return resampled path - */ -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); - -/** - * @brief A resampling function for a path. Note that in a default setting, position xy - * are resampled by spline interpolation, position z are resampled by linear interpolation, - * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is - * resampled by linear interpolation. Orientation of the resampled path are calculated by a - * forward difference method based on the interpolated position x and y. - * @param input_path input path to resample - * @param resampled_interval resampling interval point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample - * longitudinal and lateral velocity. Otherwise, it uses linear interpolation - * @param resample_input_path_stop_point If true, resample closest stop point in input path - * @return resampled path - */ -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, - 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_path_stop_point = true); - -/** - * @brief A resampling function for a trajectory. Note that in a default setting, position xy are - * resampled by spline interpolation, position z are resampled by linear interpolation, twist - * informaiton(velocity and acceleration) are resampled by zero_order_hold, and heading rate - * is resampled by linear interpolation. The rest of the category is resampled by linear - * interpolation. Orientation of the resampled path are calculated by a forward difference - * method based on the interpolated position x and y. - * @param input_trajectory input trajectory to resample - * @param resampled_arclength arclength that contains length of each resampling points from initial - * point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample - * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation - * @return resampled trajectory - */ -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true); - -/** - * @brief A resampling function for a trajectory. This function resamples closest stop point, - * terminal point and points by resample interval. Note that in a default setting, position - * xy are resampled by spline interpolation, position z are resampled by linear interpolation, twist - * informaiton(velocity and acceleration) are resampled by zero_order_hold, and heading rate - * is resampled by linear interpolation. The rest of the category is resampled by linear - * interpolation. Orientation of the resampled path are calculated by a forward difference - * method based on the interpolated position x and y. - * @param input_trajectory input trajectory to resample - * @param resampled_interval resampling interval - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample - * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation - * @param resample_input_trajectory_stop_point If true, resample closest stop point in input - * trajectory - * @return resampled trajectory - */ -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, 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 - -#endif // MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp deleted file mode 100644 index 8bb5f13e3fb78..0000000000000 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ -#define MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ - -#include "tier4_autoware_utils/system/backtrace.hpp" - -#include -#include -#include - -#include - -namespace resample_utils -{ -constexpr double close_s_threshold = 1e-6; - -#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; - -template -bool validate_size(const T & points) -{ - return points.size() >= 2; -} - -template -bool validate_resampling_range(const T & points, const std::vector & resampling_intervals) -{ - const double points_length = motion_utils::calcArcLength(points); - return points_length >= resampling_intervals.back(); -} - -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); - if (ds < close_s_threshold) { - return false; - } - } - - return true; -} - -template -bool validate_arguments(const T & input_points, const std::vector & resampling_intervals) -{ - // Check size of the arguments - if (!validate_size(input_points)) { - log_error("[resample_utils] invalid argument: The number of input points is less than 2"); - tier4_autoware_utils::print_backtrace(); - return false; - } - if (!validate_size(resampling_intervals)) { - log_error( - "[resample_utils] invalid argument: The number of resampling intervals is less than 2"); - tier4_autoware_utils::print_backtrace(); - return false; - } - - // Check resampling range - if (!validate_resampling_range(input_points, resampling_intervals)) { - log_error("[resample_utils] invalid argument: resampling interval is longer than input points"); - tier4_autoware_utils::print_backtrace(); - return false; - } - - // Check duplication - if (!validate_points_duplication(input_points)) { - log_error("[resample_utils] invalid argument: input points has some duplicated points"); - tier4_autoware_utils::print_backtrace(); - return false; - } - - return true; -} - -template -bool validate_arguments(const T & input_points, const double resampling_interval) -{ - // Check size of the arguments - if (!validate_size(input_points)) { - log_error("[resample_utils] invalid argument: The number of input points is less than 2"); - tier4_autoware_utils::print_backtrace(); - return false; - } - - // check resampling interval - if (resampling_interval < motion_utils::overlap_threshold) { - log_error( - "[resample_utils] invalid argument: resampling interval is less than " + - std::to_string(motion_utils::overlap_threshold)); - tier4_autoware_utils::print_backtrace(); - return false; - } - - // Check duplication - if (!validate_points_duplication(input_points)) { - log_error("[resample_utils] invalid argument: input points has some duplicated points"); - tier4_autoware_utils::print_backtrace(); - return false; - } - - return true; -} -} // namespace resample_utils - -#endif // MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp deleted file mode 100644 index 7d4be216e89fe..0000000000000 --- a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp +++ /dev/null @@ -1,121 +0,0 @@ -// Copyright 2021 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 MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ -#define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ - -#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" -#include "std_msgs/msg/header.hpp" - -#include - -namespace motion_utils -{ -using TrajectoryPoints = std::vector; - -/** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. - * @attention This function just clips - * std::vector up to the capacity of Trajectory. - * Therefore, the error handling out of this function is necessary if the size of the input greater - * than the capacity. - * @todo Decide how to handle the situation that we need to use the trajectory with the size of - * points larger than the capacity. (Tier IV) - */ -autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory, - const std_msgs::msg::Header & header = std_msgs::msg::Header{}); - -/** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. - */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory); - -template -autoware_auto_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) -{ - static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); - throw std::logic_error("Only specializations of convertToPath can be used."); -} - -template <> -inline autoware_auto_planning_msgs::msg::Path convertToPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input) -{ - autoware_auto_planning_msgs::msg::Path output{}; - output.header = input.header; - output.left_bound = input.left_bound; - output.right_bound = input.right_bound; - output.points.resize(input.points.size()); - for (size_t i = 0; i < input.points.size(); ++i) { - output.points.at(i) = input.points.at(i).point; - } - return output; -} - -template -TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) -{ - static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used."); - throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used."); -} - -template <> -inline TrajectoryPoints convertToTrajectoryPoints( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input) -{ - TrajectoryPoints output{}; - for (const auto & p : input.points) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tp; - tp.pose = p.point.pose; - tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; - // since path point doesn't have acc for now - tp.acceleration_mps2 = 0; - output.emplace_back(tp); - } - return output; -} - -template -autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( - [[maybe_unused]] const T & input) -{ - static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); - throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); -} - -template <> -inline autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( - const TrajectoryPoints & input) -{ - autoware_auto_planning_msgs::msg::PathWithLaneId output{}; - for (const auto & p : input) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId pp; - pp.point.pose = p.pose; - pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; - output.points.emplace_back(pp); - } - return output; -} - -} // namespace motion_utils - -#endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp deleted file mode 100644 index aec329f9f540e..0000000000000 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ -#define MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ - -#include "tier4_autoware_utils/geometry/geometry.hpp" - -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" - -#include -#include - -namespace motion_utils -{ -/** - * @brief An interpolation function that finds the closest interpolated point on the trajectory from - * the given pose - * @param trajectory input trajectory - * @param target_pose target_pose - * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for - * twist information - * @return resampled path(poses) - */ -autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, - const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); - -/** - * @brief An interpolation function that finds the closest interpolated point on the path from - * the given pose - * @param path input path - * @param target_pose target_pose - * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for - * twist information - * @return resampled path(poses) - */ -autoware_auto_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); - -/** - * @brief An interpolation function that finds the closest interpolated point on the path that is a - * certain length away from the given pose - * @param points input path - * @param target_length length from the front point of the path - * @return resampled pose - */ -template -geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double target_length) -{ - if (points.empty()) { - geometry_msgs::msg::Pose interpolated_pose; - return interpolated_pose; - } - - if (points.size() < 2 || target_length < 0.0) { - return tier4_autoware_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); - 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); - } - accumulated_length += length; - } - - return tier4_autoware_utils::getPose(points.back()); -} - -} // namespace motion_utils - -#endif // MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp deleted file mode 100644 index d0ed761b76d9a..0000000000000 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ - -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include - -#include -#include -namespace motion_utils -{ -std::optional> getPathIndexRangeWithLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); - -size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); - -size_t findNearestSegmentIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); - -// @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle -// center follow the input path -// @param [in] path with position to be followed by the center of the vehicle -// @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle -// center follow the input path NOTE: rear_to_cog is supposed to be positive -autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, - const bool enable_last_point_compensation = true); -} // namespace motion_utils - -#endif // MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp deleted file mode 100644 index 7c9b5a5378ab6..0000000000000 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ /dev/null @@ -1,2500 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#define 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 - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -namespace motion_utils -{ -#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; - -/** - * @brief validate if points container is empty or not - * @param points points of trajectory, path, ... - */ -template -void validateNonEmpty(const T & points) -{ - if (points.empty()) { - tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("[motion_utils] validateNonEmpty(): Points is empty."); - } -} - -extern template void validateNonEmpty>( - const std::vector &); -extern template void -validateNonEmpty>( - const std::vector &); -extern template void -validateNonEmpty>( - const std::vector &); - -/** - * @brief validate a point is in a non-sharp angle between two points or not - * @param point1 front point - * @param point2 point to be validated - * @param point3 back point - */ -template -void validateNonSharpAngle( - const T & point1, const T & point2, const T & point3, - const double angle_threshold = tier4_autoware_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 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); - - 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."); - } -} - -/** - * @brief checks whether a path of trajectory has forward driving direction - * @param points points of trajectory, path, ... - * @return (forward / backward) driving (true / false) - */ -template -std::optional isDrivingForward(const T & points) -{ - if (points.size() < 2) { - return std::nullopt; - } - - // 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)); - - return tier4_autoware_utils::isDrivingForward(first_pose, second_pose); -} - -extern template std::optional -isDrivingForward>( - const std::vector &); -extern template std::optional -isDrivingForward>( - const std::vector &); -extern template std::optional -isDrivingForward>( - const std::vector &); - -/** - * @brief checks whether a path of trajectory has forward driving direction using its longitudinal - * velocity - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @return (forward / backward) driving (true, false, none "if velocity is zero") - */ -template -std::optional isDrivingForwardWithTwist(const T & points_with_twist) -{ - if (points_with_twist.empty()) { - return std::nullopt; - } - if (points_with_twist.size() == 1) { - if (0.0 < tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { - return true; - } - if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { - return false; - } - return std::nullopt; - } - - return isDrivingForward(points_with_twist); -} - -extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); -extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); -extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); - -/** - * @brief remove overlapping points through points container. - * Overlapping is determined by calculating the distance between 2 consecutive points. - * If the distance between them is less than a threshold, they will be considered overlapping. - * @param points points of trajectory, path, ... - * @param start_idx index to start the overlap remove calculation from through the points - * container. Indices before that index will be considered non-overlapping. Default = 0 - * @return points container without overlapping points - */ -template -T removeOverlapPoints(const T & points, const size_t start_idx = 0) -{ - if (points.size() < start_idx + 1) { - return points; - } - - T dst; - dst.reserve(points.size()); - - for (size_t i = 0; i <= start_idx; ++i) { - dst.push_back(points.at(i)); - } - - 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)); - if (std::abs(prev_p.x - curr_p.x) < eps && std::abs(prev_p.y - curr_p.y) < eps) { - continue; - } - dst.push_back(points.at(i)); - } - - return dst; -} - -extern template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx = 0); - -/** - * @brief search through points container from specified start and end indices about first matching - * index of a zero longitudinal velocity point. - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @param src_idx start index of the search - * @param dst_idx end index of the search - * @return first matching index of a zero velocity point inside the points container. - */ -template -std::optional searchZeroVelocityIndex( - const T & points_with_twist, const size_t src_idx, const size_t dst_idx) -{ - try { - validateNonEmpty(points_with_twist); - } catch (const std::exception & e) { - log_error(e.what()); - return {}; - } - - constexpr double epsilon = 1e-3; - for (size_t i = src_idx; i < dst_idx; ++i) { - if (std::fabs(points_with_twist.at(i).longitudinal_velocity_mps) < epsilon) { - return i; - } - } - - return {}; -} - -extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, - const size_t src_idx, const size_t dst_idx); - -/** - * @brief search through points container from specified start index till end of points container - * about first matching index of a zero longitudinal velocity point. - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @param src_idx start index of the search - * @return first matching index of a zero velocity point inside the points container. - */ -template -std::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t src_idx) -{ - try { - validateNonEmpty(points_with_twist); - } catch (const std::exception & e) { - log_error(e.what()); - return {}; - } - - return searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); -} - -extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, - const size_t src_idx); - -/** - * @brief search through points container from its start to end about first matching index of a zero - * longitudinal velocity point. - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @return first matching index of a zero velocity point inside the points container. - */ -template -std::optional searchZeroVelocityIndex(const T & points_with_twist) -{ - return searchZeroVelocityIndex(points_with_twist, 0, points_with_twist.size()); -} - -extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist); - -/** - * @brief find nearest point index through points container for a given point. - * Finding nearest point is determined by looping through the points container, - * and calculating the 2D squared distance between each point in the container and the given point. - * The index of the point with minimum distance and yaw deviation comparing to the given point will - * be returned. - * @param points points of trajectory, path, ... - * @param point given point - * @return index of nearest point - */ -template -size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & point) -{ - validateNonEmpty(points); - - double min_dist = std::numeric_limits::max(); - 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); - if (dist < min_dist) { - min_dist = dist; - min_idx = i; - } - } - return min_idx; -} - -extern template size_t findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -extern template size_t -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -extern template size_t -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); - -/** - * @brief find nearest point index through points container for a given pose. - * Finding nearest point is determined by looping through the points container, - * and finding the nearest point to the given pose in terms of squared 2D distance and yaw - * deviation. The index of the point with minimum distance and yaw deviation comparing to the given - * pose will be returned. - * @param points points of trajectory, path, ... - * @param pose given pose - * @param max_dist max distance used to get squared distance for finding the nearest point to given - * pose - * @param max_yaw max yaw used for finding nearest point to given pose - * @return index of nearest point (index or none if not found) - */ -template -std::optional findNearestIndex( - const T & points, const geometry_msgs::msg::Pose & pose, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error(e.what()); - return {}; - } - - const double max_squared_dist = max_dist * max_dist; - - double min_squared_dist = std::numeric_limits::max(); - bool is_nearest_found = false; - 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); - 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); - if (std::fabs(yaw) > max_yaw) { - continue; - } - - min_squared_dist = squared_dist; - min_idx = i; - is_nearest_found = true; - } - - if (is_nearest_found) { - return min_idx; - } - return std::nullopt; -} - -extern template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); - -/** - * @brief calculate longitudinal offset (length along trajectory from seg_idx point to nearest point - * to p_target on trajectory). If seg_idx point is after that nearest point, length is negative. - * Segment is straight path between two continuous points of trajectory. - * @param points points of trajectory, path, ... - * @param seg_idx segment index of point at beginning of length - * @param p_target target point at end of length - * @param throw_exception flag to enable/disable exception throwing - * @return signed length - */ -template -double calcLongitudinalOffsetToSegment( - const T & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - const bool throw_exception = false) -{ - if (seg_idx >= points.size() - 1) { - const std::string error_message( - "[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(); - if (throw_exception) { - throw std::out_of_range(error_message); - } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); - return std::nan(""); - } - - const auto overlap_removed_points = removeOverlapPoints(points, seg_idx); - - if (throw_exception) { - validateNonEmpty(overlap_removed_points); - } else { - try { - validateNonEmpty(overlap_removed_points); - } catch (const std::exception & e) { - log_error(e.what()); - return std::nan(""); - } - } - - if (seg_idx >= overlap_removed_points.size() - 1) { - const std::string error_message( - "[motion_utils] " + std::string(__func__) + - ": Longitudinal offset calculation is not supported for the same points."); - tier4_autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::runtime_error(error_message); - } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); - 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 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}; - - return segment_vec.dot(target_vec) / segment_vec.norm(); -} - -extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, - const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); -extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - const bool throw_exception = false); -extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - const bool throw_exception = false); - -/** - * @brief find nearest segment index to point. - * Segment is straight path between two continuous points of trajectory. - * When point is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 - * @param points points of trajectory, path, ... - * @param point point to which to find nearest segment index - * @return nearest index - */ -template -size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point & point) -{ - const size_t nearest_idx = findNearestIndex(points, point); - - if (nearest_idx == 0) { - return 0; - } - if (nearest_idx == points.size() - 1) { - return points.size() - 2; - } - - const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, point); - - if (signed_length <= 0) { - return nearest_idx - 1; - } - - return nearest_idx; -} - -extern template size_t -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -extern template size_t -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -extern template size_t -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); - -/** - * @brief find nearest segment index to pose - * Segment is straight path between two continuous points of trajectory. - * When pose is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 - * @param points points of trajectory, path, .. - * @param pose pose to which to find nearest segment index - * @param max_dist max distance used for finding the nearest index to given pose - * @param max_yaw max yaw used for finding nearest index to given pose - * @return nearest index - */ -template -std::optional findNearestSegmentIndex( - const T & points, const geometry_msgs::msg::Pose & pose, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()) -{ - const auto nearest_idx = findNearestIndex(points, pose, max_dist, max_yaw); - - if (!nearest_idx) { - return std::nullopt; - } - - if (*nearest_idx == 0) { - return 0; - } - if (*nearest_idx == points.size() - 1) { - return points.size() - 2; - } - - const double signed_length = calcLongitudinalOffsetToSegment(points, *nearest_idx, pose.position); - - if (signed_length <= 0) { - return *nearest_idx - 1; - } - - return *nearest_idx; -} - -extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); - -/** - * @brief calculate lateral offset from p_target (length from p_target to trajectory) using given - * segment index. Segment is straight path between two continuous points of trajectory. - * @param points points of trajectory, path, ... - * @param p_target target point - * @param seg_idx segment index of point at beginning of length - * @param throw_exception flag to enable/disable exception throwing - * @return length (unsigned) - */ -template -double calcLateralOffset( - const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, - const bool throw_exception = false) -{ - const auto overlap_removed_points = removeOverlapPoints(points, 0); - - if (throw_exception) { - validateNonEmpty(overlap_removed_points); - } else { - try { - validateNonEmpty(overlap_removed_points); - } catch (const std::exception & e) { - log_error( - std::string(e.what()) + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); - return std::nan(""); - } - } - - if (overlap_removed_points.size() == 1) { - const std::string error_message( - "[motion_utils] " + std::string(__func__) + - ": Lateral offset calculation is not supported for the same points."); - tier4_autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::runtime_error(error_message); - } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); - return std::nan(""); - } - - const auto p_indices = overlap_removed_points.size() - 2; - 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 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}; - - const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec); - return cross_vec(2) / segment_vec.norm(); -} - -extern template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, - const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, - const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, - const bool throw_exception = false); - -/** - * @brief calculate lateral offset from p_target (length from p_target to trajectory). - * The function gets the nearest segment index between the points of trajectory and the given target - * point, then uses that segment index to calculate lateral offset. Segment is straight path between - * two continuous points of trajectory. - * @param points points of trajectory, path, ... - * @param p_target target point - * @param throw_exception flag to enable/disable exception throwing - * @return length (unsigned) - */ -template -double calcLateralOffset( - const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false) -{ - const auto overlap_removed_points = removeOverlapPoints(points, 0); - - if (throw_exception) { - validateNonEmpty(overlap_removed_points); - } else { - try { - validateNonEmpty(overlap_removed_points); - } catch (const std::exception & e) { - log_error( - std::string(e.what()) + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); - return std::nan(""); - } - } - - if (overlap_removed_points.size() == 1) { - const std::string error_message( - "[motion_utils] " + std::string(__func__) + - ": Lateral offset calculation is not supported for the same points."); - tier4_autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::runtime_error(error_message); - } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); - return std::nan(""); - } - - const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target); - return calcLateralOffset(points, p_target, seg_idx, throw_exception); -} - -extern template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); - -/** - * @brief calculate length of 2D distance between two points, specified by start and end points - * indicies through points container. - * @param points points of trajectory, path, ... - * @param src_idx index of start point - * @param dst_idx index of end point - * @return length of distance between two points. - * Length is positive if dst_idx is greater that src_idx (i.e. after it in trajectory, path, ...) - * and negative otherwise. - */ -template -double calcSignedArcLength(const T & points, const size_t src_idx, const size_t dst_idx) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error(e.what()); - return 0.0; - } - - if (src_idx > dst_idx) { - return -calcSignedArcLength(points, dst_idx, src_idx); - } - - 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)); - } - return dist_sum; -} - -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); - -/** - * @brief Computes the partial sums of the elements in the sub-ranges of the range [src_idx, - * dst_idx) and return these sum as vector - * @param points points of trajectory, path, ... - * @param src_idx index of start point - * @param dst_idx index of end point - * @return partial sums container - */ -template -std::vector calcSignedArcLengthPartialSum( - const T & points, const size_t src_idx, const size_t dst_idx) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error(e.what()); - return {}; - } - - if (src_idx + 1 > dst_idx) { - auto copied = points; - std::reverse(copied.begin(), copied.end()); - return calcSignedArcLengthPartialSum(points, dst_idx, src_idx); - } - - std::vector partial_dist; - partial_dist.reserve(dst_idx - src_idx); - - 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)); - partial_dist.push_back(dist_sum); - } - return partial_dist; -} - -extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); -extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); -extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); - -/** - * @brief calculate length of 2D distance between two points, specified by start point and end point - * index of points container. - * @param points points of trajectory, path, ... - * @param src_point start point - * @param dst_idx index of end point - * @return length of distance between two points. - * Length is positive if destination point associated to dst_idx is greater that src_idx (i.e. after - * it in trajectory, path, ...) and negative otherwise. - */ -template -double calcSignedArcLength( - const T & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error(e.what()); - return 0.0; - } - - const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); - - const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - - return signed_length_on_traj - signed_length_src_offset; -} - -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector &, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector &, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); - -/** - * @brief calculate length of 2D distance between two points, specified by start index of points - * container and end point. - * @param points points of trajectory, path, ... - * @param src_idx index of start point - * @param dst_point end point - * @return length of distance between two points - * Length is positive if destination point is greater that source point associated to src_idx (i.e. - * after it in trajectory, path, ...) and negative otherwise. - */ -template -double calcSignedArcLength( - const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error(e.what()); - return 0.0; - } - - return -calcSignedArcLength(points, dst_point, src_idx); -} - -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point); -extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); - -/** - * @brief calculate length of 2D distance between two points, specified by start point and end - * point. - * @param points points of trajectory, path, ... - * @param src_point start point - * @param dst_point end point - * @return length of distance between two points. - * Length is positive if destination point is greater that source point (i.e. after it in - * trajectory, path, ...) and negative otherwise. - * - */ -template -double calcSignedArcLength( - const T & points, const geometry_msgs::msg::Point & src_point, - const geometry_msgs::msg::Point & dst_point) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error(e.what()); - return 0.0; - } - - const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); - const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point); - - const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_seg_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - const double signed_length_dst_offset = - calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); - - return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; -} - -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); - -/** - * @brief calculate length of 2D distance for whole points container, from its start to its end. - * @param points points of trajectory, path, ... - * @return length of 2D distance for points container - */ -template -double calcArcLength(const T & points) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error(e.what()); - return 0.0; - } - - return calcSignedArcLength(points, 0, points.size() - 1); -} - -extern template double calcArcLength>( - const std::vector & points); -extern template double -calcArcLength>( - const std::vector & points); -extern template double -calcArcLength>( - const std::vector & points); - -/** - * @brief calculate curvature through points container. - * The method used for calculating the curvature is using 3 consecutive points through the points - * container. Then the curvature is the reciprocal of the radius of the circle that passes through - * these three points. - * @details more details here : https://en.wikipedia.org/wiki/Menger_curvature - * @param points points of trajectory, path, ... - * @return calculated curvature container through points container - */ -template -std::vector calcCurvature(const T & points) -{ - std::vector curvature_vec(points.size(), 0.0); - if (points.size() < 3) { - return curvature_vec; - } - - 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)); - } - curvature_vec.at(0) = curvature_vec.at(1); - curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2); - - return curvature_vec; -} - -extern template std::vector -calcCurvature>( - const std::vector & points); -extern template std::vector -calcCurvature>( - const std::vector & points); -extern template std::vector -calcCurvature>( - const std::vector & points); - -/** - * @brief calculate curvature through points container and length of 2d distance for segment used - * for curvature calculation. The method used for calculating the curvature is using 3 consecutive - * points through the points container. Then the curvature is the reciprocal of the radius of the - * circle that passes through these three points. Then length of 2D distance of these points is - * calculated - * @param points points of trajectory, path, ... - * @return Container of pairs, calculated curvature and length of 2D distance for segment used for - * curvature calculation - */ -template -std::vector> calcCurvatureAndArcLength(const T & points) -{ - // Note that arclength is for the segment, not the sum. - 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)); - curvature_arc_length_vec.emplace_back(curvature, arc_length); - } - curvature_arc_length_vec.emplace_back(0.0, 0.0); - - return curvature_arc_length_vec; -} - -extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); -extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); -extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); - -/** - * @brief calculate length of 2D distance between given start point index in points container and - * first point in container with zero longitudinal velocity - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @return Length of 2D distance between start point index in points container and first point in - * container with zero longitudinal velocity - */ -template -std::optional calcDistanceToForwardStopPoint( - const T & points_with_twist, const size_t src_idx = 0) -{ - try { - validateNonEmpty(points_with_twist); - } catch (const std::exception & e) { - log_error(e.what()); - return {}; - } - - const auto closest_stop_idx = - searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); - if (!closest_stop_idx) { - return std::nullopt; - } - - return std::max(0.0, calcSignedArcLength(points_with_twist, src_idx, *closest_stop_idx)); -} - -extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, - const size_t src_idx = 0); - -/** - * @brief calculate the point offset from source point index along the trajectory (or path) (points - * container) - * @param points points of trajectory, path, ... - * @param src_idx index of source point - * @param offset length of offset from source point - * @param throw_exception flag to enable/disable exception throwing - * @return offset point - */ -template -std::optional calcLongitudinalOffsetPoint( - const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error(e.what()); - return {}; - } - - if (points.size() - 1 < src_idx) { - const std::string error_message( - "[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(); - if (throw_exception) { - throw std::out_of_range(error_message); - } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); - return {}; - } - - if (points.size() == 1) { - return {}; - } - - if (src_idx + 1 == points.size() && offset == 0.0) { - return tier4_autoware_utils::getPoint(points.at(src_idx)); - } - - if (offset < 0.0) { - auto reverse_points = points; - std::reverse(reverse_points.begin(), reverse_points.end()); - return calcLongitudinalOffsetPoint( - reverse_points, reverse_points.size() - src_idx - 1, -offset); - } - - double dist_sum = 0.0; - - for (size_t i = src_idx; i < points.size() - 1; ++i) { - 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); - dist_sum += dist_segment; - - const auto dist_res = offset - dist_sum; - if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPoint( - p_back, p_front, std::abs(dist_res / dist_segment)); - } - } - - // not found (out of range) - return {}; -} - -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, - const double offset, const bool throw_exception = false); -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception = false); -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception = false); - -/** - * @brief calculate the point offset from source point along the trajectory (or path) (points - * container) - * @param points points of trajectory, path, ... - * @param src_point source point - * @param offset length of offset from source point - * @return offset point - */ -template -std::optional calcLongitudinalOffsetPoint( - const T & points, const geometry_msgs::msg::Point & src_point, const double offset) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); - return {}; - } - - if (offset < 0.0) { - auto reverse_points = points; - std::reverse(reverse_points.begin(), reverse_points.end()); - return calcLongitudinalOffsetPoint(reverse_points, src_point, -offset); - } - - const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - - return calcLongitudinalOffsetPoint(points, src_seg_idx, offset + signed_length_src_offset); -} - -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); - -/** - * @brief calculate the point offset from source point index along the trajectory (or path) (points - * container) - * @param points points of trajectory, path, ... - * @param src_idx index of source point - * @param offset length of offset from source point - * @param set_orientation_from_position_direction set orientation by spherical interpolation if - * false - * @return offset pose - */ -template -std::optional calcLongitudinalOffsetPose( - const T & points, const size_t src_idx, const double offset, - const bool set_orientation_from_position_direction = true, const bool throw_exception = false) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); - return {}; - } - - if (points.size() - 1 < src_idx) { - const std::string error_message( - "[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(); - if (throw_exception) { - throw std::out_of_range(error_message); - } - log_error(error_message); - return {}; - } - - if (points.size() == 1) { - log_error("Failed to calculate longitudinal offset: points size is one."); - return {}; - } - - if (src_idx + 1 == points.size() && offset == 0.0) { - return tier4_autoware_utils::getPose(points.at(src_idx)); - } - - if (offset < 0.0) { - auto reverse_points = points; - std::reverse(reverse_points.begin(), reverse_points.end()); - - double dist_sum = 0.0; - - for (size_t i = reverse_points.size() - src_idx - 1; i < reverse_points.size() - 1; ++i) { - 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); - dist_sum += dist_segment; - - const auto dist_res = -offset - dist_sum; - if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPose( - p_back, p_front, std::abs(dist_res / dist_segment), - set_orientation_from_position_direction); - } - } - } else { - double dist_sum = 0.0; - - for (size_t i = src_idx; i < points.size() - 1; ++i) { - 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); - dist_sum += dist_segment; - - const auto dist_res = offset - dist_sum; - if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPose( - p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), - set_orientation_from_position_direction); - } - } - } - - // not found (out of range) - return {}; -} - -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, - const double offset, const bool set_orientation_from_position_direction = true, - const bool throw_exception = false); -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, - const bool set_orientation_from_position_direction = true, const bool throw_exception = false); -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, - const bool set_orientation_from_position_direction = true, const bool throw_exception = false); - -/** - * @brief calculate the point offset from source point along the trajectory (or path) (points - * container) - * @param points points of trajectory, path, ... - * @param src_point source point - * @param offset length of offset from source point - * @param set_orientation_from_position_direction set orientation by spherical interpolation if - * false - * @return offset pose - */ -template -std::optional calcLongitudinalOffsetPose( - const T & points, const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction = true) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error(e.what()); - return {}; - } - - const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - - return calcLongitudinalOffsetPose( - points, src_seg_idx, offset + signed_length_src_offset, - set_orientation_from_position_direction); -} - -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction = true); -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction = true); -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction = true); - -/** - * @brief insert a point in points container (trajectory, path, ...) using segment id - * @param seg_idx segment index of point at beginning of length - * @param p_target point to be inserted - * @param points output points of trajectory, path, ... - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of segment id, where point is inserted - */ -template -std::optional insertTargetPoint( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points, - const double overlap_threshold = 1e-3) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - log_error(e.what()); - return {}; - } - - // invalid segment index - if (seg_idx + 1 >= points.size()) { - 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)); - - try { - validateNonSharpAngle(p_front, p_target, p_back); - } catch (const std::exception & e) { - log_error(e.what()); - return {}; - } - - const auto overlap_with_front = - tier4_autoware_utils::calcDistance2d(p_target, p_front) < overlap_threshold; - const auto overlap_with_back = - tier4_autoware_utils::calcDistance2d(p_target, p_back) < overlap_threshold; - - const auto is_driving_forward = isDrivingForward(points); - if (!is_driving_forward) { - return {}; - } - - 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); - - target_pose.position = p_target; - target_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); - } - - auto p_insert = points.at(seg_idx); - tier4_autoware_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); - - base_pose.position = tier4_autoware_utils::getPoint(p_base); - base_pose.orientation = tier4_autoware_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)); - } else { - tier4_autoware_utils::setPose(base_pose, points.at(seg_idx + 1)); - } - points.insert(points.begin() + seg_idx + 1, p_insert); - return seg_idx + 1; - } - - if (overlap_with_back) { - return seg_idx + 1; - } - - return seg_idx; -} - -extern template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); - -/** - * @brief insert a point in points container (trajectory, path, ...) using length of point to be - * inserted - * @param insert_point_length length to insert point from the beginning of the points - * @param p_target point to be inserted - * @param points output points of trajectory, path, ... - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of segment id, where point is inserted - */ -template -std::optional insertTargetPoint( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, T & points, - const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points); - - if (insert_point_length < 0.0) { - return std::nullopt; - } - - // Get Nearest segment index - std::optional segment_idx = std::nullopt; - for (size_t i = 1; i < points.size(); ++i) { - // TODO(Mamoru Sobue): find accumulated sum beforehand - const double length = calcSignedArcLength(points, 0, i); - if (insert_point_length <= length) { - segment_idx = i - 1; - break; - } - } - - if (!segment_idx) { - return std::nullopt; - } - - return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); -} - -extern template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); - -/** - * @brief insert a point in points container (trajectory, path, ...) using segment index and length - * of point to be inserted - * @param src_segment_idx source segment index on the trajectory - * @param insert_point_length length to insert point from the beginning of the points - * @param points output points of trajectory, path, ... - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of insert point - */ -template -std::optional insertTargetPoint( - const size_t src_segment_idx, const double insert_point_length, T & points, - const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points); - - if (src_segment_idx >= points.size() - 1) { - return std::nullopt; - } - - // Get Nearest segment index - std::optional segment_idx = std::nullopt; - if (0.0 <= insert_point_length) { - for (size_t i = src_segment_idx + 1; i < points.size(); ++i) { - const double length = calcSignedArcLength(points, src_segment_idx, i); - if (insert_point_length <= length) { - segment_idx = i - 1; - break; - } - } - } else { - for (int i = src_segment_idx - 1; 0 <= i; --i) { - const double length = calcSignedArcLength(points, src_segment_idx, i); - if (length <= insert_point_length) { - segment_idx = i; - break; - } - } - } - - if (!segment_idx) { - return std::nullopt; - } - - // Get Target Point - const double segment_length = calcSignedArcLength(points, *segment_idx, *segment_idx + 1); - 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); - - return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); -} - -extern template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold = 1e-3); - -/** - * @brief Insert a target point from a source pose on the trajectory - * @param src_pose source pose on the trajectory - * @param insert_point_length length to insert point from the beginning of the points - * @param points output points of trajectory, path, ... - * @param max_dist max distance, used to search for nearest segment index in points container to the - * given source pose - * @param max_yaw max yaw, used to search for nearest segment index in points container to the given - * source pose - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of insert point - */ -template -std::optional insertTargetPoint( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, T & points, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points); - - if (insert_point_length < 0.0) { - return std::nullopt; - } - - const auto nearest_segment_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); - if (!nearest_segment_idx) { - return std::nullopt; - } - - const double offset_length = - calcLongitudinalOffsetToSegment(points, *nearest_segment_idx, src_pose.position); - - return insertTargetPoint( - *nearest_segment_idx, insert_point_length + offset_length, points, overlap_threshold); -} - -extern template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); - -/** - * @brief Insert stop point from the source segment index - * @param src_segment_idx start segment index on the trajectory - * @param distance_to_stop_point distance to stop point from the source index - * @param points_with_twist output points of trajectory, path, ... (with velocity) - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of stop point - */ -template -std::optional insertStopPoint( - const size_t src_segment_idx, const double distance_to_stop_point, T & points_with_twist, - const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points_with_twist); - - if (distance_to_stop_point < 0.0 || src_segment_idx >= points_with_twist.size() - 1) { - return std::nullopt; - } - - const auto stop_idx = insertTargetPoint( - src_segment_idx, distance_to_stop_point, points_with_twist, overlap_threshold); - if (!stop_idx) { - return std::nullopt; - } - - for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); - } - - return stop_idx; -} - -extern template std::optional -insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); - -/** - * @brief Insert stop point from the front point of the path - * @param distance_to_stop_point distance to stop point from the front point of the path - * @param points_with_twist output points of trajectory, path, ... (with velocity) - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of stop point - */ -template -std::optional insertStopPoint( - const double distance_to_stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points_with_twist); - - if (distance_to_stop_point < 0.0) { - return std::nullopt; - } - - 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); - 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); - } - accumulated_length += length; - } - - return std::nullopt; -} - -extern template std::optional -insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); - -/** - * @brief Insert Stop point from the source pose - * @param src_pose source pose - * @param distance_to_stop_point distance to stop point from the src point - * @param points_with_twist output points of trajectory, path, ... (with velocity) - * @param max_dist max distance, used to search for nearest segment index in points container to the - * given source pose - * @param max_yaw max yaw, used to search for nearest segment index in points container to the given - * source pose - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of stop point - */ -template -std::optional insertStopPoint( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - T & points_with_twist, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points_with_twist); - - if (distance_to_stop_point < 0.0) { - return std::nullopt; - } - - const auto stop_idx = insertTargetPoint( - src_pose, distance_to_stop_point, points_with_twist, max_dist, max_yaw, overlap_threshold); - - if (!stop_idx) { - return std::nullopt; - } - - for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); - } - - return stop_idx; -} - -extern template std::optional -insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); - -/** - * @brief Insert Stop point that is in the stop segment index - * @param stop_seg_idx segment index of the stop pose - * @param stop_point stop point - * @param points_with_twist output points of trajectory, path, ... (with velocity) - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of stop point - */ -template -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); - - 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)); - } - - return insert_idx; -} - -extern template std::optional -insertStopPoint>( - const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); - -/** - * @brief Insert deceleration point from the source pose - * @param src_point source point - * @param distance_to_decel_point distance to deceleration point from the src point - * @param velocity velocity of stop point - * @param points_with_twist output points of trajectory, path, ... (with velocity) - */ -template -std::optional insertDecelPoint( - const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, - const double velocity, T & points_with_twist) -{ - const auto decel_point = - calcLongitudinalOffsetPoint(points_with_twist, src_point, distance_to_decel_point); - - if (!decel_point) { - return {}; - } - - const auto seg_idx = findNearestSegmentIndex(points_with_twist, decel_point.value()); - const auto insert_idx = insertTargetPoint(seg_idx, decel_point.value(), points_with_twist); - - if (!insert_idx) { - return {}; - } - - 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( - std::min(original_velocity, velocity), points_with_twist.at(i)); - } - - return insert_idx; -} - -extern template std::optional -insertDecelPoint>( - const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, - const double velocity, - std::vector & points_with_twist); - -/** - * @brief Insert orientation to each point in points container (trajectory, path, ...) - * @param points points of trajectory, path, ... (input / output) - * @param is_driving_forward flag indicating the order of points is forward or backward - */ -template -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)); - 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)); - } - } - } 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)); - } - // Initial orientation is same as the point after it - tier4_autoware_utils::setOrientation( - tier4_autoware_utils::getPose(points.at(1)).orientation, points.at(0)); - } -} - -extern template void insertOrientation>( - std::vector & points, const bool is_driving_forward); -extern template void -insertOrientation>( - std::vector & points, - const bool is_driving_forward); -extern template void -insertOrientation>( - std::vector & points, - const bool is_driving_forward); - -/** - * @brief Remove points with invalid orientation differences from a given points container - * (trajectory, path, ...). Check the difference between the angles of two points and the difference - * between the azimuth angle between the two points and the angle of the next point. - * @param points Points of trajectory, path, or other point container (input / output) - * @param max_yaw_diff Maximum acceptable yaw angle difference between two consecutive points in - * radians (default: M_PI_2) - */ -template -void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) -{ - for (size_t i = 1; i < points.size();) { - const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1)); - const auto p2 = tier4_autoware_utils::getPose(points.at(i)); - 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)) { - points.erase(points.begin() + i); - } else { - ++i; - } - } -} - -/** - * @brief calculate length of 2D distance between two points, specified by start point and end - * point with their segment indices in points container - * @param points points of trajectory, path, ... - * @param src_point start point - * @param src_seg_idx index of start point segment - * @param dst_point end point - * @param dst_seg_idx index of end point segment - * @return length of distance between two points. - * Length is positive if destination point is greater that source point (i.e. after it in - * trajectory, path, ...) and negative otherwise. - */ -template -double calcSignedArcLength( - const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx) -{ - validateNonEmpty(points); - - const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_seg_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - const double signed_length_dst_offset = - calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); - - return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; -} - -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); - -/** - * @brief calculate length of 2D distance between two points, specified by start point and its - * segment index in points container and end point index in points container - * @param points points of trajectory, path, ... - * @param src_point start point - * @param src_seg_idx index of start point segment - * @param dst_idx index of end point - * @return length of distance between two points - * Length is positive if destination point associated to dst_idx is greater that source point (i.e. - * after it in trajectory, path, ...) and negative otherwise. - */ -template -double calcSignedArcLength( - const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const size_t dst_idx) -{ - validateNonEmpty(points); - - const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - - return signed_length_on_traj - signed_length_src_offset; -} - -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); - -/** - * @brief calculate length of 2D distance between two points, specified by start point index in - * points container and end point and its segment index in points container - * @param points points of trajectory, path, ... - * @param src_idx index of start point start point - * @param dst_point end point - * @param dst_seg_idx index of end point segment - * @return length of distance between two points - * Length is positive if destination point is greater that source point associated to src_idx (i.e. - * after it in trajectory, path, ...) and negative otherwise. - */ -template -double calcSignedArcLength( - const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, - const size_t dst_seg_idx) -{ - validateNonEmpty(points); - - const double signed_length_on_traj = calcSignedArcLength(points, src_idx, dst_seg_idx); - const double signed_length_dst_offset = - calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); - - return signed_length_on_traj + signed_length_dst_offset; -} - -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); - -/** - * @brief find first nearest point index through points container for a given pose with soft - * distance and yaw constraints. Finding nearest point is determined by looping through the points - * container, and finding the nearest point to the given pose in terms of squared 2D distance and - * yaw deviation. The index of the point with minimum distance and yaw deviation comparing to the - * given pose will be returned. - * @param points points of trajectory, path, ... - * @param pose given pose - * @param dist_threshold distance threshold used for searching for first nearest index to given pose - * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose - * @return index of nearest point (index or none if not found) - */ -template -size_t findFirstNearestIndexWithSoftConstraints( - const T & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()) -{ - validateNonEmpty(points); - - { // with dist and yaw thresholds - const double squared_dist_threshold = dist_threshold * dist_threshold; - double min_squared_dist = std::numeric_limits::max(); - size_t min_idx = 0; - 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); - - if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) { - if (is_within_constraints) { - break; - } - continue; - } - - if (min_squared_dist <= squared_dist) { - continue; - } - - min_squared_dist = squared_dist; - min_idx = i; - is_within_constraints = true; - } - - // nearest index is found - if (is_within_constraints) { - return min_idx; - } - } - - { // with dist threshold - const double squared_dist_threshold = dist_threshold * dist_threshold; - double min_squared_dist = std::numeric_limits::max(); - size_t min_idx = 0; - 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); - - if (squared_dist_threshold < squared_dist) { - if (is_within_constraints) { - break; - } - continue; - } - - if (min_squared_dist <= squared_dist) { - continue; - } - - min_squared_dist = squared_dist; - min_idx = i; - is_within_constraints = true; - } - - // nearest index is found - if (is_within_constraints) { - return min_idx; - } - } - - // without any threshold - return findNearestIndex(points, pose.position); -} - -extern template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); -extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); -extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); - -/** - * @brief find nearest segment index to pose with soft constraints - * Segment is straight path between two continuous points of trajectory - * When pose is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 - * @param points points of trajectory, path, .. - * @param pose pose to which to find nearest segment index - * @param dist_threshold distance threshold used for searching for first nearest index to given pose - * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose - * @return nearest index - */ -template -size_t findFirstNearestSegmentIndexWithSoftConstraints( - const T & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()) -{ - // find first nearest index with soft constraints (not segment index) - const size_t nearest_idx = - findFirstNearestIndexWithSoftConstraints(points, pose, dist_threshold, yaw_threshold); - - // calculate segment index - if (nearest_idx == 0) { - return 0; - } - if (nearest_idx == points.size() - 1) { - return points.size() - 2; - } - - const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, pose.position); - - if (signed_length <= 0) { - return nearest_idx - 1; - } - - return nearest_idx; -} - -extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); -extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); -extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); - -/** - * @brief calculate the point offset from source point along the trajectory (or path) - * @brief calculate length of 2D distance between given pose and first point in container with zero - * longitudinal velocity - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @param pose given pose to start the distance calculation from - * @param max_dist max distance, used to search for nearest segment index in points container to the - * given pose - * @param max_yaw max yaw, used to search for nearest segment index in points container to the given - * pose - * @return Length of 2D distance between given pose and first point in container with zero - * longitudinal velocity - */ -template -std::optional calcDistanceToForwardStopPoint( - const T & points_with_twist, const geometry_msgs::msg::Pose & pose, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()) -{ - try { - validateNonEmpty(points_with_twist); - } catch (const std::exception & e) { - log_error("Failed to calculate stop distance" + std::string(e.what())); - return {}; - } - - const auto nearest_segment_idx = - 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( - points_with_twist, *nearest_segment_idx + 1, points_with_twist.size()); - - if (!stop_idx) { - return std::nullopt; - } - - const auto closest_stop_dist = - calcSignedArcLength(points_with_twist, pose.position, *nearest_segment_idx, *stop_idx); - - return std::max(0.0, closest_stop_dist); -} - -extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); - -// NOTE: Points after forward length from the point will be cropped -// forward_length is assumed to be positive. -template -T cropForwardPoints( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length) -{ - if (points.empty()) { - return T{}; - } - - double sum_length = - -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)); - if (forward_length < sum_length) { - const size_t end_idx = i; - return T{points.begin(), points.begin() + end_idx}; - } - } - - return points; -} - -extern template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); - -// NOTE: Points before backward length from the point will be cropped -// backward_length is assumed to be positive. -template -T cropBackwardPoints( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length) -{ - if (points.empty()) { - return T{}; - } - - double sum_length = - -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)); - if (sum_length < -backward_length) { - const size_t begin_idx = i; - return T{points.begin() + begin_idx, points.end()}; - } - } - - return points; -} - -extern template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); - -template -T cropPoints( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length) -{ - if (points.empty()) { - return T{}; - } - - // NOTE: Cropping forward must be done first in order to keep target_seg_idx. - const auto cropped_forward_points = - cropForwardPoints(points, target_pos, target_seg_idx, forward_length); - - const size_t modified_target_seg_idx = - std::min(target_seg_idx, cropped_forward_points.size() - 2); - const auto cropped_points = cropBackwardPoints( - cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); - - if (cropped_points.size() < 2) { - log_error("Return original points since cropped_points size is less than 2."); - return points; - } - - return cropped_points; -} - -extern template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); - -/** - * @brief Calculate the angle of the input pose with respect to the nearest trajectory segment. - * The function gets the nearest segment index between the points of the trajectory and the given - * pose's position, then calculates the azimuth angle of that segment and compares it to the yaw of - * the input pose. The segment is a straight path between two continuous points of the trajectory. - * @param points Points of the trajectory, path, ... - * @param pose Input pose with position and orientation (yaw) - * @param throw_exception Flag to enable/disable exception throwing - * @return Angle with respect to the trajectory segment (signed) in radians - */ -template -double calcYawDeviation( - const T & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false) -{ - const auto overlap_removed_points = removeOverlapPoints(points, 0); - - if (throw_exception) { - validateNonEmpty(overlap_removed_points); - } else { - try { - validateNonEmpty(overlap_removed_points); - } catch (const std::exception & e) { - log_error(e.what()); - return 0.0; - } - } - - if (overlap_removed_points.size() <= 1) { - const std::string error_message( - "[motion_utils] " + std::string(__func__) + - " Given points size is less than 2. Failed to calculate yaw deviation."); - tier4_autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::runtime_error(error_message); - } - log_error( - error_message + - " Return 0 since no_throw option is enabled. The maintainer must check the code."); - return 0.0; - } - - 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 pose_yaw = tf2::getYaw(pose.orientation); - - return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw); -} - -extern template double calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); -extern template double -calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); -extern template double -calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); - -/** - * @brief Check if the given target point is in front of the based pose from the trajectory. - * if the points is empty, the function returns false - * @param points Points of the trajectory, path, ... - * @param base_point Base point - * @param target_point Target point - * @param threshold threshold for judging front point - * @return true if the target pose is in front of the base pose - */ -template -bool isTargetPointFront( - const T & points, const geometry_msgs::msg::Point & base_point, - const geometry_msgs::msg::Point & target_point, const double threshold = 0.0) -{ - if (points.empty()) { - return false; - } - - const double s_base = calcSignedArcLength(points, 0, base_point); - const double s_target = calcSignedArcLength(points, 0, target_point); - - return s_target - s_base > threshold; -} - -extern template bool isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold = 0.0); -extern template bool -isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold = 0.0); -extern template bool -isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold = 0.0); - -} // namespace motion_utils - -#endif // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml deleted file mode 100644 index 5f630572c061c..0000000000000 --- a/common/motion_utils/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - motion_utils - 0.1.0 - The motion_utils package - Satoshi Ota - Takayuki Murooka - - Fumiya Watanabe - Kosuke Takeuchi - Taiki Tanaka - Takamasa Horibe - Tomoya Kimura - Mamoru Sobue - Apache License 2.0 - - Takayuki Murooka - Satoshi Ota - - ament_cmake_auto - autoware_cmake - - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs - builtin_interfaces - geometry_msgs - interpolation - libboost-dev - rclcpp - tf2 - tf2_geometry_msgs - tier4_autoware_utils - visualization_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/motion_utils/src/distance/distance.cpp b/common/motion_utils/src/distance/distance.cpp deleted file mode 100644 index 3ac37bbc7733b..0000000000000 --- a/common/motion_utils/src/distance/distance.cpp +++ /dev/null @@ -1,272 +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 "motion_utils/distance/distance.hpp" - -namespace motion_utils -{ -namespace -{ -bool validCheckDecelPlan( - const double v_end, const double a_end, const double v_target, const double a_target, - const double v_margin, const double a_margin) -{ - const double v_min = v_target - std::abs(v_margin); - const double v_max = v_target + std::abs(v_margin); - const double a_min = a_target - std::abs(a_margin); - const double a_max = a_target + std::abs(a_margin); - - if (v_end < v_min || v_max < v_end) { - return false; - } - if (a_end < a_min || a_max < a_end) { - return false; - } - - return true; -} - -/** - * @brief update traveling distance, velocity and acceleration under constant jerk. - * @param (x) current traveling distance [m/s] - * @param (v) current velocity [m/s] - * @param (a) current acceleration [m/ss] - * @param (j) target jerk [m/sss] - * @param (t) time [s] - * @return updated traveling distance, velocity and acceleration - */ -std::tuple update( - const double x, const double v, const double a, const double j, const double t) -{ - const double a_new = a + j * t; - const double v_new = v + a * t + 0.5 * j * t * t; - const double x_new = x + v * t + 0.5 * a * t * t + (1.0 / 6.0) * j * t * t * t; - - return {x_new, v_new, a_new}; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE: TRAPEZOID - * ACCELERATION PROFILE). this type of profile has ZERO JERK time. - * - * [ACCELERATION PROFILE] - * a ^ - * | - * a0 * - * |* - * ----+-*-------------------*------> t - * | * * - * | * * - * | a1 *************** - * | - * - * [JERK PROFILE] - * j ^ - * | - * | ja **** - * | * - * ----+----***************---------> t - * | * - * | * - * jd ****** - * | - * - * @param (v0) current velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (vt) target velocity [m/s] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @param (t_during_min_acc) duration of constant deceleration [s] - * @return moving distance until velocity is reached vt [m] - */ -std::optional calcDecelDistPlanType1( - const double v0, const double vt, const double a0, const double am, const double ja, - const double jd, const double t_during_min_acc) -{ - constexpr double epsilon = 1e-3; - - // negative jerk time - const double j1 = am < a0 ? jd : ja; - const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; - const auto [x1, v1, a1] = update(0.0, v0, a0, j1, t1); - - // zero jerk time - const double t2 = epsilon < t_during_min_acc ? t_during_min_acc : 0.0; - const auto [x2, v2, a2] = update(x1, v1, a1, 0.0, t2); - - // positive jerk time - const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; - const auto [x3, v3, a3] = update(x2, v2, a2, ja, t3); - - const double a_target = 0.0; - const double v_margin = 0.3; // [m/s] - const double a_margin = 0.1; // [m/s^2] - - if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x3; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE: TRIANGLE - * ACCELERATION PROFILE), This type of profile do NOT have ZERO JERK time. - * - * [ACCELERATION PROFILE] - * a ^ - * | - * a0 * - * |* - * ----+-*-----*--------------------> t - * | * * - * | * * - * | a1 * - * | - * - * [JERK PROFILE] - * j ^ - * | - * | ja **** - * | * - * ----+----*-----------------------> t - * | * - * | * - * jd ****** - * | - * - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - */ -std::optional calcDecelDistPlanType2( - const double v0, const double vt, const double a0, const double ja, const double jd) -{ - constexpr double epsilon = 1e-3; - - const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); - const double a1 = -std::sqrt(a1_square); - - // negative jerk time - const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; - const auto [x1, v1, no_use_a1] = update(0.0, v0, a0, jd, t1); - - // positive jerk time - const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; - const auto [x2, v2, a2] = update(x1, v1, a1, ja, t2); - - const double a_target = 0.0; - const double v_margin = 0.3; - const double a_margin = 0.1; - - if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x2; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE: LINEAR ACCELERATION - * PROFILE). This type of profile has only positive jerk time. - * - * [ACCELERATION PROFILE] - * a ^ - * | - * ----+----*-----------------------> t - * | * - * | * - * | * - * |* - * a0 * - * | - * - * [JERK PROFILE] - * j ^ - * | - * ja ****** - * | * - * | * - * ----+----*-----------------------> t - * | - * - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - */ -std::optional calcDecelDistPlanType3( - const double v0, const double vt, const double a0, const double ja) -{ - constexpr double epsilon = 1e-3; - - // positive jerk time - const double t_acc = (0.0 - a0) / ja; - const double t1 = epsilon < t_acc ? t_acc : 0.0; - const auto [x1, v1, a1] = update(0.0, v0, a0, ja, t1); - - const double a_target = 0.0; - const double v_margin = 0.3; - const double a_margin = 0.1; - - if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x1; -} -} // namespace - -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) -{ - if (current_vel < target_vel) { - return {}; - } - - constexpr double epsilon = 1e-3; - const double jerk_before_min_acc = acc_min < current_acc ? jerk_dec : jerk_acc; - const double t_before_min_acc = (acc_min - current_acc) / jerk_before_min_acc; - const double jerk_after_min_acc = jerk_acc; - const double t_after_min_acc = (0.0 - acc_min) / jerk_after_min_acc; - - const double t_during_min_acc = - (target_vel - current_vel - current_acc * t_before_min_acc - - 0.5 * jerk_before_min_acc * std::pow(t_before_min_acc, 2) - acc_min * t_after_min_acc - - 0.5 * jerk_after_min_acc * std::pow(t_after_min_acc, 2)) / - acc_min; - - // check if it is possible to decelerate to the target velocity - // by simply bringing the current acceleration to zero. - const auto is_decel_needed = - 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; - - if (t_during_min_acc > epsilon) { - return calcDecelDistPlanType1( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_during_min_acc); - } - if (is_decel_needed || current_acc > epsilon) { - return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); - } - - return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); -} -} // namespace motion_utils diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp deleted file mode 100644 index 6e9c45adc241b..0000000000000 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ /dev/null @@ -1,143 +0,0 @@ -// Copyright 2021 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 "motion_utils/marker/marker_helper.hpp" - -#include "tier4_autoware_utils/ros/marker_helper.hpp" - -#include - -#include - -using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createDeletedDefaultMarker; -using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerScale; -using visualization_msgs::msg::MarkerArray; - -namespace -{ - -inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( - const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, - const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, - const std_msgs::msg::ColorRGBA & color) -{ - visualization_msgs::msg::MarkerArray marker_array; - - // Virtual Wall - { - auto marker = createDefaultMarker( - "map", now, ns_prefix + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(0.1, 5.0, 2.0), color); - - marker.pose = vehicle_front_pose; - marker.pose.position.z += 1.0; - - marker_array.markers.push_back(marker); - } - - // Factor Text - { - auto marker = createDefaultMarker( - "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); - - marker.pose = vehicle_front_pose; - marker.pose.position.z += 2.0; - marker.text = module_name; - - marker_array.markers.push_back(marker); - } - - return marker_array; -} - -inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( - const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id) -{ - visualization_msgs::msg::MarkerArray marker_array; - - // Virtual Wall - { - auto marker = createDeletedDefaultMarker(now, ns_prefix + "virtual_wall", id); - marker_array.markers.push_back(marker); - } - - // Factor Text - { - auto marker = createDeletedDefaultMarker(now, ns_prefix + "factor_text", id); - marker_array.markers.push_back(marker); - } - - return marker_array; -} -} // namespace - -namespace motion_utils -{ -visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( - const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, - const bool is_driving_forward) -{ - const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( - pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); - return createVirtualWallMarkerArray( - pose_with_offset, module_name, ns_prefix + "stop_", now, id, - createMarkerColor(1.0, 0.0, 0.0, 0.5)); -} - -visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( - const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, - const bool is_driving_forward) -{ - const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( - pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); - return createVirtualWallMarkerArray( - pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, - createMarkerColor(1.0, 1.0, 0.0, 0.5)); -} - -visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( - const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, - const bool is_driving_forward) -{ - const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( - pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); - return createVirtualWallMarkerArray( - pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, - createMarkerColor(0.0, 1.0, 0.0, 0.5)); -} - -visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( - const rclcpp::Time & now, const int32_t id) -{ - return createDeletedVirtualWallMarkerArray("stop_", now, id); -} - -visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker( - const rclcpp::Time & now, const int32_t id) -{ - return createDeletedVirtualWallMarkerArray("slow_down_", now, id); -} - -visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( - const rclcpp::Time & now, const int32_t id) -{ - return createDeletedVirtualWallMarkerArray("dead_line_", now, id); -} -} // namespace motion_utils diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp deleted file mode 100644 index 834d07a87ea09..0000000000000 --- a/common/motion_utils/src/resample/resample.cpp +++ /dev/null @@ -1,727 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "motion_utils/resample/resample.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 -{ -std::vector resamplePointVector( - const std::vector & points, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, - const bool use_lerp_for_z) -{ - // validate arguments - if (!resample_utils::validate_arguments(points, resampled_arclength)) { - return points; - } - - // Input Path Information - std::vector input_arclength; - std::vector x; - std::vector y; - std::vector z; - input_arclength.reserve(points.size()); - x.reserve(points.size()); - y.reserve(points.size()); - z.reserve(points.size()); - - input_arclength.push_back(0.0); - x.push_back(points.front().x); - y.push_back(points.front().y); - z.push_back(points.front().z); - 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); - input_arclength.push_back(ds + input_arclength.back()); - x.push_back(curr_pt.x); - y.push_back(curr_pt.y); - z.push_back(curr_pt.z); - } - - // Interpolate - const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); - }; - const auto spline = [&](const auto & input) { - return interpolation::spline(input_arclength, input, resampled_arclength); - }; - const auto spline_by_akima = [&](const auto & input) { - return interpolation::splineByAkima(input_arclength, input, resampled_arclength); - }; - - const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x); - const auto interpolated_y = use_akima_spline_for_xy ? lerp(y) : spline_by_akima(y); - const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z); - - std::vector resampled_points; - resampled_points.resize(interpolated_x.size()); - - // Insert Position - for (size_t i = 0; i < resampled_points.size(); ++i) { - geometry_msgs::msg::Point point; - point.x = interpolated_x.at(i); - point.y = interpolated_y.at(i); - point.z = interpolated_z.at(i); - resampled_points.at(i) = point; - } - - return resampled_points; -} - -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); - - 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; - return points; - } - - // Insert terminal point - if (input_length - resampling_arclength.back() < motion_utils::overlap_threshold) { - resampling_arclength.back() = input_length; - } else { - resampling_arclength.push_back(input_length); - } - - return resamplePointVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); -} - -std::vector resamplePoseVector( - const std::vector & points_raw, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, - const bool use_lerp_for_z) -{ - // Remove overlap points for resampling - const auto points = motion_utils::removeOverlapPoints(points_raw); - - // validate arguments - if (!resample_utils::validate_arguments(points, resampled_arclength)) { - return points_raw; - } - - std::vector position(points.size()); - for (size_t i = 0; i < points.size(); ++i) { - position.at(i) = points.at(i).position; - } - const auto resampled_position = - resamplePointVector(position, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); - - std::vector resampled_points(resampled_position.size()); - - // Insert Position - for (size_t i = 0; i < resampled_position.size(); ++i) { - geometry_msgs::msg::Pose pose; - pose.position.x = resampled_position.at(i).x; - pose.position.y = resampled_position.at(i).y; - pose.position.z = resampled_position.at(i).z; - resampled_points.at(i) = pose; - } - - const bool is_driving_forward = - tier4_autoware_utils::isDrivingForward(points.at(0), points.at(1)); - motion_utils::insertOrientation(resampled_points, is_driving_forward); - - // Initial orientation is depend on the initial value of the resampled_arclength - // when backward driving - if (!is_driving_forward && resampled_arclength.front() < 1e-3) { - resampled_points.at(0).orientation = points.at(0).orientation; - } - - return resampled_points; -} - -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); - - 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; - return points; - } - - // Insert terminal point - if (input_length - resampling_arclength.back() < motion_utils::overlap_threshold) { - resampling_arclength.back() = input_length; - } else { - resampling_arclength.push_back(input_length); - } - - return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); -} - -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, - const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) -{ - auto resampling_arclength = resampled_arclength; - - // Add resampling_arclength to insert input points which have multiple lane_ids - for (size_t i = 0; i < input_path.points.size(); ++i) { - if (input_path.points.at(i).lane_ids.size() < 2) { - continue; - } - - const double distance_to_resampling_point = calcSignedArcLength(input_path.points, 0, i); - for (size_t j = 1; j < resampling_arclength.size(); ++j) { - if ( - resampling_arclength.at(j - 1) <= distance_to_resampling_point && - distance_to_resampling_point < resampling_arclength.at(j)) { - const double dist_to_prev_point = - 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) { - resampling_arclength.at(j - 1) = distance_to_resampling_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { - resampling_arclength.at(j) = distance_to_resampling_point; - } else { - resampling_arclength.insert( - resampling_arclength.begin() + j, distance_to_resampling_point); - } - break; - } - } - } - - // validate arguments - if (!resample_utils::validate_arguments(input_path.points, resampling_arclength)) { - return input_path; - } - - // For LaneIds, is_final - // - // ------|----|----|----|----|----|----|-------> resampled - // [0] [1] [2] [3] [4] [5] [6] - // - // ------|----------------|----------|---------> base - // [0] [1] [2] - // - // resampled[0~3] = base[0] - // resampled[4~5] = base[1] - // resampled[6] = base[2] - - // Input Path Information - std::vector input_arclength; - std::vector input_pose; - std::vector v_lon; - std::vector v_lat; - std::vector heading_rate; - std::vector is_final; - std::vector> lane_ids; - input_arclength.reserve(input_path.points.size()); - input_pose.reserve(input_path.points.size()); - v_lon.reserve(input_path.points.size()); - v_lat.reserve(input_path.points.size()); - heading_rate.reserve(input_path.points.size()); - is_final.reserve(input_path.points.size()); - lane_ids.reserve(input_path.points.size()); - - input_arclength.push_back(0.0); - input_pose.push_back(input_path.points.front().point.pose); - v_lon.push_back(input_path.points.front().point.longitudinal_velocity_mps); - v_lat.push_back(input_path.points.front().point.lateral_velocity_mps); - heading_rate.push_back(input_path.points.front().point.heading_rate_rps); - is_final.push_back(input_path.points.front().point.is_final); - lane_ids.push_back(input_path.points.front().lane_ids); - for (size_t i = 1; i < input_path.points.size(); ++i) { - 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); - input_arclength.push_back(ds + input_arclength.back()); - input_pose.push_back(curr_pt.pose); - v_lon.push_back(curr_pt.longitudinal_velocity_mps); - v_lat.push_back(curr_pt.lateral_velocity_mps); - heading_rate.push_back(curr_pt.heading_rate_rps); - is_final.push_back(curr_pt.is_final); - lane_ids.push_back(input_path.points.at(i).lane_ids); - } - - if (input_arclength.back() < resampling_arclength.back()) { - std::cerr << "[motion_utils]: resampled path length is longer than input path length" - << std::endl; - return input_path; - } - - // Interpolate - const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampling_arclength); - }; - - auto closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); - - const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); - }; - - const auto interpolated_pose = - resamplePoseVector(input_pose, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); - const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); - const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); - const auto interpolated_heading_rate = lerp(heading_rate); - const auto interpolated_is_final = zoh(is_final); - - // interpolate lane_ids - std::vector> interpolated_lane_ids; - interpolated_lane_ids.resize(resampling_arclength.size()); - constexpr double epsilon = 1e-6; - for (size_t i = 0; i < resampling_arclength.size(); ++i) { - const size_t seg_idx = std::min(closest_segment_indices.at(i), input_path.points.size() - 2); - const auto & prev_lane_ids = input_path.points.at(seg_idx).lane_ids; - const auto & next_lane_ids = input_path.points.at(seg_idx + 1).lane_ids; - - if (std::abs(input_arclength.at(seg_idx) - resampling_arclength.at(i)) <= epsilon) { - interpolated_lane_ids.at(i).insert( - interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end()); - } else if (std::abs(input_arclength.at(seg_idx + 1) - resampling_arclength.at(i)) <= epsilon) { - interpolated_lane_ids.at(i).insert( - interpolated_lane_ids.at(i).end(), next_lane_ids.begin(), next_lane_ids.end()); - } else { - // extract lane_ids those prev_lane_ids and next_lane_ids have in common - for (const auto target_lane_id : prev_lane_ids) { - if ( - std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != - next_lane_ids.end()) { - interpolated_lane_ids.at(i).push_back(target_lane_id); - } - } - // If there are no common lane_ids, the prev_lane_ids is assigned. - if (interpolated_lane_ids.at(i).empty()) { - interpolated_lane_ids.at(i).insert( - interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end()); - } - } - } - - if (interpolated_pose.size() != resampling_arclength.size()) { - std::cerr << "[motion_utils]: Resampled pose size is different from resampled arclength" - << std::endl; - return input_path; - } - - autoware_auto_planning_msgs::msg::PathWithLaneId resampled_path; - resampled_path.header = input_path.header; - resampled_path.left_bound = input_path.left_bound; - resampled_path.right_bound = input_path.right_bound; - resampled_path.points.resize(interpolated_pose.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - autoware_auto_planning_msgs::msg::PathPoint path_point; - path_point.pose = interpolated_pose.at(i); - path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); - path_point.lateral_velocity_mps = interpolated_v_lat.at(i); - path_point.heading_rate_rps = interpolated_heading_rate.at(i); - path_point.is_final = interpolated_is_final.at(i); - resampled_path.points.at(i).point = path_point; - resampled_path.points.at(i).lane_ids = interpolated_lane_ids.at(i); - } - - return resampled_path; -} - -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, - const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) -{ - // validate arguments - if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { - return input_path; - } - - // transform input_path - std::vector transformed_input_path( - input_path.points.size()); - for (size_t i = 0; i < input_path.points.size(); ++i) { - 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); - - 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; - return input_path; - } - - // Insert terminal point - if (input_path_len - resampling_arclength.back() < motion_utils::overlap_threshold) { - resampling_arclength.back() = input_path_len; - } else { - resampling_arclength.push_back(input_path_len); - } - - // Insert stop point - if (resample_input_path_stop_point) { - const auto distance_to_stop_point = - 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 ( - resampling_arclength.at(i - 1) <= *distance_to_stop_point && - *distance_to_stop_point < resampling_arclength.at(i)) { - const double dist_to_prev_point = - 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) { - resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { - resampling_arclength.at(i) = *distance_to_stop_point; - } else { - resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); - } - break; - } - } - } - } - - return resamplePath( - input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, - use_zero_order_hold_for_v); -} - -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, - const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) -{ - // validate arguments - if (!resample_utils::validate_arguments(input_path.points, resampled_arclength)) { - return input_path; - } - - // Input Path Information - std::vector input_arclength; - std::vector input_pose; - std::vector v_lon; - std::vector v_lat; - std::vector heading_rate; - input_arclength.reserve(input_path.points.size()); - input_pose.reserve(input_path.points.size()); - v_lon.reserve(input_path.points.size()); - v_lat.reserve(input_path.points.size()); - heading_rate.reserve(input_path.points.size()); - - input_arclength.push_back(0.0); - input_pose.push_back(input_path.points.front().pose); - v_lon.push_back(input_path.points.front().longitudinal_velocity_mps); - v_lat.push_back(input_path.points.front().lateral_velocity_mps); - heading_rate.push_back(input_path.points.front().heading_rate_rps); - for (size_t i = 1; i < input_path.points.size(); ++i) { - 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); - input_arclength.push_back(ds + input_arclength.back()); - input_pose.push_back(curr_pt.pose); - v_lon.push_back(curr_pt.longitudinal_velocity_mps); - v_lat.push_back(curr_pt.lateral_velocity_mps); - heading_rate.push_back(curr_pt.heading_rate_rps); - } - - // Interpolate - const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); - }; - - std::vector closest_segment_indices; - if (use_zero_order_hold_for_v) { - closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); - } - const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); - }; - - const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); - const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); - const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); - 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; - return input_path; - } - - autoware_auto_planning_msgs::msg::Path resampled_path; - resampled_path.header = input_path.header; - resampled_path.left_bound = input_path.left_bound; - resampled_path.right_bound = input_path.right_bound; - resampled_path.points.resize(interpolated_pose.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - autoware_auto_planning_msgs::msg::PathPoint path_point; - path_point.pose = interpolated_pose.at(i); - path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); - path_point.lateral_velocity_mps = interpolated_v_lat.at(i); - path_point.heading_rate_rps = interpolated_heading_rate.at(i); - resampled_path.points.at(i) = path_point; - } - - return resampled_path; -} - -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, - const bool use_akima_spline_for_xy, const bool use_lerp_for_z, - const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point) -{ - // validate arguments - if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { - return input_path; - } - - const double input_path_len = 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; - return input_path; - } - - // Insert terminal point - if (input_path_len - resampling_arclength.back() < motion_utils::overlap_threshold) { - resampling_arclength.back() = input_path_len; - } else { - resampling_arclength.push_back(input_path_len); - } - - // Insert stop point - if (resample_input_path_stop_point) { - const auto distance_to_stop_point = - 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 ( - resampling_arclength.at(i - 1) <= *distance_to_stop_point && - *distance_to_stop_point < resampling_arclength.at(i)) { - const double dist_to_prev_point = - 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) { - resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { - resampling_arclength.at(i) = *distance_to_stop_point; - } else { - resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); - } - break; - } - } - } - } - - return resamplePath( - input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, - use_zero_order_hold_for_twist); -} - -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, - const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist) -{ - // validate arguments - if (!resample_utils::validate_arguments(input_trajectory.points, resampled_arclength)) { - return input_trajectory; - } - - // Input Trajectory Information - std::vector input_arclength; - std::vector input_pose; - std::vector v_lon; - std::vector v_lat; - std::vector heading_rate; - std::vector acceleration; - std::vector front_wheel_angle; - std::vector rear_wheel_angle; - std::vector time_from_start; - input_arclength.reserve(input_trajectory.points.size()); - input_pose.reserve(input_trajectory.points.size()); - v_lon.reserve(input_trajectory.points.size()); - v_lat.reserve(input_trajectory.points.size()); - heading_rate.reserve(input_trajectory.points.size()); - acceleration.reserve(input_trajectory.points.size()); - front_wheel_angle.reserve(input_trajectory.points.size()); - rear_wheel_angle.reserve(input_trajectory.points.size()); - time_from_start.reserve(input_trajectory.points.size()); - - input_arclength.push_back(0.0); - input_pose.push_back(input_trajectory.points.front().pose); - v_lon.push_back(input_trajectory.points.front().longitudinal_velocity_mps); - v_lat.push_back(input_trajectory.points.front().lateral_velocity_mps); - heading_rate.push_back(input_trajectory.points.front().heading_rate_rps); - acceleration.push_back(input_trajectory.points.front().acceleration_mps2); - front_wheel_angle.push_back(input_trajectory.points.front().front_wheel_angle_rad); - rear_wheel_angle.push_back(input_trajectory.points.front().rear_wheel_angle_rad); - time_from_start.push_back( - rclcpp::Duration(input_trajectory.points.front().time_from_start).seconds()); - for (size_t i = 1; i < input_trajectory.points.size(); ++i) { - 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); - input_arclength.push_back(ds + input_arclength.back()); - input_pose.push_back(curr_pt.pose); - v_lon.push_back(curr_pt.longitudinal_velocity_mps); - v_lat.push_back(curr_pt.lateral_velocity_mps); - heading_rate.push_back(curr_pt.heading_rate_rps); - acceleration.push_back(curr_pt.acceleration_mps2); - front_wheel_angle.push_back(curr_pt.front_wheel_angle_rad); - rear_wheel_angle.push_back(curr_pt.rear_wheel_angle_rad); - time_from_start.push_back(rclcpp::Duration(curr_pt.time_from_start).seconds()); - } - - // Interpolate - const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); - }; - - std::vector closest_segment_indices; - if (use_zero_order_hold_for_twist) { - closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); - } - const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); - }; - - const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); - const auto interpolated_v_lon = use_zero_order_hold_for_twist ? zoh(v_lon) : lerp(v_lon); - const auto interpolated_v_lat = use_zero_order_hold_for_twist ? zoh(v_lat) : lerp(v_lat); - const auto interpolated_heading_rate = lerp(heading_rate); - const auto interpolated_acceleration = - use_zero_order_hold_for_twist ? zoh(acceleration) : lerp(acceleration); - const auto interpolated_front_wheel_angle = lerp(front_wheel_angle); - const auto interpolated_rear_wheel_angle = lerp(rear_wheel_angle); - 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; - return input_trajectory; - } - - autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory; - resampled_trajectory.header = input_trajectory.header; - resampled_trajectory.points.resize(interpolated_pose.size()); - for (size_t i = 0; i < resampled_trajectory.points.size(); ++i) { - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose = interpolated_pose.at(i); - traj_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); - traj_point.lateral_velocity_mps = interpolated_v_lat.at(i); - traj_point.heading_rate_rps = interpolated_heading_rate.at(i); - traj_point.acceleration_mps2 = interpolated_acceleration.at(i); - traj_point.front_wheel_angle_rad = interpolated_front_wheel_angle.at(i); - traj_point.rear_wheel_angle_rad = interpolated_rear_wheel_angle.at(i); - traj_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time_from_start.at(i)); - resampled_trajectory.points.at(i) = traj_point; - } - - return resampled_trajectory; -} - -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, - const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point) -{ - // validate arguments - if (!resample_utils::validate_arguments(input_trajectory.points, resample_interval)) { - return input_trajectory; - } - - const double input_trajectory_len = 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; - return input_trajectory; - } - - // Insert terminal point - if (input_trajectory_len - resampling_arclength.back() < motion_utils::overlap_threshold) { - resampling_arclength.back() = input_trajectory_len; - } else { - resampling_arclength.push_back(input_trajectory_len); - } - - // Insert stop point - if (resample_input_trajectory_stop_point) { - const auto distance_to_stop_point = - 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 ( - resampling_arclength.at(i - 1) <= *distance_to_stop_point && - *distance_to_stop_point < resampling_arclength.at(i)) { - const double dist_to_prev_point = - 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) { - resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { - resampling_arclength.at(i) = *distance_to_stop_point; - } else { - resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); - } - break; - } - } - } - } - - return resampleTrajectory( - input_trajectory, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, - use_zero_order_hold_for_twist); -} - -} // namespace motion_utils diff --git a/common/motion_utils/src/trajectory/conversion.cpp b/common/motion_utils/src/trajectory/conversion.cpp deleted file mode 100644 index f198003d84091..0000000000000 --- a/common/motion_utils/src/trajectory/conversion.cpp +++ /dev/null @@ -1,54 +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 "motion_utils/trajectory/conversion.hpp" - -#include - -namespace motion_utils -{ -/** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. - * @attention This function just clips - * std::vector up to the capacity of Trajectory. - * Therefore, the error handling out of this function is necessary if the size of the input greater - * than the capacity. - * @todo Decide how to handle the situation that we need to use the trajectory with the size of - * points larger than the capacity. (Tier IV) - */ -autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory, - const std_msgs::msg::Header & header) -{ - autoware_auto_planning_msgs::msg::Trajectory output{}; - output.header = header; - for (const auto & pt : trajectory) output.points.push_back(pt); - return output; -} - -/** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. - */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) -{ - std::vector output(trajectory.points.size()); - std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); - return output; -} - -} // namespace motion_utils diff --git a/common/motion_utils/src/trajectory/trajectory.cpp b/common/motion_utils/src/trajectory/trajectory.cpp deleted file mode 100644 index 51c07a75076c8..0000000000000 --- a/common/motion_utils/src/trajectory/trajectory.cpp +++ /dev/null @@ -1,625 +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 "motion_utils/trajectory/trajectory.hpp" - -namespace motion_utils -{ - -// -template void validateNonEmpty>( - const std::vector &); -template void validateNonEmpty>( - const std::vector &); -template void validateNonEmpty>( - const std::vector &); - -// -template std::optional -isDrivingForward>( - const std::vector &); -template std::optional -isDrivingForward>( - const std::vector &); -template std::optional -isDrivingForward>( - const std::vector &); - -// -template std::optional -isDrivingForwardWithTwist>( - const std::vector &); -template std::optional -isDrivingForwardWithTwist>( - const std::vector &); -template std::optional -isDrivingForwardWithTwist>( - const std::vector &); - -// -template std::vector -removeOverlapPoints>( - const std::vector & points, const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx); - -// -template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, - const size_t src_idx, const size_t dst_idx); - -// -template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, - const size_t src_idx); - -// -template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist); - -// -template size_t findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -template size_t -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -template size_t findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); - -// -template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); - -// -template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, - const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); - -// -template size_t findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -template size_t -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -template size_t -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); - -// -template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); - -// -template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double -calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); - -// -template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double -calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception); - -// -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); - -// -template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); -template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); -template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); - -// -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector &, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector &, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); - -// -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); - -// -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); - -// -template double calcArcLength>( - const std::vector & points); -template double calcArcLength>( - const std::vector & points); -template double calcArcLength>( - const std::vector & points); - -// -template std::vector -calcCurvature>( - const std::vector & points); -template std::vector -calcCurvature>( - const std::vector & points); -template std::vector -calcCurvature>( - const std::vector & points); - -// -template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); -template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); -template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); - -// -template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, - const size_t src_idx); - -// -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, - const double offset, const bool throw_exception); -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception); -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception); - -// -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); - -// -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, - const double offset, const bool set_orientation_from_position_direction, - const bool throw_exception); -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, - const bool throw_exception); -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, - const bool throw_exception); - -// -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction); -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction); -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction); - -// -template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); -template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); -template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); - -// -template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); -template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); -template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); - -// -template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold); -template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold); -template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold); - -// -template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, - const double max_yaw, const double overlap_threshold); -template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, - const double max_dist, const double max_yaw, const double overlap_threshold); -template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, - const double max_yaw, const double overlap_threshold); - -// -template std::optional -insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -template std::optional -insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -template std::optional -insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); - -// -template std::optional -insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold); -template std::optional -insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold); -template std::optional -insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold); - -// -template std::optional -insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist, const double max_yaw, const double overlap_threshold); -template std::optional -insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist, const double max_yaw, const double overlap_threshold); -template std::optional -insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist, const double max_yaw, const double overlap_threshold); - -// -template std::optional -insertStopPoint>( - const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, - std::vector & points_with_twist, - const double overlap_threshold); - -// -template std::optional -insertDecelPoint>( - const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, - const double velocity, - std::vector & points_with_twist); - -// -template void insertOrientation>( - std::vector & points, const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, - const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, - const bool is_driving_forward); - -// -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); - -// -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); - -// -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); - -// -template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); -template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); -template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); - -// -template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); -template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); -template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); - -// -template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); - -// -template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); - -// -template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); - -// -template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); - -// -template double calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double -calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception); - -// -template bool isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold); -template bool -isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold); -template bool isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold); - -} // namespace motion_utils diff --git a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp b/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp index c6dd0104b1e11..6d07d161bf069 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp @@ -15,15 +15,15 @@ #ifndef OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ #define OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ -#include -#include +#include +#include namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; DetectedObject toDetectedObject(const TrackedObject & tracked_object); DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects); diff --git a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp b/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp index ae3013c728032..28cc9a786ecac 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp @@ -15,9 +15,9 @@ #ifndef OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ #define OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ -#include -#include -#include +#include +#include +#include #include namespace object_recognition_utils @@ -36,22 +36,19 @@ inline geometry_msgs::msg::Pose getPose(const geometry_msgs::msg::Pose & p) } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::DetectedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::DetectedObject & obj) { return obj.kinematics.pose_with_covariance.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::TrackedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::TrackedObject & obj) { return obj.kinematics.pose_with_covariance.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::PredictedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::PredictedObject & obj) { return obj.kinematics.initial_pose_with_covariance.pose; } 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/object_classification.hpp b/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp index 9c0745bb28af9..4ffafc22339d6 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp @@ -15,14 +15,14 @@ #ifndef OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ #define OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" #include #include namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::ObjectClassification; inline ObjectClassification getHighestProbClassification( const std::vector & object_classifications) 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 73cdce6c6444e..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,9 +15,9 @@ #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 #include #include @@ -34,7 +34,7 @@ namespace object_recognition_utils * @return interpolated pose */ boost::optional calcInterpolatedPose( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time); + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time); /** * @brief Resampling predicted path by time step vector. Note this function does not substitute @@ -44,8 +44,8 @@ boost::optional calcInterpolatedPose( * time_step*(num_of_path_points)] * @return resampled path */ -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const std::vector & resampled_time, const bool use_spline_for_xy = true, const bool use_spline_for_z = false); @@ -57,10 +57,10 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( * @param sampling_horizon sampling time horizon * @return resampled path */ -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, - const double sampling_time_interval, const double sampling_horizon, - const bool use_spline_for_xy = true, const bool use_spline_for_z = false); +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy = true, + const bool use_spline_for_z = false); } // namespace object_recognition_utils #endif // OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 796e276c376d6..e7eaea96907f2 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -13,7 +13,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs + 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/conversion.cpp b/common/object_recognition_utils/src/conversion.cpp index f6a25cee2c7d5..c8b8e6585f5a7 100644 --- a/common/object_recognition_utils/src/conversion.cpp +++ b/common/object_recognition_utils/src/conversion.cpp @@ -16,10 +16,10 @@ namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; DetectedObject toDetectedObject(const TrackedObject & tracked_object) { @@ -43,7 +43,7 @@ DetectedObject toDetectedObject(const TrackedObject & tracked_object) DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects) { - autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; + autoware_perception_msgs::msg::DetectedObjects detected_objects; detected_objects.header = tracked_objects.header; for (auto & tracked_object : tracked_objects.objects) { diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index 81a22c98d88e5..e26c9e6a7e1ea 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -23,7 +23,7 @@ namespace object_recognition_utils { boost::optional calcInterpolatedPose( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time) + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time) { // Check if relative time is in the valid range if (path.path.empty() || relative_time < 0.0) { @@ -38,15 +38,15 @@ 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); } } return boost::none; } -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const std::vector & resampled_time, const bool use_spline_for_xy, const bool use_spline_for_z) { @@ -83,14 +83,14 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( const auto interpolated_z = use_spline_for_z ? spline(z) : lerp(z); const auto interpolated_quat = slerp(quat); - autoware_auto_perception_msgs::msg::PredictedPath resampled_path; + autoware_perception_msgs::msg::PredictedPath resampled_path; const auto resampled_size = std::min(resampled_path.path.max_size(), resampled_time.size()); resampled_path.confidence = path.confidence; resampled_path.path.resize(resampled_size); // 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); @@ -99,10 +99,9 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( return resampled_path; } -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, - const double sampling_time_interval, const double sampling_horizon, const bool use_spline_for_xy, - const bool use_spline_for_z) +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy, const bool use_spline_for_z) { if (path.path.empty()) { throw std::invalid_argument("Predicted Path is empty"); diff --git a/common/object_recognition_utils/test/src/test_conversion.cpp b/common/object_recognition_utils/test/src/test_conversion.cpp index 61fa0efd07d04..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 @@ -28,10 +28,10 @@ geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const return p; } -autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassification( +autoware_perception_msgs::msg::ObjectClassification createObjectClassification( const std::uint8_t label, const double probability) { - autoware_auto_perception_msgs::msg::ObjectClassification classification; + autoware_perception_msgs::msg::ObjectClassification classification; classification.label = label; classification.probability = probability; @@ -42,10 +42,10 @@ autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassificat // NOTE: covariance is not checked TEST(conversion, test_toDetectedObject) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toDetectedObject; - autoware_auto_perception_msgs::msg::TrackedObject tracked_obj; + autoware_perception_msgs::msg::TrackedObject tracked_obj; // existence probability tracked_obj.existence_probability = 1.0; // classification @@ -160,10 +160,10 @@ TEST(conversion, test_toDetectedObject) // NOTE: covariance is not checked TEST(conversion, test_toTrackedObject) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toTrackedObject; - autoware_auto_perception_msgs::msg::DetectedObject detected_obj; + autoware_perception_msgs::msg::DetectedObject detected_obj; // existence probability detected_obj.existence_probability = 1.0; // classification diff --git a/common/object_recognition_utils/test/src/test_geometry.cpp b/common/object_recognition_utils/test/src/test_geometry.cpp index fe9f61b2424c6..ab3ba8fcec7d4 100644 --- a/common/object_recognition_utils/test/src/test_geometry.cpp +++ b/common/object_recognition_utils/test/src/test_geometry.cpp @@ -50,7 +50,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::DetectedObject p; + autoware_perception_msgs::msg::DetectedObject p; p.kinematics.pose_with_covariance.pose.position.x = x_ans; p.kinematics.pose_with_covariance.pose.position.y = y_ans; p.kinematics.pose_with_covariance.pose.position.z = z_ans; @@ -70,7 +70,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::TrackedObject p; + autoware_perception_msgs::msg::TrackedObject p; p.kinematics.pose_with_covariance.pose.position.x = x_ans; p.kinematics.pose_with_covariance.pose.position.y = y_ans; p.kinematics.pose_with_covariance.pose.position.z = z_ans; @@ -90,7 +90,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::PredictedObject p; + autoware_perception_msgs::msg::PredictedObject p; p.kinematics.initial_pose_with_covariance.pose.position.x = x_ans; p.kinematics.initial_pose_with_covariance.pose.position.y = y_ans; p.kinematics.initial_pose_with_covariance.pose.position.z = z_ans; diff --git a/common/object_recognition_utils/test/src/test_matching.cpp b/common/object_recognition_utils/test/src/test_matching.cpp index 8603dd54c433c..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 #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,14 +30,14 @@ 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 TEST(matching, test_get2dIoU) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dIoU; const double quart_circle = 0.16237976320958225; @@ -45,13 +45,13 @@ TEST(matching, test_get2dIoU) { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -61,13 +61,13 @@ TEST(matching, test_get2dIoU) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -77,13 +77,13 @@ TEST(matching, test_get2dIoU) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -103,15 +103,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) const double quart_circle = 0.16237976320958225; { // non overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -119,15 +119,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) } { // partially overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -135,15 +135,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) } { // fully overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -153,20 +153,20 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) TEST(matching, test_get2dPrecision) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dPrecision; const double quart_circle = 0.16237976320958225; { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -180,13 +180,13 @@ TEST(matching, test_get2dPrecision) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -200,13 +200,13 @@ TEST(matching, test_get2dPrecision) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -220,20 +220,20 @@ TEST(matching, test_get2dPrecision) TEST(matching, test_get2dRecall) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dRecall; const double quart_circle = 0.16237976320958225; { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); @@ -247,13 +247,13 @@ TEST(matching, test_get2dRecall) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); @@ -267,13 +267,13 @@ TEST(matching, test_get2dRecall) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); diff --git a/common/object_recognition_utils/test/src/test_object_classification.cpp b/common/object_recognition_utils/test/src/test_object_classification.cpp index 1637dc16346cd..697a7e8447732 100644 --- a/common/object_recognition_utils/test/src/test_object_classification.cpp +++ b/common/object_recognition_utils/test/src/test_object_classification.cpp @@ -20,10 +20,10 @@ constexpr double epsilon = 1e-06; namespace { -autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassification( +autoware_perception_msgs::msg::ObjectClassification createObjectClassification( const std::uint8_t label, const double probability) { - autoware_auto_perception_msgs::msg::ObjectClassification classification; + autoware_perception_msgs::msg::ObjectClassification classification; classification.label = label; classification.probability = probability; @@ -34,17 +34,17 @@ autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassificat TEST(object_classification, test_getHighestProbLabel) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::getHighestProbLabel; { // empty - std::vector classifications; + std::vector classifications; std::uint8_t label = getHighestProbLabel(classifications); EXPECT_EQ(label, ObjectClassification::UNKNOWN); } { // normal case - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -54,7 +54,7 @@ TEST(object_classification, test_getHighestProbLabel) } { // labels with the same probability - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -67,7 +67,7 @@ TEST(object_classification, test_getHighestProbLabel) // Test isVehicle TEST(object_classification, test_isVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isVehicle; { // True Case with uint8_t @@ -87,7 +87,7 @@ TEST(object_classification, test_isVehicle) // True Case with object_classifications { // normal case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classification.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -96,7 +96,7 @@ TEST(object_classification, test_isVehicle) // False Case with object_classifications { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::PEDESTRIAN, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.7)); EXPECT_FALSE(isVehicle(classification)); @@ -106,7 +106,7 @@ TEST(object_classification, test_isVehicle) // TEST isCarLikeVehicle TEST(object_classification, test_isCarLikeVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isCarLikeVehicle; { // True Case with uint8_t @@ -126,7 +126,7 @@ TEST(object_classification, test_isCarLikeVehicle) // True Case with object_classifications { // normal case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classification.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.7)); @@ -135,7 +135,7 @@ TEST(object_classification, test_isCarLikeVehicle) // False Case with object_classifications { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.8)); EXPECT_FALSE(isCarLikeVehicle(classification)); @@ -146,7 +146,7 @@ TEST(object_classification, test_isCarLikeVehicle) // getHighestProbLabel() returns only first highest-scored label. // so, in edge case it returns a label earlier added. { // When car and non-car label has same probability - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); EXPECT_FALSE( @@ -157,7 +157,7 @@ TEST(object_classification, test_isCarLikeVehicle) // TEST isLargeVehicle TEST(object_classification, test_isLargeVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isLargeVehicle; { // True Case with uint8_t @@ -176,7 +176,7 @@ TEST(object_classification, test_isLargeVehicle) } { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); @@ -188,7 +188,7 @@ TEST(object_classification, test_isLargeVehicle) // getHighestProbLabel() returns only first highest-scored label. // so, in edge case it returns a label earlier added. { // When large-vehicle and non-large-vehicle label has same probability - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); EXPECT_TRUE(isLargeVehicle(classification)); // evaluated with earlier appended "BUS" label @@ -197,18 +197,18 @@ TEST(object_classification, test_isLargeVehicle) TEST(object_classification, test_getHighestProbClassification) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::getHighestProbClassification; { // empty - std::vector classifications; + std::vector classifications; auto classification = getHighestProbClassification(classifications); EXPECT_EQ(classification.label, ObjectClassification::UNKNOWN); EXPECT_DOUBLE_EQ(classification.probability, 0.0); } { // normal case - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -219,7 +219,7 @@ TEST(object_classification, test_getHighestProbClassification) } { // labels with the same probability - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -232,7 +232,7 @@ TEST(object_classification, test_getHighestProbClassification) TEST(object_classification, test_fromString) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toLabel; using object_recognition_utils::toObjectClassification; using object_recognition_utils::toObjectClassifications; @@ -266,7 +266,7 @@ TEST(object_classification, test_fromString) TEST(object_classification, test_convertLabelToString) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::convertLabelToString; // from label @@ -290,7 +290,7 @@ TEST(object_classification, test_convertLabelToString) // from ObjectClassifications { - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); 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 b466273f4defd..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_auto_perception_msgs::msg::PredictedPath; -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; +using autoware_perception_msgs::msg::PredictedPath; 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); @@ -174,7 +174,7 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) } } - // Resample which exceeds the maximum size + // Resample the path with more than 100 points { std::vector resampling_vec(101); for (size_t i = 0; i < 101; ++i) { @@ -183,10 +183,9 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) const auto resampled_path = resamplePredictedPath(path, resampling_vec); - EXPECT_EQ(resampled_path.path.size(), resampled_path.path.max_size()); EXPECT_NEAR(path.confidence, resampled_path.confidence, epsilon); - for (size_t i = 0; i < resampled_path.path.max_size(); ++i) { + for (size_t i = 0; i < resampled_path.path.size(); ++i) { EXPECT_NEAR(resampled_path.path.at(i).position.x, resampling_vec.at(i), epsilon); EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); @@ -206,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/Readme.md b/common/path_distance_calculator/Readme.md index acbdc6998c46f..fed0476047ba9 100644 --- a/common/path_distance_calculator/Readme.md +++ b/common/path_distance_calculator/Readme.md @@ -11,10 +11,10 @@ Note that the distance means the arc-length along the path, not the Euclidean di ### Input -| Name | Type | Description | -| ----------------------------------------------------------------- | ---------------------------------------- | -------------- | -| `/planning/scenario_planning/lane_driving/behavior_planning/path` | `autoware_auto_planning_msgs::msg::Path` | Reference path | -| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | +| Name | Type | Description | +| ----------------------------------------------------------------- | ----------------------------------- | -------------- | +| `/planning/scenario_planning/lane_driving/behavior_planning/path` | `autoware_planning_msgs::msg::Path` | Reference path | +| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | ### Output diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index e796bbd0d20f7..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_auto_planning_msgs - motion_utils + autoware_motion_utils + autoware_planning_msgs + 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 aa20dd1ffc7ce..47dd8258de5bb 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,9 +25,9 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & options) : Node("path_distance_calculator", options), self_pose_listener_(this) { - sub_path_ = create_subscription( + sub_path_ = create_subscription( "~/input/path", rclcpp::QoS(1), - [this](const autoware_auto_planning_msgs::msg::Path::SharedPtr msg) { path_ = msg; }); + [this](const autoware_planning_msgs::msg::Path::SharedPtr msg) { path_ = msg; }); pub_dist_ = create_publisher("~/output/distance", rclcpp::QoS(1)); @@ -48,8 +48,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 f709701024f51..8e8fbf8d6a019 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.hpp +++ b/common/path_distance_calculator/src/path_distance_calculator.hpp @@ -15,10 +15,10 @@ #ifndef PATH_DISTANCE_CALCULATOR_HPP_ #define PATH_DISTANCE_CALCULATOR_HPP_ +#include #include -#include -#include +#include #include class PathDistanceCalculator : public rclcpp::Node @@ -27,11 +27,11 @@ class PathDistanceCalculator : public rclcpp::Node explicit PathDistanceCalculator(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_path_; + rclcpp::Subscription::SharedPtr sub_path_; rclcpp::Publisher::SharedPtr pub_dist_; rclcpp::TimerBase::SharedPtr timer_; - tier4_autoware_utils::SelfPoseListener self_pose_listener_; - autoware_auto_planning_msgs::msg::Path::SharedPtr path_; + autoware::universe_utils::SelfPoseListener self_pose_listener_; + autoware_planning_msgs::msg::Path::SharedPtr path_; }; #endif // PATH_DISTANCE_CALCULATOR_HPP_ diff --git a/common/rtc_manager_rviz_plugin/CMakeLists.txt b/common/rtc_manager_rviz_plugin/CMakeLists.txt deleted file mode 100644 index f2fad9e24665f..0000000000000 --- a/common/rtc_manager_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(rtc_manager_rviz_plugin) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) -set(CMAKE_AUTOMOC ON) -set(CMAKE_INCLUDE_CURRENT_DIR ON) -add_definitions(-DQT_NO_KEYWORDS) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/rtc_manager_panel.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${QT_LIBRARIES} -) - -# Export the plugin to be imported by rviz2 -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - icons - plugins -) diff --git a/common/rtc_manager_rviz_plugin/README.md b/common/rtc_manager_rviz_plugin/README.md deleted file mode 100644 index 7a15d56fe235a..0000000000000 --- a/common/rtc_manager_rviz_plugin/README.md +++ /dev/null @@ -1,36 +0,0 @@ -# rtc_manager_rviz_plugin - -## Purpose - -The purpose of this Rviz plugin is - -1. To display each content of RTC status. - -2. To switch each module of RTC auto mode. - -3. To change RTC cooperate commands by button. - -![rtc_manager_panel](./images/rtc_manager_panel.png) - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ------------------------------ | ------------------------------------------- | --------------------------------------- | -| `/api/external/get/rtc_status` | `tier4_rtc_msgs::msg::CooperateStatusArray` | The statuses of each Cooperate Commands | - -### Output - -| Name | Type | Description | -| -------------------------------- | ---------------------------------------- | ---------------------------------------------------- | -| `/api/external/set/rtc_commands` | `tier4_rtc_msgs::src::CooperateCommands` | The Cooperate Commands for each planning | -| `/planning/enable_auto_mode/*` | `tier4_rtc_msgs::src::AutoMode` | The Cooperate Commands mode for each planning module | - -## HowToUse - -1. Start rviz and select panels/Add new panel. - ![select_panel](./images/select_panels.png) - -2. tier4_state_rviz_plugin/RTCManagerPanel and press OK. - ![select_rtc_panel](./images/rtc_selection.png) diff --git a/common/rtc_manager_rviz_plugin/images/rtc_manager_panel.png b/common/rtc_manager_rviz_plugin/images/rtc_manager_panel.png deleted file mode 100644 index 36c1b4760308b..0000000000000 Binary files a/common/rtc_manager_rviz_plugin/images/rtc_manager_panel.png and /dev/null differ diff --git a/common/rtc_manager_rviz_plugin/images/rtc_selection.png b/common/rtc_manager_rviz_plugin/images/rtc_selection.png deleted file mode 100644 index f9c5d120bdd70..0000000000000 Binary files a/common/rtc_manager_rviz_plugin/images/rtc_selection.png and /dev/null differ diff --git a/common/rtc_manager_rviz_plugin/images/select_panels.png b/common/rtc_manager_rviz_plugin/images/select_panels.png deleted file mode 100644 index a691602c42c3c..0000000000000 Binary files a/common/rtc_manager_rviz_plugin/images/select_panels.png and /dev/null differ diff --git a/common/rtc_manager_rviz_plugin/package.xml b/common/rtc_manager_rviz_plugin/package.xml deleted file mode 100644 index 53f00386bdb4d..0000000000000 --- a/common/rtc_manager_rviz_plugin/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - rtc_manager_rviz_plugin - 0.0.0 - The rtc manager rviz plugin package - Taiki Tanaka - Tomoya Kimura - - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - libqt5-core - libqt5-gui - libqt5-widgets - qtbase5-dev - rclcpp - rviz_common - tier4_external_api_msgs - tier4_planning_msgs - tier4_rtc_msgs - unique_identifier_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml b/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml deleted file mode 100644 index 001ae153e6762..0000000000000 --- a/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - RTCManagerPanel - - - diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp deleted file mode 100644 index 058a5d5deb5d6..0000000000000 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ /dev/null @@ -1,474 +0,0 @@ -// -// 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 "rtc_manager_panel.hpp" - -#include -#include -#include -#include - -#include - -namespace rviz_plugins -{ -inline std::string Bool2String(const bool var) -{ - return var ? "True" : "False"; -} -inline bool uint2bool(uint8_t var) -{ - return var == static_cast(0) ? false : true; -} -using std::placeholders::_1; -using std::placeholders::_2; - -std::string getModuleName(const uint8_t module_type) -{ - switch (module_type) { - case Module::LANE_CHANGE_LEFT: { - return "lane_change_left"; - } - case Module::LANE_CHANGE_RIGHT: { - return "lane_change_right"; - } - case Module::EXT_REQUEST_LANE_CHANGE_LEFT: { - return "external_request_lane_change_left"; - } - case Module::EXT_REQUEST_LANE_CHANGE_RIGHT: { - return "external_request_lane_change_right"; - } - case Module::AVOIDANCE_BY_LC_LEFT: { - return "avoidance_by_lane_change_left"; - } - case Module::AVOIDANCE_BY_LC_RIGHT: { - return "avoidance_by_lane_change_right"; - } - case Module::AVOIDANCE_LEFT: { - return "avoidance_left"; - } - case Module::AVOIDANCE_RIGHT: { - return "avoidance_right"; - } - case Module::GOAL_PLANNER: { - return "goal_planner"; - } - case Module::START_PLANNER: { - return "start_planner"; - } - case Module::TRAFFIC_LIGHT: { - return "traffic_light"; - } - case Module::INTERSECTION: { - return "intersection"; - } - case Module::CROSSWALK: { - return "crosswalk"; - } - case Module::BLIND_SPOT: { - return "blind_spot"; - } - case Module::DETECTION_AREA: { - return "detection_area"; - } - case Module::NO_STOPPING_AREA: { - return "no_stopping_area"; - } - case Module::OCCLUSION_SPOT: { - return "occlusion_spot"; - } - case Module::INTERSECTION_OCCLUSION: { - return "intersection_occlusion"; - } - } - return "NONE"; -} - -bool isPathChangeModule(const uint8_t module_type) -{ - if ( - module_type == Module::LANE_CHANGE_LEFT || module_type == Module::LANE_CHANGE_RIGHT || - module_type == Module::EXT_REQUEST_LANE_CHANGE_LEFT || - module_type == Module::EXT_REQUEST_LANE_CHANGE_RIGHT || - module_type == Module::AVOIDANCE_BY_LC_LEFT || module_type == Module::AVOIDANCE_BY_LC_RIGHT || - module_type == Module::AVOIDANCE_LEFT || module_type == Module::AVOIDANCE_RIGHT || - module_type == Module::GOAL_PLANNER || module_type == Module::START_PLANNER) { - return true; - } - return false; -} - -RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent) -{ - // TODO(tanaka): replace this magic number to Module::SIZE - const size_t module_size = 19; - auto_modes_.reserve(module_size); - auto * v_layout = new QVBoxLayout; - auto vertical_header = new QHeaderView(Qt::Vertical); - vertical_header->hide(); - auto horizontal_header = new QHeaderView(Qt::Horizontal); - horizontal_header->setSectionResizeMode(QHeaderView::Stretch); - auto_mode_table_ = new QTableWidget(); - auto_mode_table_->setColumnCount(4); - auto_mode_table_->setHorizontalHeaderLabels( - {"Module", "ToAutoMode", "ToManualMode", "ApprovalMode"}); - auto_mode_table_->setVerticalHeader(vertical_header); - auto_mode_table_->setHorizontalHeader(horizontal_header); - const size_t num_modules = module_size; - auto_mode_table_->setRowCount(num_modules); - for (size_t i = 0; i < num_modules; i++) { - auto * rtc_auto_mode = new RTCAutoMode(); - rtc_auto_mode->setParent(this); - // module - { - const uint8_t module_type = static_cast(i); - rtc_auto_mode->module_name = getModuleName(module_type); - std::string module_name = rtc_auto_mode->module_name; - auto label = new QLabel(QString::fromStdString(module_name)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(module_name)); - if (isPathChangeModule(module_type)) - label->setStyleSheet(BG_PURPLE); - else - label->setStyleSheet(BG_ORANGE); - auto_mode_table_->setCellWidget(i, 0, label); - } - // mode button - { - // auto mode button - rtc_auto_mode->auto_module_button_ptr = new QPushButton("auto mode"); - rtc_auto_mode->auto_module_button_ptr->setCheckable(true); - connect( - rtc_auto_mode->auto_module_button_ptr, &QPushButton::clicked, rtc_auto_mode, - &RTCAutoMode::onChangeToAutoMode); - auto_mode_table_->setCellWidget(i, 1, rtc_auto_mode->auto_module_button_ptr); - // manual mode button - rtc_auto_mode->manual_module_button_ptr = new QPushButton("manual mode"); - rtc_auto_mode->manual_module_button_ptr->setCheckable(true); - connect( - rtc_auto_mode->manual_module_button_ptr, &QPushButton::clicked, rtc_auto_mode, - &RTCAutoMode::onChangeToManualMode); - auto_mode_table_->setCellWidget(i, 2, rtc_auto_mode->manual_module_button_ptr); - } - // current mode - { - QString mode = QString::fromStdString("INIT"); - rtc_auto_mode->auto_manual_mode_label = new QLabel(mode); - rtc_auto_mode->auto_manual_mode_label->setAlignment(Qt::AlignCenter); - rtc_auto_mode->auto_manual_mode_label->setText(mode); - auto_mode_table_->setCellWidget(i, 3, rtc_auto_mode->auto_manual_mode_label); - } - auto_modes_.emplace_back(rtc_auto_mode); - } - v_layout->addWidget(auto_mode_table_); - - num_rtc_status_ptr_ = new QLabel("Init"); - v_layout->addWidget(num_rtc_status_ptr_); - - // lateral execution - auto * exe_path_change_layout = new QHBoxLayout; - { - exec_path_change_button_ptr_ = new QPushButton("Execute Path Change"); - exec_path_change_button_ptr_->setCheckable(false); - exec_path_change_button_ptr_->setStyleSheet(BG_PURPLE); - connect( - exec_path_change_button_ptr_, &QPushButton::clicked, this, - &RTCManagerPanel::onClickExecutePathChange); - exe_path_change_layout->addWidget(exec_path_change_button_ptr_); - wait_path_change_button_ptr_ = new QPushButton("Wait Path Change"); - wait_path_change_button_ptr_->setCheckable(false); - wait_path_change_button_ptr_->setStyleSheet(BG_PURPLE); - connect( - wait_path_change_button_ptr_, &QPushButton::clicked, this, - &RTCManagerPanel::onClickWaitPathChange); - exe_path_change_layout->addWidget(wait_path_change_button_ptr_); - } - v_layout->addLayout(exe_path_change_layout); - - // longitudinal execution - auto * exe_vel_change_layout = new QHBoxLayout; - { - exec_vel_change_button_ptr_ = new QPushButton("Execute Velocity Change"); - exec_vel_change_button_ptr_->setCheckable(false); - exec_vel_change_button_ptr_->setStyleSheet(BG_ORANGE); - connect( - exec_vel_change_button_ptr_, &QPushButton::clicked, this, - &RTCManagerPanel::onClickExecuteVelChange); - exe_vel_change_layout->addWidget(exec_vel_change_button_ptr_); - wait_vel_change_button_ptr_ = new QPushButton("Wait Velocity Change"); - wait_vel_change_button_ptr_->setCheckable(false); - wait_vel_change_button_ptr_->setStyleSheet(BG_ORANGE); - connect( - wait_vel_change_button_ptr_, &QPushButton::clicked, this, - &RTCManagerPanel::onClickWaitVelChange); - exe_vel_change_layout->addWidget(wait_vel_change_button_ptr_); - } - v_layout->addLayout(exe_vel_change_layout); - - // execution - auto * rtc_exe_layout = new QHBoxLayout; - { - exec_button_ptr_ = new QPushButton("Execute All"); - exec_button_ptr_->setCheckable(false); - connect(exec_button_ptr_, &QPushButton::clicked, this, &RTCManagerPanel::onClickExecution); - rtc_exe_layout->addWidget(exec_button_ptr_); - wait_button_ptr_ = new QPushButton("Wait All"); - wait_button_ptr_->setCheckable(false); - connect(wait_button_ptr_, &QPushButton::clicked, this, &RTCManagerPanel::onClickWait); - rtc_exe_layout->addWidget(wait_button_ptr_); - } - v_layout->addLayout(rtc_exe_layout); - - // statuses - auto * rtc_table_layout = new QHBoxLayout; - { - auto vertical_header = new QHeaderView(Qt::Vertical); - vertical_header->hide(); - auto horizontal_header = new QHeaderView(Qt::Horizontal); - horizontal_header->setSectionResizeMode(QHeaderView::Stretch); - rtc_table_ = new QTableWidget(); - rtc_table_->setColumnCount(column_size_); - rtc_table_->setHorizontalHeaderLabels( - {"ID", "Module", "AW Safe", "Received Cmd", "AutoMode", "StartDistance", "FinishDistance"}); - rtc_table_->setVerticalHeader(vertical_header); - rtc_table_->setHorizontalHeader(horizontal_header); - rtc_table_layout->addWidget(rtc_table_); - v_layout->addLayout(rtc_table_layout); - } - setLayout(v_layout); -} - -void RTCManagerPanel::onInitialize() -{ - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - client_rtc_commands_ = - raw_node_->create_client("/api/external/set/rtc_commands"); - - for (size_t i = 0; i < auto_modes_.size(); i++) { - auto & a = auto_modes_.at(i); - // auto mode - a->enable_auto_mode_cli = - raw_node_->create_client(enable_auto_mode_namespace_ + "/" + a->module_name); - } - - sub_rtc_status_ = raw_node_->create_subscription( - "/api/external/get/rtc_status", 1, std::bind(&RTCManagerPanel::onRTCStatus, this, _1)); -} - -void RTCAutoMode::onChangeToAutoMode() -{ - AutoMode::Request::SharedPtr request = std::make_shared(); - request->enable = true; - enable_auto_mode_cli->async_send_request(request); - auto_manual_mode_label->setText("AutoMode"); - auto_manual_mode_label->setStyleSheet(BG_BLUE); - auto_module_button_ptr->setChecked(true); - manual_module_button_ptr->setChecked(false); -} - -void RTCAutoMode::onChangeToManualMode() -{ - AutoMode::Request::SharedPtr request = std::make_shared(); - request->enable = false; - enable_auto_mode_cli->async_send_request(request); - auto_manual_mode_label->setText("ManualMode"); - auto_manual_mode_label->setStyleSheet(BG_YELLOW); - manual_module_button_ptr->setChecked(true); - auto_module_button_ptr->setChecked(false); -} - -CooperateCommand setRTCCommandFromStatus(CooperateStatus & status) -{ - CooperateCommand cooperate_command; - cooperate_command.uuid = status.uuid; - cooperate_command.module = status.module; - cooperate_command.command = status.command_status; - return cooperate_command; -} - -void RTCManagerPanel::onClickChangeRequest(const bool is_path_change, const uint8_t command) -{ - if (!cooperate_statuses_ptr_) return; - if (cooperate_statuses_ptr_->statuses.empty()) return; - auto executable_cooperate_commands_request = std::make_shared(); - executable_cooperate_commands_request->stamp = cooperate_statuses_ptr_->stamp; - // send coop request - for (auto status : cooperate_statuses_ptr_->statuses) { - if (is_path_change ^ isPathChangeModule(status.module.type)) continue; - CooperateCommand cooperate_command = setRTCCommandFromStatus(status); - cooperate_command.command.type = command; - executable_cooperate_commands_request->commands.emplace_back(cooperate_command); - // To consider needs to change path step by step - if (is_path_change && !status.auto_mode && status.command_status.type ^ command) { - break; - } - } - client_rtc_commands_->async_send_request(executable_cooperate_commands_request); -} - -void RTCManagerPanel::onClickCommandRequest(const uint8_t command) -{ - if (!cooperate_statuses_ptr_) return; - if (cooperate_statuses_ptr_->statuses.empty()) return; - auto executable_cooperate_commands_request = std::make_shared(); - executable_cooperate_commands_request->stamp = cooperate_statuses_ptr_->stamp; - // send coop request - for (auto status : cooperate_statuses_ptr_->statuses) { - CooperateCommand cooperate_command = setRTCCommandFromStatus(status); - cooperate_command.command.type = command; - executable_cooperate_commands_request->commands.emplace_back(cooperate_command); - } - client_rtc_commands_->async_send_request(executable_cooperate_commands_request); -} - -void RTCManagerPanel::onClickExecuteVelChange() -{ - onClickChangeRequest(false, Command::ACTIVATE); -} -void RTCManagerPanel::onClickWaitVelChange() -{ - onClickChangeRequest(false, Command::DEACTIVATE); -} -void RTCManagerPanel::onClickExecutePathChange() -{ - onClickChangeRequest(true, Command::ACTIVATE); -} -void RTCManagerPanel::onClickWaitPathChange() -{ - onClickChangeRequest(true, Command::DEACTIVATE); -} -void RTCManagerPanel::onClickExecution() -{ - onClickCommandRequest(Command::ACTIVATE); -} -void RTCManagerPanel::onClickWait() -{ - onClickCommandRequest(Command::DEACTIVATE); -} - -void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg) -{ - cooperate_statuses_ptr_ = std::make_shared(*msg); - rtc_table_->clearContents(); - num_rtc_status_ptr_->setText( - QString::fromStdString("The Number of RTC Statuses: " + std::to_string(msg->statuses.size()))); - if (msg->statuses.empty()) { - rtc_table_->update(); - return; - } - // this is to stable rtc display not to occupy too much - size_t min_display_size{5}; - size_t max_display_size{10}; - // rtc messages are already sorted by distance - rtc_table_->setRowCount( - std::max(min_display_size, std::min(msg->statuses.size(), max_display_size))); - int cnt = 0; - - auto sorted_statuses = msg->statuses; - std::partition(sorted_statuses.begin(), sorted_statuses.end(), [](const auto & status) { - return !status.auto_mode && !uint2bool(status.command_status.type); - }); - - for (auto status : sorted_statuses) { - if (static_cast(cnt) >= max_display_size) { - rtc_table_->update(); - return; - } - // uuid - { - std::stringstream uuid; - uuid << std::setw(4) << std::setfill('0') << static_cast(status.uuid.uuid.at(0)); - auto label = new QLabel(QString::fromStdString(uuid.str())); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(uuid.str())); - rtc_table_->setCellWidget(cnt, 0, label); - } - - // module name - { - std::string module_name = getModuleName(status.module.type); - auto label = new QLabel(QString::fromStdString(module_name)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(module_name)); - rtc_table_->setCellWidget(cnt, 1, label); - } - - // is aw safe - bool is_aw_safe = status.safe; - { - std::string is_safe = Bool2String(is_aw_safe); - auto label = new QLabel(QString::fromStdString(is_safe)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(is_safe)); - rtc_table_->setCellWidget(cnt, 2, label); - } - - // is operator safe - const bool is_execute = uint2bool(status.command_status.type); - { - std::string text = is_execute ? "EXECUTE" : "WAIT"; - if (status.auto_mode) text = "NONE"; - auto label = new QLabel(QString::fromStdString(text)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(text)); - rtc_table_->setCellWidget(cnt, 3, label); - } - - // is auto mode - const bool is_rtc_auto_mode = status.auto_mode; - { - std::string is_auto_mode = Bool2String(is_rtc_auto_mode); - auto label = new QLabel(QString::fromStdString(is_auto_mode)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(is_auto_mode)); - rtc_table_->setCellWidget(cnt, 4, label); - } - - // start distance - { - std::string start_distance = std::to_string(status.start_distance); - auto label = new QLabel(QString::fromStdString(start_distance)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(start_distance)); - rtc_table_->setCellWidget(cnt, 5, label); - } - - // finish distance - { - std::string finish_distance = std::to_string(status.finish_distance); - auto label = new QLabel(QString::fromStdString(finish_distance)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(finish_distance)); - rtc_table_->setCellWidget(cnt, 6, label); - } - - // add color for recognition - if (is_rtc_auto_mode || (is_aw_safe && is_execute)) { - rtc_table_->cellWidget(cnt, 1)->setStyleSheet(BG_GREEN); - } else if (is_aw_safe || is_execute) { - rtc_table_->cellWidget(cnt, 1)->setStyleSheet(BG_YELLOW); - } else { - rtc_table_->cellWidget(cnt, 1)->setStyleSheet(BG_RED); - } - cnt++; - } - rtc_table_->update(); -} -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::RTCManagerPanel, rviz_common::Panel) diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp deleted file mode 100644 index 8bdaef94b6254..0000000000000 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp +++ /dev/null @@ -1,132 +0,0 @@ -// -// 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 RTC_MANAGER_PANEL_HPP_ -#define RTC_MANAGER_PANEL_HPP_ - -#include -#include -#include -#include -#include - -#ifndef Q_MOC_RUN -// cpp -#include -#include -#include -#include -// ros -#include -#include - -#include -// autoware -#include -#include -#include -#include -#include -#include -#include -#include -#endif - -namespace rviz_plugins -{ -using tier4_rtc_msgs::msg::Command; -using tier4_rtc_msgs::msg::CooperateCommand; -using tier4_rtc_msgs::msg::CooperateResponse; -using tier4_rtc_msgs::msg::CooperateStatus; -using tier4_rtc_msgs::msg::CooperateStatusArray; -using tier4_rtc_msgs::msg::Module; -using tier4_rtc_msgs::srv::AutoMode; -using tier4_rtc_msgs::srv::CooperateCommands; -using unique_identifier_msgs::msg::UUID; - -static const QString BG_BLUE = "background-color: #3dffff;"; -static const QString BG_YELLOW = "background-color: #ffff3d;"; -static const QString BG_PURPLE = "background-color: #9e3dff;"; -static const QString BG_ORANGE = "background-color: #ff7f00;"; -static const QString BG_GREEN = "background-color: #3dff3d;"; -static const QString BG_RED = "background-color: #ff3d3d;"; - -struct RTCAutoMode : public QObject -{ - Q_OBJECT - -public Q_SLOTS: - void onChangeToAutoMode(); - void onChangeToManualMode(); - -public: - std::string module_name; - QPushButton * auto_module_button_ptr; - QPushButton * manual_module_button_ptr; - QLabel * auto_manual_mode_label; - rclcpp::Client::SharedPtr enable_auto_mode_cli; -}; - -class RTCManagerPanel : public rviz_common::Panel -{ - Q_OBJECT -public Q_SLOTS: - void onClickExecution(); - void onClickWait(); - void onClickVelocityChangeRequest(); - void onClickExecutePathChange(); - void onClickWaitPathChange(); - void onClickExecuteVelChange(); - void onClickWaitVelChange(); - -public: - explicit RTCManagerPanel(QWidget * parent = nullptr); - void onInitialize() override; - -protected: - void onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg); - void onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) const; - void onClickCommandRequest(const uint8_t command); - void onClickChangeRequest(const bool is_path_change, const uint8_t command); - -private: - rclcpp::Node::SharedPtr raw_node_; - rclcpp::Subscription::SharedPtr sub_rtc_status_; - rclcpp::Client::SharedPtr client_rtc_commands_; - rclcpp::Client::SharedPtr enable_auto_mode_cli_; - std::vector auto_modes_; - - std::shared_ptr cooperate_statuses_ptr_; - QTableWidget * rtc_table_; - QTableWidget * auto_mode_table_; - QPushButton * path_change_button_ptr_ = {nullptr}; - QPushButton * velocity_change_button_ptr_ = {nullptr}; - QPushButton * exec_path_change_button_ptr_ = {nullptr}; - QPushButton * wait_path_change_button_ptr_ = {nullptr}; - QPushButton * exec_vel_change_button_ptr_ = {nullptr}; - QPushButton * wait_vel_change_button_ptr_ = {nullptr}; - QPushButton * exec_button_ptr_ = {nullptr}; - QPushButton * wait_button_ptr_ = {nullptr}; - QLabel * num_rtc_status_ptr_ = {nullptr}; - - size_t column_size_ = {7}; - std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; -}; - -} // namespace rviz_plugins - -#endif // RTC_MANAGER_PANEL_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/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png b/common/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png similarity index 100% rename from common/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png rename to common/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png 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_automatic_goal_rviz_plugin/CMakeLists.txt b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt deleted file mode 100644 index cdc57743a6cb1..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tier4_automatic_goal_rviz_plugin) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) -set(CMAKE_AUTOMOC ON) -set(CMAKE_INCLUDE_CURRENT_DIR ON) -add_definitions(-DQT_NO_KEYWORDS) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/automatic_checkpoint_append_tool.cpp - src/automatic_goal_sender.cpp - src/automatic_goal_append_tool.cpp - src/automatic_goal_panel.cpp -) - -ament_auto_add_executable(automatic_goal_sender - src/automatic_goal_sender.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${QT_LIBRARIES} -) - -# Export the plugin to be imported by rviz2 -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - launch - icons - plugins -) diff --git a/common/tier4_automatic_goal_rviz_plugin/README.md b/common/tier4_automatic_goal_rviz_plugin/README.md deleted file mode 100644 index 6fd626d5a7642..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/README.md +++ /dev/null @@ -1,98 +0,0 @@ -# tier4_automatic_goal_rviz_plugin - -## Purpose - -1. Defining a `GoalsList` by adding goals using `RvizTool` (Pose on the map). - -2. Automatic execution of the created `GoalsList` from the selected goal - it can be stopped and restarted. - -3. Looping the current `GoalsList`. - -4. Saving achieved goals to a file. - -5. Plan the route to one (single) selected goal and starting that route - it can be stopped and restarted. - -6. Remove any goal from the list or clear the current route. - -7. Save the current `GoalsList` to a file and load the list from the file. - -8. The application enables/disables access to options depending on the current state. - -9. The saved `GoalsList` can be executed without using a plugin - using a node `automatic_goal_sender`. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ---------------------------- | ------------------------------------------------- | ------------------------------------------------ | -| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | The topic represents the state of operation mode | -| `/api/routing/state` | `autoware_adapi_v1_msgs::msg::RouteState` | The topic represents the state of route | -| `/rviz2/automatic_goal/goal` | `geometry_msgs::msgs::PoseStamped` | The topic for adding goals to GoalsList | - -### Output - -| Name | Type | Description | -| ------------------------------------------ | -------------------------------------------------- | -------------------------------------------------- | -| `/api/operation_mode/change_to_autonomous` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to autonomous | -| `/api/operation_mode/change_to_stop` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to stop | -| `/api/routing/set_route_points` | `autoware_adapi_v1_msgs::srv::SetRoutePoints` | The service to set route | -| `/api/routing/clear_route` | `autoware_adapi_v1_msgs::srv::ClearRoute` | The service to clear route state | -| `/rviz2/automatic_goal/markers` | `visualization_msgs::msg::MarkerArray` | The topic to visualize goals as rviz markers | - -## HowToUse - -1. Start rviz and select panels/Add new panel. - - ![select_panel](./images/select_panels.png) - -2. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalPanel` and press OK. - -3. Select Add a new tool. - - ![select_tool](./images/select_tool.png) - -4. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalTool` and press OK. - -5. Add goals visualization as markers to `Displays`. - - ![markers](./images/markers.png) - -6. Append goals to the `GoalsList` to be achieved using `2D Append Goal` - in such a way that routes can be planned. - -7. Start sequential planning and goal achievement by clicking `Send goals automatically` - - ![panel](./images/panel.png) - -8. You can save `GoalsList` by clicking `Save to file`. - -9. After saving, you can run the `GoalsList` without using a plugin also: - - example: `ros2 launch tier4_automatic_goal_rviz_plugin automatic_goal_sender.launch.xml goals_list_file_path:="/tmp/goals_list.yaml" goals_achieved_dir_path:="/tmp/"` - - `goals_list_file_path` - is the path to the saved `GoalsList` file to be loaded - - `goals_achieved_dir_path` - is the path to the directory where the file `goals_achieved.log` will be created and the achieved goals will be written to it - -### Hints - -If the application (Engagement) goes into `ERROR` mode (usually returns to `EDITING` later), it means that one of the services returned a calling error (`code!=0`). -In this situation, check the terminal output for more information. - -- Often it is enough to try again. -- Sometimes a clearing of the current route is required before retrying. - -## Material Design Icons - -This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0. - -Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products. - -### License - -The Material Design Icons are licensed under the Apache License, Version 2.0. You may obtain a copy of the License at: - - - -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. - -### Acknowledgments - -We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects. diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png deleted file mode 100644 index 4f5b4961f2500..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png and /dev/null differ diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png deleted file mode 100644 index 6a67573717ae1..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png and /dev/null differ diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png deleted file mode 100644 index 58d12f95ebfd6..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png and /dev/null differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/markers.png b/common/tier4_automatic_goal_rviz_plugin/images/markers.png deleted file mode 100644 index 8dd4d9d02bef4..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/images/markers.png and /dev/null differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/panel.png b/common/tier4_automatic_goal_rviz_plugin/images/panel.png deleted file mode 100644 index 1800202ea9f57..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/images/panel.png and /dev/null differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png b/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png deleted file mode 100644 index 61dd9e1d7a1b3..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png and /dev/null differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png b/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png deleted file mode 100644 index a6ddc6d24c575..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png and /dev/null differ diff --git a/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml b/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml deleted file mode 100644 index a9af89c078348..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/common/tier4_automatic_goal_rviz_plugin/package.xml b/common/tier4_automatic_goal_rviz_plugin/package.xml deleted file mode 100644 index fb5331379acb6..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - tier4_automatic_goal_rviz_plugin - 0.0.0 - The autoware automatic goal rviz plugin package - Shumpei Wakabayashi - Dawid Moszyński - Kyoichi Sugahara - Satoshi Ota - Apache License 2.0 - Dawid Moszyński - - ament_cmake_auto - autoware_cmake - autoware_adapi_v1_msgs - geometry_msgs - libqt5-core - libqt5-gui - libqt5-widgets - qtbase5-dev - rclcpp - rviz_common - rviz_default_plugins - tf2 - tf2_geometry_msgs - tier4_autoware_utils - visualization_msgs - yaml-cpp - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml deleted file mode 100644 index e9d77491941ed..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - AutowareAutomaticGoalPanel - - - AutowareAutomaticGoalTool - - - AutowareAutomaticCheckpointTool - - diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp deleted file mode 100644 index 4efa6fedbaabd..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp +++ /dev/null @@ -1,122 +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. -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "automatic_checkpoint_append_tool.hpp" - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif -#include - -#include - -namespace rviz_plugins -{ -AutowareAutomaticCheckpointTool::AutowareAutomaticCheckpointTool() -{ - shortcut_key_ = 'c'; - - pose_topic_property_ = new rviz_common::properties::StringProperty( - "Pose Topic", "~/automatic_goal/checkpoint", "The topic on which to publish automatic_goal.", - getPropertyContainer(), SLOT(updateTopic()), this); - std_dev_x_ = new rviz_common::properties::FloatProperty( - "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer()); - std_dev_y_ = new rviz_common::properties::FloatProperty( - "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer()); - std_dev_theta_ = new rviz_common::properties::FloatProperty( - "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]", - getPropertyContainer()); - position_z_ = new rviz_common::properties::FloatProperty( - "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer()); - std_dev_x_->setMin(0); - std_dev_y_->setMin(0); - std_dev_theta_->setMin(0); - position_z_->setMin(0); -} - -void AutowareAutomaticCheckpointTool::onInitialize() -{ - PoseTool::onInitialize(); - setName("2D AppendCheckpoint"); - updateTopic(); -} - -void AutowareAutomaticCheckpointTool::updateTopic() -{ - rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - pose_pub_ = raw_node->create_publisher( - pose_topic_property_->getStdString(), 1); - clock_ = raw_node->get_clock(); -} - -void AutowareAutomaticCheckpointTool::onPoseSet(double x, double y, double theta) -{ - // pose - std::string fixed_frame = context_->getFixedFrame().toStdString(); - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = fixed_frame; - pose.header.stamp = clock_->now(); - pose.pose.position.x = x; - pose.pose.position.y = y; - pose.pose.position.z = position_z_->getFloat(); - - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, theta); - pose.pose.orientation = tf2::toMsg(quat); - RCLCPP_INFO( - rclcpp::get_logger("AutowareAutomaticCheckpointTool"), - "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, y, position_z_->getFloat(), theta, - fixed_frame.c_str()); - pose_pub_->publish(pose); -} - -} // end namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticCheckpointTool, rviz_common::Tool) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp deleted file mode 100644 index 4ea3fa4bd009a..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp +++ /dev/null @@ -1,91 +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. -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ -#define AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ - -#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 -#include -#include -#include -#include -#include -#include -#endif - -#include - -namespace rviz_plugins -{ -class AutowareAutomaticCheckpointTool : public rviz_default_plugins::tools::PoseTool -{ - Q_OBJECT - -public: - AutowareAutomaticCheckpointTool(); - void onInitialize() override; - -protected: - void onPoseSet(double x, double y, double theta) override; - -private Q_SLOTS: - void updateTopic(); - -private: // NOLINT for Qt - rclcpp::Clock::SharedPtr clock_{nullptr}; - rclcpp::Publisher::SharedPtr pose_pub_{nullptr}; - - rviz_common::properties::StringProperty * pose_topic_property_{nullptr}; - rviz_common::properties::FloatProperty * std_dev_x_{nullptr}; - rviz_common::properties::FloatProperty * std_dev_y_{nullptr}; - rviz_common::properties::FloatProperty * std_dev_theta_{nullptr}; - rviz_common::properties::FloatProperty * position_z_{nullptr}; -}; - -} // namespace rviz_plugins - -#endif // AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp deleted file mode 100644 index 43fc0edcccf84..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp +++ /dev/null @@ -1,121 +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. -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "automatic_goal_append_tool.hpp" - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif -#include - -#include - -namespace rviz_plugins -{ -AutowareAutomaticGoalTool::AutowareAutomaticGoalTool() -{ - shortcut_key_ = 'c'; - - pose_topic_property_ = new rviz_common::properties::StringProperty( - "Pose Topic", "~/automatic_goal/goal", "The topic on which to publish automatic_goal.", - getPropertyContainer(), SLOT(updateTopic()), this); - std_dev_x_ = new rviz_common::properties::FloatProperty( - "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer()); - std_dev_y_ = new rviz_common::properties::FloatProperty( - "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer()); - std_dev_theta_ = new rviz_common::properties::FloatProperty( - "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]", - getPropertyContainer()); - position_z_ = new rviz_common::properties::FloatProperty( - "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer()); - std_dev_x_->setMin(0); - std_dev_y_->setMin(0); - std_dev_theta_->setMin(0); - position_z_->setMin(0); -} - -void AutowareAutomaticGoalTool::onInitialize() -{ - PoseTool::onInitialize(); - setName("2D AppendGoal"); - updateTopic(); -} - -void AutowareAutomaticGoalTool::updateTopic() -{ - rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - pose_pub_ = raw_node->create_publisher( - pose_topic_property_->getStdString(), 1); - clock_ = raw_node->get_clock(); -} - -void AutowareAutomaticGoalTool::onPoseSet(double x, double y, double theta) -{ - // pose - std::string fixed_frame = context_->getFixedFrame().toStdString(); - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = fixed_frame; - pose.header.stamp = clock_->now(); - pose.pose.position.x = x; - pose.pose.position.y = y; - pose.pose.position.z = position_z_->getFloat(); - - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, theta); - pose.pose.orientation = tf2::toMsg(quat); - RCLCPP_INFO( - rclcpp::get_logger("AutowareAutomaticGoalTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", - x, y, position_z_->getFloat(), theta, fixed_frame.c_str()); - pose_pub_->publish(pose); -} - -} // end namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticGoalTool, rviz_common::Tool) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp deleted file mode 100644 index 6fc98cee9afa1..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp +++ /dev/null @@ -1,91 +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. -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef AUTOMATIC_GOAL_APPEND_TOOL_HPP_ -#define AUTOMATIC_GOAL_APPEND_TOOL_HPP_ - -#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 -#include -#include -#include -#include -#include -#include -#endif - -#include - -namespace rviz_plugins -{ -class AutowareAutomaticGoalTool : public rviz_default_plugins::tools::PoseTool -{ - Q_OBJECT - -public: - AutowareAutomaticGoalTool(); - void onInitialize() override; - -protected: - void onPoseSet(double x, double y, double theta) override; - -private Q_SLOTS: - void updateTopic(); - -private: // NOLINT for Qt - rclcpp::Clock::SharedPtr clock_{nullptr}; - rclcpp::Publisher::SharedPtr pose_pub_{nullptr}; - - rviz_common::properties::StringProperty * pose_topic_property_{nullptr}; - rviz_common::properties::FloatProperty * std_dev_x_{nullptr}; - rviz_common::properties::FloatProperty * std_dev_y_{nullptr}; - rviz_common::properties::FloatProperty * std_dev_theta_{nullptr}; - rviz_common::properties::FloatProperty * position_z_{nullptr}; -}; - -} // namespace rviz_plugins - -#endif // AUTOMATIC_GOAL_APPEND_TOOL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp deleted file mode 100644 index 6b8d7765a989a..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp +++ /dev/null @@ -1,532 +0,0 @@ -// -// Copyright 2023 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 "automatic_goal_panel.hpp" - -#include - -namespace rviz_plugins -{ -AutowareAutomaticGoalPanel::AutowareAutomaticGoalPanel(QWidget * parent) -: rviz_common::Panel(parent) -{ - qt_timer_ = new QTimer(this); - connect( - qt_timer_, &QTimer::timeout, this, &AutowareAutomaticGoalPanel::updateAutoExecutionTimerTick); - - auto * h_layout = new QHBoxLayout(this); - auto * v_layout = new QVBoxLayout(this); - h_layout->addWidget(makeGoalsListGroup()); - v_layout->addWidget(makeEngagementGroup()); - v_layout->addWidget(makeRoutingGroup()); - h_layout->addLayout(v_layout); - setLayout(h_layout); -} - -// Layouts -QGroupBox * AutowareAutomaticGoalPanel::makeGoalsListGroup() -{ - auto * group = new QGroupBox("GoalsList", this); - auto * grid = new QGridLayout(group); - - load_file_btn_ptr_ = new QPushButton("Load from file", group); - connect(load_file_btn_ptr_, SIGNAL(clicked()), SLOT(onClickLoadListFromFile())); - grid->addWidget(load_file_btn_ptr_, 0, 0); - - save_file_btn_ptr_ = new QPushButton("Save to file", group); - connect(save_file_btn_ptr_, SIGNAL(clicked()), SLOT(onClickSaveListToFile())); - grid->addWidget(save_file_btn_ptr_, 1, 0); - - goals_list_widget_ptr_ = new QListWidget(group); - goals_list_widget_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(goals_list_widget_ptr_, 2, 0); - - remove_selected_goal_btn_ptr_ = new QPushButton("Remove selected", group); - connect(remove_selected_goal_btn_ptr_, SIGNAL(clicked()), SLOT(onClickRemove())); - grid->addWidget(remove_selected_goal_btn_ptr_, 3, 0); - - loop_list_btn_ptr_ = new QPushButton("Loop list", group); - loop_list_btn_ptr_->setCheckable(true); - connect(loop_list_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleLoopList(bool))); - grid->addWidget(loop_list_btn_ptr_, 4, 0); - - goals_achieved_btn_ptr_ = new QPushButton("Saving achieved goals to file", group); - goals_achieved_btn_ptr_->setCheckable(true); - connect(goals_achieved_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleSaveGoalsAchievement(bool))); - grid->addWidget(goals_achieved_btn_ptr_, 5, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareAutomaticGoalPanel::makeRoutingGroup() -{ - auto * group = new QGroupBox("Routing", this); - auto * grid = new QGridLayout(group); - - routing_label_ptr_ = new QLabel("INIT", group); - routing_label_ptr_->setMinimumSize(100, 25); - routing_label_ptr_->setAlignment(Qt::AlignCenter); - routing_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(routing_label_ptr_, 0, 0); - - clear_route_btn_ptr_ = new QPushButton("Clear planned route", group); - connect(clear_route_btn_ptr_, &QPushButton::clicked, [this]() { onClickClearRoute(); }); - grid->addWidget(clear_route_btn_ptr_, 1, 0); - group->setLayout(grid); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareAutomaticGoalPanel::makeEngagementGroup() -{ - auto * group = new QGroupBox("Engagement", this); - auto * grid = new QGridLayout(group); - - engagement_label_ptr_ = new QLabel("INITIALIZING", group); - engagement_label_ptr_->setMinimumSize(100, 25); - engagement_label_ptr_->setAlignment(Qt::AlignCenter); - engagement_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(engagement_label_ptr_, 0, 0); - - automatic_mode_btn_ptr_ = new QPushButton("Send goals automatically", group); - automatic_mode_btn_ptr_->setCheckable(true); - - connect(automatic_mode_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleAutoMode(bool))); - grid->addWidget(automatic_mode_btn_ptr_, 1, 0); - - plan_btn_ptr_ = new QPushButton("Plan to selected goal", group); - connect(plan_btn_ptr_, &QPushButton::clicked, [this] { onClickPlan(); }); - grid->addWidget(plan_btn_ptr_, 2, 0); - - start_btn_ptr_ = new QPushButton("Start current plan", group); - connect(start_btn_ptr_, &QPushButton::clicked, [this] { onClickStart(); }); - grid->addWidget(start_btn_ptr_, 3, 0); - - stop_btn_ptr_ = new QPushButton("Stop current plan", group); - connect(stop_btn_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); - grid->addWidget(stop_btn_ptr_, 4, 0); - group->setLayout(grid); - - group->setLayout(grid); - return group; -} - -void AutowareAutomaticGoalPanel::showMessageBox(const QString & string) -{ - QMessageBox msg_box(this); - msg_box.setText(string); - msg_box.exec(); -} - -// Slots -void AutowareAutomaticGoalPanel::onInitialize() -{ - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - pub_marker_ = raw_node_->create_publisher("~/automatic_goal/markers", 0); - sub_append_goal_ = raw_node_->create_subscription( - "~/automatic_goal/goal", 5, - std::bind(&AutowareAutomaticGoalPanel::onAppendGoal, this, std::placeholders::_1)); - sub_append_checkpoint_ = raw_node_->create_subscription( - "~/automatic_goal/checkpoint", 5, - std::bind(&AutowareAutomaticGoalPanel::onAppendCheckpoint, this, std::placeholders::_1)); - initCommunication(raw_node_.get()); -} - -void AutowareAutomaticGoalPanel::onToggleLoopList(bool checked) -{ - is_loop_list_on_ = checked; - updateGUI(); -} - -void AutowareAutomaticGoalPanel::onToggleSaveGoalsAchievement(bool checked) -{ - if (checked) { - QString file_name = QFileDialog::getSaveFileName( - this, tr("Save File with GoalsList"), "/tmp/goals_achieved.log", - tr("Achieved goals (*.log)")); - goals_achieved_file_path_ = file_name.toStdString(); - } else { - goals_achieved_file_path_ = ""; - } - updateGUI(); -} - -void AutowareAutomaticGoalPanel::onToggleAutoMode(bool checked) -{ - if (checked && goals_list_widget_ptr_->selectedItems().count() != 1) { - showMessageBox("Select the first goal in GoalsList"); - automatic_mode_btn_ptr_->setChecked(false); - } else { - if (checked) current_goal_ = goals_list_widget_ptr_->currentRow(); - is_automatic_mode_on_ = checked; - is_automatic_mode_on_ ? qt_timer_->start(1000) : qt_timer_->stop(); - onClickClearRoute(); // here will be set State::AUTO_NEXT or State::EDITING; - } -} - -void AutowareAutomaticGoalPanel::onClickPlan() -{ - if (goals_list_widget_ptr_->selectedItems().count() != 1) { - showMessageBox("Select a goal in GoalsList"); - return; - } - - if (callPlanToGoalIndex(cli_set_route_, goals_list_widget_ptr_->currentRow())) { - state_ = State::PLANNING; - updateGUI(); - } -} - -void AutowareAutomaticGoalPanel::onClickStart() -{ - if (callService(cli_change_to_autonomous_)) { - state_ = State::STARTING; - updateGUI(); - } -} - -void AutowareAutomaticGoalPanel::onClickStop() -{ - // if ERROR is set then the ego is already stopped - if (state_ == State::ERROR) { - state_ = State::STOPPED; - updateGUI(); - } else if (callService(cli_change_to_stop_)) { - state_ = State::STOPPING; - updateGUI(); - } -} - -void AutowareAutomaticGoalPanel::onClickClearRoute() -{ - if (callService(cli_clear_route_)) { - state_ = State::CLEARING; - updateGUI(); - } -} - -void AutowareAutomaticGoalPanel::onClickRemove() -{ - if (static_cast(goals_list_widget_ptr_->currentRow()) < goals_list_.size()) - goals_list_.erase(goals_list_.begin() + goals_list_widget_ptr_->currentRow()); - resetAchievedGoals(); - updateGUI(); - updateGoalsList(); -} - -void AutowareAutomaticGoalPanel::onClickLoadListFromFile() -{ - QString file_name = QFileDialog::getOpenFileName( - this, tr("Open File with GoalsList"), "/tmp", tr("Goal lists (*.yaml)")); - if (file_name.count() > 0) loadGoalsList(file_name.toStdString()); -} - -void AutowareAutomaticGoalPanel::onClickSaveListToFile() -{ - if (!goals_list_.empty()) { - QString file_name = QFileDialog::getSaveFileName( - this, tr("Save File with GoalsList"), "/tmp/goals_list.yaml", tr("Goal lists (*.yaml)")); - if (file_name.count() > 0) saveGoalsList(file_name.toStdString()); - } -} - -// Inputs -void AutowareAutomaticGoalPanel::onAppendGoal(const PoseStamped::ConstSharedPtr pose) -{ - if (state_ == State::EDITING) { - goals_list_.emplace_back(pose); - updateGoalsList(); - updateGUI(); - } -} - -void AutowareAutomaticGoalPanel::onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose) -{ - if (goals_list_widget_ptr_->selectedItems().count() != 1) { - showMessageBox("Select a goal in GoalsList before set checkpoint"); - return; - } - - current_goal_ = goals_list_widget_ptr_->currentRow(); - if (current_goal_ >= goals_list_.size()) { - return; - } - - goals_list_.at(current_goal_).checkpoint_pose_ptrs.push_back(pose); - publishMarkers(); -} - -// Override -void AutowareAutomaticGoalPanel::onCallResult() -{ - updateGUI(); -} -void AutowareAutomaticGoalPanel::onGoalListUpdated() -{ - goals_list_widget_ptr_->clear(); - for (auto const & goal : goals_achieved_) { - auto * item = - new QListWidgetItem(QString::fromStdString(goal.second.first), goals_list_widget_ptr_); - goals_list_widget_ptr_->addItem(item); - updateGoalIcon(goals_list_widget_ptr_->count() - 1, QColor("lightGray")); - } - publishMarkers(); -} -void AutowareAutomaticGoalPanel::onOperationModeUpdated(const OperationModeState::ConstSharedPtr) -{ - updateGUI(); -} -void AutowareAutomaticGoalPanel::onRouteUpdated(const RouteState::ConstSharedPtr msg) -{ - std::pair style; - if (msg->state == RouteState::UNSET) - style = std::make_pair("UNSET", "background-color: #FFFF00;"); // yellow - else if (msg->state == RouteState::SET) - style = std::make_pair("SET", "background-color: #00FF00;"); // green - else if (msg->state == RouteState::ARRIVED) - style = std::make_pair("ARRIVED", "background-color: #FFA500;"); // orange - else if (msg->state == RouteState::CHANGING) - style = std::make_pair("CHANGING", "background-color: #FFFF00;"); // yellow - else - style = std::make_pair("UNKNOWN", "background-color: #FF0000;"); // red - - updateLabel(routing_label_ptr_, style.first, style.second); - updateGUI(); -} - -void AutowareAutomaticGoalPanel::updateAutoExecutionTimerTick() -{ - if (is_automatic_mode_on_) { - if (state_ == State::AUTO_NEXT) { - // end if loop is off - if (current_goal_ >= goals_list_.size() && !is_loop_list_on_) { - disableAutomaticMode(); - return; - } - // plan to next goal - current_goal_ = current_goal_ % goals_list_.size(); - if (callPlanToGoalIndex(cli_set_route_, current_goal_)) { - state_ = State::PLANNING; - updateGUI(); - } - } else if (state_ == State::PLANNED) { - updateGoalIcon(current_goal_, QColor("yellow")); - onClickStart(); - } else if (state_ == State::ARRIVED) { - goals_achieved_[current_goal_].second++; - updateAchievedGoalsFile(current_goal_); - updateGoalIcon(current_goal_++, QColor("green")); - onClickClearRoute(); // will be set AUTO_NEXT as next state_ - } else if (state_ == State::STOPPED || state_ == State::ERROR) { - disableAutomaticMode(); - } - } -} - -// Visual updates -void AutowareAutomaticGoalPanel::updateGUI() -{ - deactivateButton(automatic_mode_btn_ptr_); - deactivateButton(remove_selected_goal_btn_ptr_); - deactivateButton(clear_route_btn_ptr_); - deactivateButton(plan_btn_ptr_); - deactivateButton(start_btn_ptr_); - deactivateButton(stop_btn_ptr_); - deactivateButton(load_file_btn_ptr_); - deactivateButton(save_file_btn_ptr_); - deactivateButton(loop_list_btn_ptr_); - deactivateButton(goals_achieved_btn_ptr_); - - std::pair style; - switch (state_) { - case State::EDITING: - activateButton(load_file_btn_ptr_); - if (!goals_list_.empty()) { - activateButton(goals_achieved_btn_ptr_); - activateButton(plan_btn_ptr_); - activateButton(remove_selected_goal_btn_ptr_); - activateButton(automatic_mode_btn_ptr_); - activateButton(save_file_btn_ptr_); - activateButton(loop_list_btn_ptr_); - } - style = std::make_pair("EDITING", "background-color: #FFFF00;"); - break; - case State::PLANNED: - activateButton(start_btn_ptr_); - activateButton(clear_route_btn_ptr_); - activateButton(save_file_btn_ptr_); - style = std::make_pair("PLANNED", "background-color: #00FF00;"); - break; - case State::STARTED: - activateButton(stop_btn_ptr_); - activateButton(save_file_btn_ptr_); - style = std::make_pair("STARTED", "background-color: #00FF00;"); - break; - case State::STOPPED: - activateButton(start_btn_ptr_); - activateButton(automatic_mode_btn_ptr_); - activateButton(clear_route_btn_ptr_); - activateButton(save_file_btn_ptr_); - style = std::make_pair("STOPPED", "background-color: #00FF00;"); - break; - case State::ARRIVED: - if (!is_automatic_mode_on_) onClickClearRoute(); // will be set EDITING as next state_ - break; - case State::CLEARED: - is_automatic_mode_on_ ? state_ = State::AUTO_NEXT : state_ = State::EDITING; - updateGUI(); - break; - case State::ERROR: - activateButton(stop_btn_ptr_); - if (!goals_list_.empty()) activateButton(save_file_btn_ptr_); - style = std::make_pair("ERROR", "background-color: #FF0000;"); - break; - case State::PLANNING: - activateButton(clear_route_btn_ptr_); - style = std::make_pair("PLANNING", "background-color: #FFA500;"); - break; - case State::STARTING: - style = std::make_pair("STARTING", "background-color: #FFA500;"); - break; - case State::STOPPING: - style = std::make_pair("STOPPING", "background-color: #FFA500;"); - break; - case State::CLEARING: - style = std::make_pair("CLEARING", "background-color: #FFA500;"); - break; - default: - break; - } - - automatic_mode_btn_ptr_->setStyleSheet(""); - loop_list_btn_ptr_->setStyleSheet(""); - goals_achieved_btn_ptr_->setStyleSheet(""); - if (is_automatic_mode_on_) automatic_mode_btn_ptr_->setStyleSheet("background-color: green"); - if (is_loop_list_on_) loop_list_btn_ptr_->setStyleSheet("background-color: green"); - if (!goals_achieved_file_path_.empty()) - goals_achieved_btn_ptr_->setStyleSheet("background-color: green"); - - updateLabel(engagement_label_ptr_, style.first, style.second); -} - -void AutowareAutomaticGoalPanel::updateGoalIcon(const unsigned goal_index, const QColor & color) -{ - QPixmap pixmap(24, 24); - pixmap.fill(color); - QPainter painter(&pixmap); - painter.setPen(QColor("black")); - painter.setFont(QFont("fixed-width", 10)); - QString text = QString::number(goals_achieved_[goal_index].second); - painter.drawText(QRect(0, 0, 24, 24), Qt::AlignCenter, text); - QIcon icon(pixmap); - goals_list_widget_ptr_->item(static_cast(goal_index))->setIcon(icon); -} - -void AutowareAutomaticGoalPanel::publishMarkers() -{ - using tier4_autoware_utils::createDefaultMarker; - using tier4_autoware_utils::createMarkerColor; - using tier4_autoware_utils::createMarkerScale; - - MarkerArray text_array; - MarkerArray arrow_array; - // Clear existing - { - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", 0L, Marker::CUBE, - createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - marker.action = visualization_msgs::msg::Marker::DELETEALL; - text_array.markers.push_back(marker); - pub_marker_->publish(text_array); - } - - { - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", 0L, Marker::CUBE, - createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - arrow_array.markers.push_back(marker); - pub_marker_->publish(arrow_array); - } - - text_array.markers.clear(); - arrow_array.markers.clear(); - - const auto push_arrow_marker = [&](const auto & pose, const auto & color, const size_t id) { - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", id, Marker::ARROW, - createMarkerScale(1.6, 0.5, 0.5), color); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = pose; - marker.lifetime = rclcpp::Duration(0, 0); - marker.frame_locked = false; - arrow_array.markers.push_back(marker); - }; - - const auto push_text_marker = [&](const auto & pose, const auto & text, const size_t id) { - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", id, Marker::TEXT_VIEW_FACING, - createMarkerScale(1.6, 1.6, 1.6), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = pose; - marker.lifetime = rclcpp::Duration(0, 0); - marker.frame_locked = false; - marker.text = text; - text_array.markers.push_back(marker); - }; - - // Publish current - size_t id = 0; - for (size_t i = 0; i < goals_list_.size(); ++i) { - { - const auto pose = goals_list_.at(i).goal_pose_ptr->pose; - push_arrow_marker(pose, createMarkerColor(0.0, 1.0, 0.0, 0.999), id++); - push_text_marker(pose, "Goal:" + std::to_string(i), id++); - } - - for (size_t j = 0; j < goals_list_.at(i).checkpoint_pose_ptrs.size(); ++j) { - const auto pose = goals_list_.at(i).checkpoint_pose_ptrs.at(j)->pose; - push_arrow_marker(pose, createMarkerColor(1.0, 1.0, 0.0, 0.999), id++); - push_text_marker( - pose, "Checkpoint:" + std::to_string(i) + "[Goal:" + std::to_string(j) + "]", id++); - } - } - pub_marker_->publish(text_array); - pub_marker_->publish(arrow_array); -} - -// File -void AutowareAutomaticGoalPanel::saveGoalsList(const std::string & file_path) -{ - YAML::Node node; - for (unsigned i = 0; i < goals_list_.size(); ++i) { - node[i]["position_x"] = goals_list_[i].goal_pose_ptr->pose.position.x; - node[i]["position_y"] = goals_list_[i].goal_pose_ptr->pose.position.y; - node[i]["position_z"] = goals_list_[i].goal_pose_ptr->pose.position.z; - node[i]["orientation_x"] = goals_list_[i].goal_pose_ptr->pose.orientation.x; - node[i]["orientation_y"] = goals_list_[i].goal_pose_ptr->pose.orientation.y; - node[i]["orientation_z"] = goals_list_[i].goal_pose_ptr->pose.orientation.z; - node[i]["orientation_w"] = goals_list_[i].goal_pose_ptr->pose.orientation.w; - } - std::ofstream file_out(file_path); - file_out << node; - file_out.close(); -} - -} // namespace rviz_plugins -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticGoalPanel, rviz_common::Panel) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp deleted file mode 100644 index 72a5bd4fb80fe..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp +++ /dev/null @@ -1,151 +0,0 @@ -// -// Copyright 2023 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 AUTOMATIC_GOAL_PANEL_HPP_ -#define AUTOMATIC_GOAL_PANEL_HPP_ - -#include "automatic_goal_sender.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -namespace rviz_plugins -{ -class AutowareAutomaticGoalPanel : public rviz_common::Panel, - public automatic_goal::AutowareAutomaticGoalSender -{ - using State = automatic_goal::AutomaticGoalState; - using Pose = geometry_msgs::msg::Pose; - using PoseStamped = geometry_msgs::msg::PoseStamped; - using Marker = visualization_msgs::msg::Marker; - using MarkerArray = visualization_msgs::msg::MarkerArray; - 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 SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; - using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; - Q_OBJECT - -public: - explicit AutowareAutomaticGoalPanel(QWidget * parent = nullptr); - void onInitialize() override; - -public Q_SLOTS: // NOLINT for Qt - void onToggleLoopList(bool checked); - void onToggleAutoMode(bool checked); - void onToggleSaveGoalsAchievement(bool checked); - void onClickPlan(); - void onClickStart(); - void onClickStop(); - void onClickClearRoute(); - void onClickRemove(); - void onClickLoadListFromFile(); - void onClickSaveListToFile(); - -private: - // Override - void updateAutoExecutionTimerTick() override; - void onRouteUpdated(const RouteState::ConstSharedPtr msg) override; - void onOperationModeUpdated(const OperationModeState::ConstSharedPtr msg) override; - void onCallResult() override; - void onGoalListUpdated() override; - - // Inputs - void onAppendGoal(const PoseStamped::ConstSharedPtr pose); - void onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose); - - // Visual updates - void updateGUI(); - void updateGoalIcon(const unsigned goal_index, const QColor & color); - void publishMarkers(); - void showMessageBox(const QString & string); - void disableAutomaticMode() { automatic_mode_btn_ptr_->setChecked(false); } - static void activateButton(QAbstractButton * btn) { btn->setEnabled(true); } - static void deactivateButton(QAbstractButton * btn) { btn->setEnabled(false); } - static void updateLabel(QLabel * label, const QString & text, const QString & style_sheet) - { - label->setText(text); - label->setStyleSheet(style_sheet); - } - // File - void saveGoalsList(const std::string & file); - - // Pub/Sub - rclcpp::Publisher::SharedPtr pub_marker_{nullptr}; - rclcpp::Subscription::SharedPtr sub_append_goal_{nullptr}; - rclcpp::Subscription::SharedPtr sub_append_checkpoint_{nullptr}; - - // Containers - rclcpp::Node::SharedPtr raw_node_{nullptr}; - bool is_automatic_mode_on_{false}; - bool is_loop_list_on_{false}; - - // QT Containers - QGroupBox * makeGoalsListGroup(); - QGroupBox * makeRoutingGroup(); - QGroupBox * makeEngagementGroup(); - QTimer * qt_timer_{nullptr}; - QListWidget * goals_list_widget_ptr_{nullptr}; - QLabel * routing_label_ptr_{nullptr}; - QLabel * operation_mode_label_ptr_{nullptr}; - QLabel * engagement_label_ptr_{nullptr}; - QPushButton * loop_list_btn_ptr_{nullptr}; - QPushButton * goals_achieved_btn_ptr_{nullptr}; - QPushButton * load_file_btn_ptr_{nullptr}; - QPushButton * save_file_btn_ptr_{nullptr}; - QPushButton * automatic_mode_btn_ptr_{nullptr}; - QPushButton * remove_selected_goal_btn_ptr_{nullptr}; - QPushButton * clear_route_btn_ptr_{nullptr}; - QPushButton * plan_btn_ptr_{nullptr}; - QPushButton * start_btn_ptr_{nullptr}; - QPushButton * stop_btn_ptr_{nullptr}; -}; -} // namespace rviz_plugins - -#endif // AUTOMATIC_GOAL_PANEL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp deleted file mode 100644 index 3ca368a3bd1a4..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp +++ /dev/null @@ -1,227 +0,0 @@ -// Copyright 2016 Open Source Robotics Foundation, 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 "automatic_goal_sender.hpp" - -namespace automatic_goal -{ -AutowareAutomaticGoalSender::AutowareAutomaticGoalSender() : Node("automatic_goal_sender") -{ -} - -void AutowareAutomaticGoalSender::init() -{ - loadParams(this); - initCommunication(this); - loadGoalsList(goals_list_file_path_); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&AutowareAutomaticGoalSender::updateAutoExecutionTimerTick, this)); - - // Print info - RCLCPP_INFO_STREAM(get_logger(), "GoalsList has been loaded from: " << goals_list_file_path_); - for (auto const & goal : goals_achieved_) - RCLCPP_INFO_STREAM(get_logger(), "Loaded: " << goal.second.first); - RCLCPP_INFO_STREAM( - get_logger(), "Achieved goals will be saved in: " << goals_achieved_file_path_); -} - -void AutowareAutomaticGoalSender::loadParams(rclcpp::Node * node) -{ - namespace fs = std::filesystem; - node->declare_parameter("goals_list_file_path", ""); - node->declare_parameter("goals_achieved_dir_path", ""); - // goals_list - goals_list_file_path_ = node->get_parameter("goals_list_file_path").as_string(); - if (!fs::exists(goals_list_file_path_) || !fs::is_regular_file(goals_list_file_path_)) - throw std::invalid_argument( - "goals_list_file_path parameter - file path is invalid: " + goals_list_file_path_); - // goals_achieved - goals_achieved_file_path_ = node->get_parameter("goals_achieved_dir_path").as_string(); - if (!fs::exists(goals_achieved_file_path_) || fs::is_regular_file(goals_achieved_file_path_)) - throw std::invalid_argument( - "goals_achieved_dir_path - directory path is invalid: " + goals_achieved_file_path_); - goals_achieved_file_path_ += "goals_achieved.log"; -} - -void AutowareAutomaticGoalSender::initCommunication(rclcpp::Node * node) -{ - // Executing - sub_operation_mode_ = node->create_subscription( - "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), - std::bind(&AutowareAutomaticGoalSender::onOperationMode, this, std::placeholders::_1)); - - cli_change_to_autonomous_ = - node->create_client("/api/operation_mode/change_to_autonomous"); - - cli_change_to_stop_ = - node->create_client("/api/operation_mode/change_to_stop"); - - // Planning - sub_route_ = node->create_subscription( - "/api/routing/state", rclcpp::QoS{1}.transient_local(), - std::bind(&AutowareAutomaticGoalSender::onRoute, this, std::placeholders::_1)); - - cli_clear_route_ = node->create_client("/api/routing/clear_route"); - - cli_set_route_ = node->create_client("/api/routing/set_route_points"); -} - -// Sub -void AutowareAutomaticGoalSender::onRoute(const RouteState::ConstSharedPtr msg) -{ - if (msg->state == RouteState::UNSET && state_ == State::CLEARING) - state_ = State::CLEARED; - else if (msg->state == RouteState::SET && state_ == State::PLANNING) - state_ = State::PLANNED; - else if (msg->state == RouteState::ARRIVED && state_ == State::STARTED) - state_ = State::ARRIVED; - onRouteUpdated(msg); -} - -void AutowareAutomaticGoalSender::onOperationMode(const OperationModeState::ConstSharedPtr msg) -{ - if (msg->mode == OperationModeState::STOP && state_ == State::INITIALIZING) - state_ = State::EDITING; - else if (msg->mode == OperationModeState::STOP && state_ == State::STOPPING) - state_ = State::STOPPED; - else if (msg->mode == OperationModeState::AUTONOMOUS && state_ == State::STARTING) - state_ = State::STARTED; - onOperationModeUpdated(msg); -} - -// Update -void AutowareAutomaticGoalSender::updateGoalsList() -{ - unsigned i = 0; - for (const auto & goal : goals_list_) { - std::stringstream ss; - ss << std::fixed << std::setprecision(2); - tf2::Quaternion tf2_quat; - tf2::convert(goal.goal_pose_ptr->pose.orientation, tf2_quat); - ss << "G" << i << " (" << goal.goal_pose_ptr->pose.position.x << ", "; - ss << goal.goal_pose_ptr->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; - goals_achieved_.insert({i++, std::make_pair(ss.str(), 0)}); - } - onGoalListUpdated(); -} - -void AutowareAutomaticGoalSender::updateAutoExecutionTimerTick() -{ - auto goal = goals_achieved_[current_goal_].first; - - if (state_ == State::INITIALIZING) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 3000, "Initializing... waiting for OperationModeState::STOP"); - - } else if (state_ == State::EDITING) { // skip the editing step by default - state_ = State::AUTO_NEXT; - - } else if (state_ == State::AUTO_NEXT) { // plan to next goal - RCLCPP_INFO_STREAM(get_logger(), goal << ": Goal set as the next. Planning in progress..."); - if (callPlanToGoalIndex(cli_set_route_, current_goal_)) state_ = State::PLANNING; - - } else if (state_ == State::PLANNED) { // start plan to next goal - RCLCPP_INFO_STREAM(get_logger(), goal << ": Route has been planned. Route starting..."); - if (callService(cli_change_to_autonomous_)) state_ = State::STARTING; - - } else if (state_ == State::STARTED) { - RCLCPP_INFO_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, goal << ": Driving to the goal."); - - } else if (state_ == State::ARRIVED) { // clear plan after achieving next goal, goal++ - RCLCPP_INFO_STREAM( - get_logger(), goal << ": Goal has been ACHIEVED " << ++goals_achieved_[current_goal_].second - << " times. Route clearing..."); - updateAchievedGoalsFile(current_goal_); - if (callService(cli_clear_route_)) state_ = State::CLEARING; - - } else if (state_ == State::CLEARED) { - RCLCPP_INFO_STREAM(get_logger(), goal << ": Route has been cleared."); - current_goal_++; - current_goal_ = current_goal_ % goals_list_.size(); - state_ = State::AUTO_NEXT; - - } else if (state_ == State::STOPPED) { - RCLCPP_WARN_STREAM( - get_logger(), goal << ": The execution has been stopped unexpectedly! Restarting..."); - if (callService(cli_change_to_autonomous_)) state_ = State::STARTING; - - } else if (state_ == State::ERROR) { - timer_->cancel(); - throw std::runtime_error(goal + ": Error encountered during execution!"); - } -} - -// File -void AutowareAutomaticGoalSender::loadGoalsList(const std::string & file_path) -{ - YAML::Node node = YAML::LoadFile(file_path); - goals_list_.clear(); - for (auto && goal : node) { - std::shared_ptr pose = std::make_shared(); - pose->header.frame_id = "map"; - pose->header.stamp = rclcpp::Time(); - pose->pose.position.x = goal["position_x"].as(); - pose->pose.position.y = goal["position_y"].as(); - pose->pose.position.z = goal["position_z"].as(); - pose->pose.orientation.x = goal["orientation_x"].as(); - pose->pose.orientation.y = goal["orientation_y"].as(); - pose->pose.orientation.z = goal["orientation_z"].as(); - pose->pose.orientation.w = goal["orientation_w"].as(); - goals_list_.emplace_back(pose); - } - resetAchievedGoals(); - updateGoalsList(); -} - -void AutowareAutomaticGoalSender::updateAchievedGoalsFile(const unsigned goal_index) -{ - if (!goals_achieved_file_path_.empty()) { - std::ofstream out(goals_achieved_file_path_, std::fstream::app); - std::stringstream ss; - ss << "[" << getTimestamp() << "] Achieved: " << goals_achieved_[goal_index].first; - ss << ", Current number of achievements: " << goals_achieved_[goal_index].second << "\n"; - out << ss.str(); - out.close(); - } -} - -void AutowareAutomaticGoalSender::resetAchievedGoals() -{ - goals_achieved_.clear(); - if (!goals_achieved_file_path_.empty()) { - std::ofstream out(goals_achieved_file_path_, std::fstream::app); - out << "[" << getTimestamp() - << "] GoalsList was loaded from a file or a goal was removed - counters have been reset\n"; - out.close(); - } -} -} // namespace automatic_goal - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - std::shared_ptr node{nullptr}; - try { - node = std::make_shared(); - node->init(); - } catch (const std::exception & e) { - fprintf(stderr, "%s Exiting...\n", e.what()); - return 1; - } - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp deleted file mode 100644 index 88b7b5e7dac20..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp +++ /dev/null @@ -1,184 +0,0 @@ -// Copyright 2016 Open Source Robotics Foundation, 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 AUTOMATIC_GOAL_SENDER_HPP_ -#define AUTOMATIC_GOAL_SENDER_HPP_ -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace automatic_goal -{ -enum class AutomaticGoalState { - INITIALIZING, - EDITING, - PLANNING, - PLANNED, - STARTING, - STARTED, - ARRIVED, - AUTO_NEXT, - STOPPING, - STOPPED, - CLEARING, - CLEARED, - ERROR, -}; - -class AutowareAutomaticGoalSender : public rclcpp::Node -{ - using State = AutomaticGoalState; - using PoseStamped = geometry_msgs::msg::PoseStamped; - 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 SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; - using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; - -public: - AutowareAutomaticGoalSender(); - void init(); - -protected: - void initCommunication(rclcpp::Node * node); - // Calls - bool callPlanToGoalIndex( - const rclcpp::Client::SharedPtr client, const unsigned goal_index) - { - if (!client->service_is_ready()) { - RCLCPP_WARN(get_logger(), "SetRoutePoints client is unavailable"); - return false; - } - - auto req = std::make_shared(); - req->header = goals_list_.at(goal_index).goal_pose_ptr->header; - req->goal = goals_list_.at(goal_index).goal_pose_ptr->pose; - for (const auto & checkpoint : goals_list_.at(goal_index).checkpoint_pose_ptrs) { - req->waypoints.push_back(checkpoint->pose); - } - client->async_send_request( - req, [this](typename rclcpp::Client::SharedFuture result) { - if (result.get()->status.code != 0) state_ = State::ERROR; - printCallResult(result); - onCallResult(); - }); - return true; - } - template - bool callService(const typename rclcpp::Client::SharedPtr client) - { - if (!client->service_is_ready()) { - RCLCPP_WARN(get_logger(), "Client is unavailable"); - return false; - } - - auto req = std::make_shared(); - client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { - if (result.get()->status.code != 0) state_ = State::ERROR; - printCallResult(result); - onCallResult(); - }); - return true; - } - template - void printCallResult(typename rclcpp::Client::SharedFuture result) - { - if (result.get()->status.code != 0) { - RCLCPP_ERROR( - get_logger(), "Service type \"%s\" status: %d, %s", typeid(T).name(), - result.get()->status.code, result.get()->status.message.c_str()); - } else { - RCLCPP_DEBUG( - get_logger(), "Service type \"%s\" status: %d, %s", typeid(T).name(), - result.get()->status.code, result.get()->status.message.c_str()); - } - } - - struct Route - { - explicit Route(const PoseStamped::ConstSharedPtr & goal) : goal_pose_ptr{goal} {} - PoseStamped::ConstSharedPtr goal_pose_ptr{}; - std::vector checkpoint_pose_ptrs{}; - }; - - // Update - void updateGoalsList(); - virtual void updateAutoExecutionTimerTick(); - - // File - void loadGoalsList(const std::string & file_path); - void updateAchievedGoalsFile(const unsigned goal_index); - void resetAchievedGoals(); - static std::string getTimestamp() - { - char buffer[128]; - std::time_t now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); - std::strftime(&buffer[0], sizeof(buffer), "%Y-%m-%d %H:%M:%S", std::localtime(&now)); - return std::string{buffer}; - } - - // Sub - void onRoute(const RouteState::ConstSharedPtr msg); - void onOperationMode(const OperationModeState::ConstSharedPtr msg); - - // Interface - virtual void onRouteUpdated(const RouteState::ConstSharedPtr) {} - virtual void onOperationModeUpdated(const OperationModeState::ConstSharedPtr) {} - virtual void onCallResult() {} - virtual void onGoalListUpdated() {} - - // Cli - rclcpp::Client::SharedPtr cli_change_to_autonomous_{nullptr}; - rclcpp::Client::SharedPtr cli_change_to_stop_{nullptr}; - rclcpp::Client::SharedPtr cli_clear_route_{nullptr}; - rclcpp::Client::SharedPtr cli_set_route_{nullptr}; - - // Containers - unsigned current_goal_{0}; - State state_{State::INITIALIZING}; - std::vector goals_list_{}; - std::map> goals_achieved_{}; - std::string goals_achieved_file_path_{}; - -private: - void loadParams(rclcpp::Node * node); - - // Sub - rclcpp::Subscription::SharedPtr sub_route_{nullptr}; - rclcpp::Subscription::SharedPtr sub_operation_mode_{nullptr}; - - // Containers - std::string goals_list_file_path_{}; - rclcpp::TimerBase::SharedPtr timer_{nullptr}; -}; -} // namespace automatic_goal -#endif // AUTOMATIC_GOAL_SENDER_HPP_ diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt deleted file mode 100644 index 9cb54e52362a5..0000000000000 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tier4_autoware_utils) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Boost REQUIRED) - -ament_auto_add_library(tier4_autoware_utils SHARED - src/geometry/geometry.cpp - src/geometry/pose_deviation.cpp - src/geometry/boost_polygon_utils.cpp - src/math/sin_table.cpp - src/math/trigonometry.cpp - src/ros/msg_operation.cpp - src/ros/marker_helper.cpp - src/ros/logger_level_configure.cpp - src/system/backtrace.cpp -) - -if(BUILD_TESTING) - find_package(ament_cmake_ros REQUIRED) - - file(GLOB_RECURSE test_files test/**/*.cpp) - - ament_add_ros_isolated_gtest(test_tier4_autoware_utils ${test_files}) - - target_link_libraries(test_tier4_autoware_utils - tier4_autoware_utils - ) -endif() - -ament_auto_package() 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_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp deleted file mode 100644 index 5d9001affa308..0000000000000 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ - -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" - -#include -#include -#include -#include - -#include - -namespace tier4_autoware_utils -{ -bool isClockwise(const Polygon2d & polygon); -Polygon2d inverseClockwise(const Polygon2d & polygon); -geometry_msgs::msg::Polygon rotatePolygon( - const geometry_msgs::msg::Polygon & polygon, const double & angle); -/// @brief rotate a polygon by some angle around the origin -/// @param[in] polygon input polygon -/// @param[in] angle angle of rotation [rad] -/// @return rotated polygon -Polygon2d rotatePolygon(const Polygon2d & polygon, const double angle); -Polygon2d toPolygon2d( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::DetectedObject & object); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::TrackedObject & object); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::PredictedObject & object); -Polygon2d toFootprint( - const geometry_msgs::msg::Pose & base_link_pose, const double base_to_front, - const double base_to_rear, const double width); -double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); -Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset); -} // namespace tier4_autoware_utils - -#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp deleted file mode 100644 index 984fa04016b57..0000000000000 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/constants.hpp +++ /dev/null @@ -1,24 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ - -namespace tier4_autoware_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 - -#endif // TIER4_AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp deleted file mode 100644 index 0c53a9e3941dd..0000000000000 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp +++ /dev/null @@ -1,27 +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 TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ - -namespace tier4_autoware_utils -{ - -float sin(float radian); - -float cos(float radian); - -} // namespace tier4_autoware_utils - -#endif // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp deleted file mode 100644 index 38a0a7696775a..0000000000000 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/unit_conversion.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ -#define TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ - -#include "tier4_autoware_utils/math/constants.hpp" - -namespace tier4_autoware_utils -{ -constexpr double deg2rad(const double deg) -{ - return deg * pi / 180.0; -} -constexpr double rad2deg(const double rad) -{ - return rad * 180.0 / pi; -} - -constexpr double kmph2mps(const double kmph) -{ - return kmph * 1000.0 / 3600.0; -} -constexpr double mps2kmph(const double mps) -{ - return mps * 3600.0 / 1000.0; -} -} // namespace tier4_autoware_utils - -#endif // TIER4_AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp deleted file mode 100644 index c00c3c1c364df..0000000000000 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp +++ /dev/null @@ -1,81 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ - -#include - -#include - -#include -#include - -namespace tier4_autoware_utils -{ -inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) -{ - geometry_msgs::msg::Point point; - point.x = x; - point.y = y; - point.z = z; - return point; -} - -inline geometry_msgs::msg::Quaternion createMarkerOrientation( - double x, double y, double z, double w) -{ - geometry_msgs::msg::Quaternion quaternion; - quaternion.x = x; - quaternion.y = y; - quaternion.z = z; - quaternion.w = w; - return quaternion; -} - -inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z) -{ - geometry_msgs::msg::Vector3 scale; - scale.x = x; - scale.y = y; - scale.z = z; - return scale; -} - -inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a) -{ - std_msgs::msg::ColorRGBA color; - color.r = r; - color.g = g; - color.b = b; - color.a = a; - return color; -} - -visualization_msgs::msg::Marker createDefaultMarker( - const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, - const int32_t type, const geometry_msgs::msg::Vector3 & scale, - const std_msgs::msg::ColorRGBA & color); - -visualization_msgs::msg::Marker createDeletedDefaultMarker( - const rclcpp::Time & now, const std::string & ns, const int32_t id); - -void appendMarkerArray( - const visualization_msgs::msg::MarkerArray & additional_marker_array, - visualization_msgs::msg::MarkerArray * marker_array, - const std::optional & current_time = {}); - -} // namespace tier4_autoware_utils - -#endif // TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp deleted file mode 100644 index f8d5baaf02777..0000000000000 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp +++ /dev/null @@ -1,66 +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 TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ - -#include - -#include - -namespace tier4_autoware_utils -{ - -template -class InterProcessPollingSubscriber -{ -private: - typename rclcpp::Subscription::SharedPtr subscriber_; - std::optional data_; - -public: - explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name) - { - auto noexec_callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - auto noexec_subscription_options = rclcpp::SubscriptionOptions(); - noexec_subscription_options.callback_group = noexec_callback_group; - - subscriber_ = node->create_subscription( - topic_name, rclcpp::QoS{1}, - [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); }, - noexec_subscription_options); - }; - bool updateLatestData() - { - rclcpp::MessageInfo message_info; - T tmp; - // The queue size (QoS) must be 1 to get the last message data. - if (subscriber_->take(tmp, message_info)) { - data_ = tmp; - } - return data_.has_value(); - }; - const T & getData() const - { - if (!data_.has_value()) { - throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber"); - } - return data_.value(); - }; -}; - -} // namespace tier4_autoware_utils - -#endif // TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp deleted file mode 100644 index c784d71ad5060..0000000000000 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp +++ /dev/null @@ -1,25 +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 TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ -#define TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ - -namespace tier4_autoware_utils -{ - -void print_backtrace(); - -} // namespace tier4_autoware_utils - -#endif // TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml deleted file mode 100644 index c34d3b5fdfdd0..0000000000000 --- a/common/tier4_autoware_utils/package.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - tier4_autoware_utils - 0.1.0 - The tier4_autoware_utils package - Takamasa Horibe - Takayuki Murooka - Mamoru Sobue - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs - autoware_internal_msgs - builtin_interfaces - diagnostic_msgs - geometry_msgs - logging_demo - pcl_conversions - pcl_ros - rclcpp - tf2 - tf2_geometry_msgs - tier4_debug_msgs - unique_identifier_msgs - visualization_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/tier4_autoware_utils/src/math/trigonometry.cpp deleted file mode 100644 index 15f5c71012722..0000000000000 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ /dev/null @@ -1,52 +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 "tier4_autoware_utils/math/trigonometry.hpp" - -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/sin_table.hpp" - -#include - -namespace tier4_autoware_utils -{ - -float sin(float radian) -{ - float degree = radian * (180.f / static_cast(tier4_autoware_utils::pi)) * - (discrete_arcs_num_360 / 360.f); - size_t idx = - (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % - discrete_arcs_num_360; - - float mul = 1.f; - if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) { - idx = 2 * discrete_arcs_num_90 - idx; - } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) { - mul = -1.f; - idx = idx - 2 * discrete_arcs_num_90; - } else if (3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90) { - mul = -1.f; - idx = 4 * discrete_arcs_num_90 - idx; - } - - return mul * g_sin_table[idx]; -} - -float cos(float radian) -{ - return sin(radian + static_cast(tier4_autoware_utils::pi) / 2.f); -} - -} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/ros/marker_helper.cpp b/common/tier4_autoware_utils/src/ros/marker_helper.cpp deleted file mode 100644 index fda997edc83db..0000000000000 --- a/common/tier4_autoware_utils/src/ros/marker_helper.cpp +++ /dev/null @@ -1,71 +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 "tier4_autoware_utils/ros/marker_helper.hpp" - -namespace tier4_autoware_utils -{ - -visualization_msgs::msg::Marker createDefaultMarker( - const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, - const int32_t type, const geometry_msgs::msg::Vector3 & scale, - const std_msgs::msg::ColorRGBA & color) -{ - visualization_msgs::msg::Marker marker; - - marker.header.frame_id = frame_id; - marker.header.stamp = now; - marker.ns = ns; - marker.id = id; - marker.type = type; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - - marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); - marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); - marker.scale = scale; - marker.color = color; - marker.frame_locked = true; - - return marker; -} - -visualization_msgs::msg::Marker createDeletedDefaultMarker( - const rclcpp::Time & now, const std::string & ns, const int32_t id) -{ - visualization_msgs::msg::Marker marker; - - marker.header.stamp = now; - marker.ns = ns; - marker.id = id; - marker.action = visualization_msgs::msg::Marker::DELETE; - - return marker; -} - -void appendMarkerArray( - const visualization_msgs::msg::MarkerArray & additional_marker_array, - visualization_msgs::msg::MarkerArray * marker_array, - const std::optional & current_time) -{ - for (const auto & marker : additional_marker_array.markers) { - marker_array->markers.push_back(marker); - - if (current_time) { - marker_array->markers.back().header.stamp = current_time.value(); - } - } -} - -} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp deleted file mode 100644 index c10e04736b5bb..0000000000000 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ /dev/null @@ -1,1831 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" - -#include - -#include - -#include - -constexpr double epsilon = 1e-6; - -TEST(geometry, getPoint) -{ - using tier4_autoware_utils::getPoint; - - const double x_ans = 1.0; - const double y_ans = 2.0; - const double z_ans = 3.0; - - { - struct AnyPoint - { - double x; - double y; - double z; - }; - const AnyPoint p{x_ans, y_ans, z_ans}; - const geometry_msgs::msg::Point p_out = getPoint(p); - EXPECT_DOUBLE_EQ(p_out.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.z, z_ans); - } - - { - geometry_msgs::msg::Point p; - p.x = x_ans; - p.y = y_ans; - p.z = z_ans; - const geometry_msgs::msg::Point p_out = getPoint(p); - EXPECT_DOUBLE_EQ(p_out.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.z, z_ans); - } - - { - geometry_msgs::msg::Pose p; - p.position.x = x_ans; - p.position.y = y_ans; - p.position.z = z_ans; - const geometry_msgs::msg::Point p_out = getPoint(p); - EXPECT_DOUBLE_EQ(p_out.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.z, z_ans); - } - - { - geometry_msgs::msg::PoseStamped p; - p.pose.position.x = x_ans; - p.pose.position.y = y_ans; - p.pose.position.z = z_ans; - const geometry_msgs::msg::Point p_out = getPoint(p); - EXPECT_DOUBLE_EQ(p_out.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.z, z_ans); - } - - { - geometry_msgs::msg::PoseWithCovarianceStamped p; - p.pose.pose.position.x = x_ans; - p.pose.pose.position.y = y_ans; - p.pose.pose.position.z = z_ans; - const geometry_msgs::msg::Point p_out = getPoint(p); - EXPECT_DOUBLE_EQ(p_out.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.z, z_ans); - } - - { - autoware_auto_planning_msgs::msg::PathPoint p; - p.pose.position.x = x_ans; - p.pose.position.y = y_ans; - p.pose.position.z = z_ans; - const geometry_msgs::msg::Point p_out = getPoint(p); - EXPECT_DOUBLE_EQ(p_out.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.z, z_ans); - } - - { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; - p.pose.position.x = x_ans; - p.pose.position.y = y_ans; - p.pose.position.z = z_ans; - const geometry_msgs::msg::Point p_out = getPoint(p); - EXPECT_DOUBLE_EQ(p_out.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.z, z_ans); - } -} - -TEST(geometry, getPose) -{ - using tier4_autoware_utils::getPose; - - const double x_ans = 1.0; - const double y_ans = 2.0; - const double z_ans = 3.0; - const double q_x_ans = 0.1; - const double q_y_ans = 0.2; - const double q_z_ans = 0.3; - const double q_w_ans = 0.4; - - { - geometry_msgs::msg::Pose p; - p.position.x = x_ans; - p.position.y = y_ans; - p.position.z = z_ans; - p.orientation.x = q_x_ans; - p.orientation.y = q_y_ans; - p.orientation.z = q_z_ans; - p.orientation.w = q_w_ans; - const geometry_msgs::msg::Pose p_out = getPose(p); - EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); - } - - { - geometry_msgs::msg::PoseStamped p; - p.pose.position.x = x_ans; - p.pose.position.y = y_ans; - p.pose.position.z = z_ans; - p.pose.orientation.x = q_x_ans; - p.pose.orientation.y = q_y_ans; - p.pose.orientation.z = q_z_ans; - p.pose.orientation.w = q_w_ans; - const geometry_msgs::msg::Pose p_out = getPose(p); - EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); - } - - { - autoware_auto_planning_msgs::msg::PathPoint p; - p.pose.position.x = x_ans; - p.pose.position.y = y_ans; - p.pose.position.z = z_ans; - p.pose.orientation.x = q_x_ans; - p.pose.orientation.y = q_y_ans; - p.pose.orientation.z = q_z_ans; - p.pose.orientation.w = q_w_ans; - const geometry_msgs::msg::Pose p_out = getPose(p); - EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); - } - - { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; - p.pose.position.x = x_ans; - p.pose.position.y = y_ans; - p.pose.position.z = z_ans; - p.pose.orientation.x = q_x_ans; - p.pose.orientation.y = q_y_ans; - p.pose.orientation.z = q_z_ans; - p.pose.orientation.w = q_w_ans; - const geometry_msgs::msg::Pose p_out = getPose(p); - EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); - } -} - -TEST(geometry, getLongitudinalVelocity) -{ - using tier4_autoware_utils::getLongitudinalVelocity; - - const double velocity = 1.0; - - { - autoware_auto_planning_msgs::msg::PathPoint p; - p.longitudinal_velocity_mps = velocity; - EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); - } - - { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; - p.longitudinal_velocity_mps = velocity; - EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); - } -} - -TEST(geometry, setPose) -{ - using tier4_autoware_utils::setPose; - - const double x_ans = 1.0; - const double y_ans = 2.0; - const double z_ans = 3.0; - const double q_x_ans = 0.1; - const double q_y_ans = 0.2; - const double q_z_ans = 0.3; - const double q_w_ans = 0.4; - - geometry_msgs::msg::Pose p; - p.position.x = x_ans; - p.position.y = y_ans; - p.position.z = z_ans; - p.orientation.x = q_x_ans; - p.orientation.y = q_y_ans; - p.orientation.z = q_z_ans; - p.orientation.w = q_w_ans; - - { - geometry_msgs::msg::Pose p_out{}; - setPose(p, p_out); - EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); - EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); - } - - { - geometry_msgs::msg::PoseStamped p_out{}; - setPose(p, p_out); - EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans); - } - - { - autoware_auto_planning_msgs::msg::PathPoint p_out{}; - setPose(p, p_out); - EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans); - } - - { - autoware_auto_planning_msgs::msg::TrajectoryPoint p_out{}; - setPose(p, p_out); - EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); - EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); - EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans); - EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans); - } -} - -TEST(geometry, setOrientation) -{ - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::setOrientation; - - geometry_msgs::msg::Pose p; - const auto orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - setOrientation(orientation, p); - - EXPECT_DOUBLE_EQ(p.orientation.x, orientation.x); - EXPECT_DOUBLE_EQ(p.orientation.y, orientation.y); - EXPECT_DOUBLE_EQ(p.orientation.z, orientation.z); - EXPECT_DOUBLE_EQ(p.orientation.w, orientation.w); -} - -TEST(geometry, setLongitudinalVelocity) -{ - using tier4_autoware_utils::setLongitudinalVelocity; - - const double velocity = 1.0; - - { - autoware_auto_planning_msgs::msg::PathPoint p{}; - setLongitudinalVelocity(velocity, p); - EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity); - } - - { - autoware_auto_planning_msgs::msg::TrajectoryPoint p{}; - setLongitudinalVelocity(velocity, p); - EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity); - } -} - -TEST(geometry, createPoint) -{ - using tier4_autoware_utils::createPoint; - - const geometry_msgs::msg::Point p_out = createPoint(1.0, 2.0, 3.0); - EXPECT_DOUBLE_EQ(p_out.x, 1.0); - EXPECT_DOUBLE_EQ(p_out.y, 2.0); - EXPECT_DOUBLE_EQ(p_out.z, 3.0); -} - -TEST(geometry, createQuaternion) -{ - using tier4_autoware_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 = - createQuaternion(0.18257419, 0.36514837, 0.54772256, 0.73029674); - EXPECT_DOUBLE_EQ(q_out.x, 0.18257419); - EXPECT_DOUBLE_EQ(q_out.y, 0.36514837); - EXPECT_DOUBLE_EQ(q_out.z, 0.54772256); - EXPECT_DOUBLE_EQ(q_out.w, 0.73029674); -} - -TEST(geometry, createTranslation) -{ - using tier4_autoware_utils::createTranslation; - - const geometry_msgs::msg::Vector3 v_out = createTranslation(1.0, 2.0, 3.0); - EXPECT_DOUBLE_EQ(v_out.x, 1.0); - EXPECT_DOUBLE_EQ(v_out.y, 2.0); - EXPECT_DOUBLE_EQ(v_out.z, 3.0); -} - -TEST(geometry, createQuaternionFromRPY) -{ - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - - { - const auto q_out = createQuaternionFromRPY(0, 0, 0); - EXPECT_DOUBLE_EQ(q_out.x, 0.0); - EXPECT_DOUBLE_EQ(q_out.y, 0.0); - EXPECT_DOUBLE_EQ(q_out.z, 0.0); - EXPECT_DOUBLE_EQ(q_out.w, 1.0); - } - - { - const auto q_out = createQuaternionFromRPY(0, 0, deg2rad(90)); - EXPECT_DOUBLE_EQ(q_out.x, 0.0); - EXPECT_DOUBLE_EQ(q_out.y, 0.0); - EXPECT_DOUBLE_EQ(q_out.z, 0.70710678118654757); - EXPECT_DOUBLE_EQ(q_out.w, 0.70710678118654757); - } - - { - const auto q_out = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - EXPECT_DOUBLE_EQ(q_out.x, 0.17677669529663687); - EXPECT_DOUBLE_EQ(q_out.y, 0.30618621784789724); - EXPECT_DOUBLE_EQ(q_out.z, 0.17677669529663692); - EXPECT_DOUBLE_EQ(q_out.w, 0.91855865354369193); - } -} - -TEST(geometry, createQuaternionFromYaw) -{ - using tier4_autoware_utils::createQuaternionFromYaw; - using tier4_autoware_utils::deg2rad; - - { - const auto q_out = createQuaternionFromYaw(0); - EXPECT_DOUBLE_EQ(q_out.x, 0.0); - EXPECT_DOUBLE_EQ(q_out.y, 0.0); - EXPECT_DOUBLE_EQ(q_out.z, 0.0); - EXPECT_DOUBLE_EQ(q_out.w, 1.0); - } - - { - const auto q_out = createQuaternionFromYaw(deg2rad(90)); - EXPECT_DOUBLE_EQ(q_out.x, 0.0); - EXPECT_DOUBLE_EQ(q_out.y, 0.0); - EXPECT_DOUBLE_EQ(q_out.z, 0.70710678118654757); - EXPECT_DOUBLE_EQ(q_out.w, 0.70710678118654757); - } - - { - const auto q_out = createQuaternionFromYaw(deg2rad(30)); - EXPECT_DOUBLE_EQ(q_out.x, 0.0); - EXPECT_DOUBLE_EQ(q_out.y, 0.0); - EXPECT_DOUBLE_EQ(q_out.z, 0.25881904510252074); - EXPECT_DOUBLE_EQ(q_out.w, 0.96592582628906831); - } -} - -TEST(geometry, calcElevationAngle) -{ - using tier4_autoware_utils::calcElevationAngle; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - - { - const auto p1 = createPoint(1.0, 1.0, 1.0); - const auto p2 = createPoint(1.0, 1.0, -10.0); - EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(-90.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 0.0, 0.0); - const auto p2 = createPoint(1.0, 0.0, -std::sqrt(3.0)); - EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(-60.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 0.0, -1.0); - const auto p2 = createPoint(0.0, 1.0, -2.0); - EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(-45.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 0.0, 1.0); - const auto p2 = createPoint(1.0, 1.0, 1.0); - EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(0.0), epsilon); - } - { - const auto p1 = createPoint(-100, -100, 0.0); - const auto p2 = createPoint(0.0, 0.0, 0.0); - EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(0.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 0.0, 1.0); - const auto p2 = createPoint(0.0, 1.0, 2.0); - EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(45.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 0.0, 0.0); - const auto p2 = createPoint(1.0, 0.0, std::sqrt(3.0)); - EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(60.0), epsilon); - } - { - const auto p1 = createPoint(1.0, 1.0, 1.0); - const auto p2 = createPoint(1.0, 1.0, 10.0); - EXPECT_NEAR(calcElevationAngle(p1, p2), deg2rad(90.0), epsilon); - } -} - -TEST(geometry, calcAzimuthAngle) -{ - using tier4_autoware_utils::calcAzimuthAngle; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - - { - const auto p1 = createPoint(0.0, 0.0, 9.0); - const auto p2 = createPoint(-100, -epsilon, 0.0); - EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-180.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 0.0, 2.0); - const auto p2 = createPoint(-1.0, -1.0, 0.0); - EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-135.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 10.0, 0.0); - const auto p2 = createPoint(0.0, 0.0, 6.0); - EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-90.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 0.0, 0.0); - const auto p2 = createPoint(1.0, -1.0, 4.0); - EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(-45.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 1.0, 3.3); - const auto p2 = createPoint(10.0, 1.0, -10.0); - EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(0.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 0.0, 2.0); - const auto p2 = createPoint(1.0, 1.0, 0.0); - EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(45.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 0.0, 10.0); - const auto p2 = createPoint(0.0, 10.0, 0.0); - EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(90.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 0.0, 2.0); - const auto p2 = createPoint(-1.0, 1.0, 0.0); - EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(135.0), epsilon); - } - { - const auto p1 = createPoint(0.0, 0.0, 9.0); - const auto p2 = createPoint(-100, epsilon, 0.0); - EXPECT_NEAR(calcAzimuthAngle(p1, p2), deg2rad(180.0), epsilon); - } -} - -TEST(geometry, calcDistance2d) -{ - using tier4_autoware_utils::calcDistance2d; - - geometry_msgs::msg::Point point; - point.x = 1.0; - point.y = 2.0; - point.z = 3.0; - - geometry_msgs::msg::Pose pose; - pose.position.x = 5.0; - pose.position.y = 5.0; - pose.position.z = 4.0; - - EXPECT_DOUBLE_EQ(calcDistance2d(point, pose), 5.0); -} - -TEST(geometry, calcSquaredDistance2d) -{ - using tier4_autoware_utils::calcSquaredDistance2d; - - geometry_msgs::msg::Point point; - point.x = 1.0; - point.y = 2.0; - point.z = 3.0; - - geometry_msgs::msg::Pose pose; - pose.position.x = 5.0; - pose.position.y = 5.0; - pose.position.z = 4.0; - - EXPECT_DOUBLE_EQ(calcSquaredDistance2d(point, pose), 25.0); -} - -TEST(geometry, calcDistance3d) -{ - using tier4_autoware_utils::calcDistance3d; - - geometry_msgs::msg::Point point; - point.x = 1.0; - point.y = 2.0; - point.z = 3.0; - - geometry_msgs::msg::Pose pose; - pose.position.x = 3.0; - pose.position.y = 4.0; - pose.position.z = 4.0; - - EXPECT_DOUBLE_EQ(calcDistance3d(point, pose), 3.0); -} - -TEST(geometry, getRPY) -{ - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getRPY; - - { - const double ans_roll = deg2rad(5); - const double ans_pitch = deg2rad(10); - const double ans_yaw = deg2rad(15); - const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); - const auto rpy = getRPY(quat); - EXPECT_NEAR(rpy.x, ans_roll, epsilon); - EXPECT_NEAR(rpy.y, ans_pitch, epsilon); - EXPECT_NEAR(rpy.z, ans_yaw, epsilon); - } - { - const double ans_roll = deg2rad(0); - const double ans_pitch = deg2rad(5); - const double ans_yaw = deg2rad(-10); - const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); - const auto rpy = getRPY(quat); - EXPECT_NEAR(rpy.x, ans_roll, epsilon); - EXPECT_NEAR(rpy.y, ans_pitch, epsilon); - EXPECT_NEAR(rpy.z, ans_yaw, epsilon); - } - { - const double ans_roll = deg2rad(30); - const double ans_pitch = deg2rad(-20); - const double ans_yaw = deg2rad(0); - const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); - const auto rpy = getRPY(quat); - EXPECT_NEAR(rpy.x, ans_roll, epsilon); - EXPECT_NEAR(rpy.y, ans_pitch, epsilon); - EXPECT_NEAR(rpy.z, ans_yaw, epsilon); - } -} - -TEST(geometry, getRPY_wrapper) -{ - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getRPY; - - { - const double ans_roll = deg2rad(45); - const double ans_pitch = deg2rad(25); - const double ans_yaw = deg2rad(-5); - const auto quat = createQuaternionFromRPY(ans_roll, ans_pitch, ans_yaw); - - // geometry_msgs::Pose - { - geometry_msgs::msg::Pose pose{}; - pose.orientation = quat; - const auto rpy = getRPY(pose); - EXPECT_NEAR(rpy.x, ans_roll, epsilon); - EXPECT_NEAR(rpy.y, ans_pitch, epsilon); - EXPECT_NEAR(rpy.z, ans_yaw, epsilon); - } - // geometry_msgs::PoseStamped - { - geometry_msgs::msg::PoseStamped pose{}; - pose.pose.orientation = quat; - const auto rpy = getRPY(pose); - EXPECT_NEAR(rpy.x, ans_roll, epsilon); - EXPECT_NEAR(rpy.y, ans_pitch, epsilon); - EXPECT_NEAR(rpy.z, ans_yaw, epsilon); - } - // geometry_msgs::PoseWithCovarianceStamped - { - geometry_msgs::msg::PoseWithCovarianceStamped pose{}; - pose.pose.pose.orientation = quat; - const auto rpy = getRPY(pose); - EXPECT_NEAR(rpy.x, ans_roll, epsilon); - EXPECT_NEAR(rpy.y, ans_pitch, epsilon); - EXPECT_NEAR(rpy.z, ans_yaw, epsilon); - } - } -} - -TEST(geometry, transform2pose) -{ - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::transform2pose; - - { - geometry_msgs::msg::Transform transform; - transform.translation.x = 1.0; - transform.translation.y = 2.0; - transform.translation.z = 3.0; - transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const geometry_msgs::msg::Pose pose = transform2pose(transform); - - EXPECT_DOUBLE_EQ(transform.translation.x, pose.position.x); - EXPECT_DOUBLE_EQ(transform.translation.y, pose.position.y); - EXPECT_DOUBLE_EQ(transform.translation.z, pose.position.z); - EXPECT_DOUBLE_EQ(transform.rotation.x, pose.orientation.x); - EXPECT_DOUBLE_EQ(transform.rotation.y, pose.orientation.y); - EXPECT_DOUBLE_EQ(transform.rotation.z, pose.orientation.z); - EXPECT_DOUBLE_EQ(transform.rotation.w, pose.orientation.w); - } - - { - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped.header.frame_id = "test"; - transform_stamped.header.stamp = rclcpp::Time(2.0); - transform_stamped.transform.translation.x = 1.0; - transform_stamped.transform.translation.y = 2.0; - transform_stamped.transform.translation.z = 3.0; - transform_stamped.transform.rotation = - createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const geometry_msgs::msg::PoseStamped pose_stamped = transform2pose(transform_stamped); - - EXPECT_EQ(transform_stamped.header.frame_id, pose_stamped.header.frame_id); - EXPECT_DOUBLE_EQ( - rclcpp::Time(transform_stamped.header.stamp).seconds(), - rclcpp::Time(pose_stamped.header.stamp).seconds()); - - EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.x, pose_stamped.pose.position.x); - EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.y, pose_stamped.pose.position.y); - EXPECT_DOUBLE_EQ(transform_stamped.transform.translation.z, pose_stamped.pose.position.z); - EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.x, pose_stamped.pose.orientation.x); - EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.y, pose_stamped.pose.orientation.y); - EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.z, pose_stamped.pose.orientation.z); - EXPECT_DOUBLE_EQ(transform_stamped.transform.rotation.w, pose_stamped.pose.orientation.w); - } -} - -TEST(geometry, pose2transform) -{ - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::pose2transform; - - { - geometry_msgs::msg::Pose pose; - pose.position.x = 1.0; - pose.position.y = 2.0; - pose.position.z = 3.0; - pose.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const geometry_msgs::msg::Transform transform = pose2transform(pose); - - EXPECT_DOUBLE_EQ(pose.position.x, transform.translation.x); - EXPECT_DOUBLE_EQ(pose.position.y, transform.translation.y); - EXPECT_DOUBLE_EQ(pose.position.z, transform.translation.z); - EXPECT_DOUBLE_EQ(pose.orientation.x, transform.rotation.x); - EXPECT_DOUBLE_EQ(pose.orientation.y, transform.rotation.y); - EXPECT_DOUBLE_EQ(pose.orientation.z, transform.rotation.z); - EXPECT_DOUBLE_EQ(pose.orientation.w, transform.rotation.w); - } - - { - geometry_msgs::msg::PoseStamped pose_stamped; - pose_stamped.header.frame_id = "test"; - pose_stamped.header.stamp = rclcpp::Time(2.0); - pose_stamped.pose.position.x = 1.0; - pose_stamped.pose.position.y = 2.0; - pose_stamped.pose.position.z = 3.0; - pose_stamped.pose.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - const std::string child_frame_id = "child"; - - const geometry_msgs::msg::TransformStamped transform_stamped = - pose2transform(pose_stamped, child_frame_id); - - EXPECT_EQ(pose_stamped.header.frame_id, transform_stamped.header.frame_id); - EXPECT_EQ(child_frame_id, transform_stamped.child_frame_id); - EXPECT_DOUBLE_EQ( - rclcpp::Time(pose_stamped.header.stamp).seconds(), - rclcpp::Time(transform_stamped.header.stamp).seconds()); - - EXPECT_DOUBLE_EQ(pose_stamped.pose.position.x, transform_stamped.transform.translation.x); - EXPECT_DOUBLE_EQ(pose_stamped.pose.position.y, transform_stamped.transform.translation.y); - EXPECT_DOUBLE_EQ(pose_stamped.pose.position.z, transform_stamped.transform.translation.z); - EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.x, transform_stamped.transform.rotation.x); - EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.y, transform_stamped.transform.rotation.y); - EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.z, transform_stamped.transform.rotation.z); - EXPECT_DOUBLE_EQ(pose_stamped.pose.orientation.w, transform_stamped.transform.rotation.w); - } -} - -TEST(geometry, point2tfVector) -{ - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::point2tfVector; - - // Point - { - geometry_msgs::msg::Point src; - src.x = 1.0; - src.y = 2.0; - src.z = 3.0; - - geometry_msgs::msg::Point dst; - dst.x = 10.0; - dst.y = 5.0; - dst.z = -5.0; - - const auto vec = point2tfVector(src, dst); - - EXPECT_DOUBLE_EQ(vec.x(), 9.0); - EXPECT_DOUBLE_EQ(vec.y(), 3.0); - EXPECT_DOUBLE_EQ(vec.z(), -8.0); - } - - // Pose - { - geometry_msgs::msg::Pose src; - src.position.x = 1.0; - src.position.y = 2.0; - src.position.z = 3.0; - src.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - geometry_msgs::msg::Pose dst; - dst.position.x = 10.0; - dst.position.y = 5.0; - dst.position.z = -5.0; - dst.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(10), deg2rad(10)); - - const auto vec = point2tfVector(src, dst); - - EXPECT_DOUBLE_EQ(vec.x(), 9.0); - EXPECT_DOUBLE_EQ(vec.y(), 3.0); - EXPECT_DOUBLE_EQ(vec.z(), -8.0); - } - - // Point and Pose - { - geometry_msgs::msg::Point src; - src.x = 1.0; - src.y = 2.0; - src.z = 3.0; - - geometry_msgs::msg::Pose dst; - dst.position.x = 10.0; - dst.position.y = 5.0; - dst.position.z = -5.0; - dst.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(10), deg2rad(10)); - - const auto vec = point2tfVector(src, dst); - - EXPECT_DOUBLE_EQ(vec.x(), 9.0); - EXPECT_DOUBLE_EQ(vec.y(), 3.0); - EXPECT_DOUBLE_EQ(vec.z(), -8.0); - } -} - -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; - - { - const Point2d p(1.0, 2.0); - - geometry_msgs::msg::Transform transform; - transform.translation.x = 1.0; - transform.translation.y = 2.0; - transform.rotation = createQuaternionFromRPY(0, 0, deg2rad(30)); - - const Point2d p_transformed = transformPoint(p, transform); - - EXPECT_DOUBLE_EQ(p_transformed.x(), 0.86602540378443882); - EXPECT_DOUBLE_EQ(p_transformed.y(), 4.2320508075688767); - } - - { - const Point3d p(1.0, 2.0, 3.0); - - geometry_msgs::msg::Transform transform; - transform.translation.x = 1.0; - transform.translation.y = 2.0; - transform.translation.z = 3.0; - transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const Point3d p_transformed = transformPoint(p, transform); - - EXPECT_DOUBLE_EQ(p_transformed.x(), 3.1919872981077804); - EXPECT_DOUBLE_EQ(p_transformed.y(), 3.5334936490538906); - EXPECT_DOUBLE_EQ(p_transformed.z(), 5.6160254037844393); - } - - { - const Eigen::Vector3d p(1.0, 2.0, 3.0); - - geometry_msgs::msg::Pose pose_transform; - pose_transform.position.x = 1.0; - pose_transform.position.y = 2.0; - pose_transform.position.z = 3.0; - pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const Eigen::Vector3d p_transformed = transformPoint(p, pose_transform); - - EXPECT_DOUBLE_EQ(p_transformed.x(), 3.1919872981077804); - EXPECT_DOUBLE_EQ(p_transformed.y(), 3.5334936490538906); - EXPECT_DOUBLE_EQ(p_transformed.z(), 5.6160254037844393); - } - - { - geometry_msgs::msg::Point p; - p.x = 1.0; - p.y = 2.0; - p.z = 3.0; - - geometry_msgs::msg::Pose pose_transform; - pose_transform.position.x = 1.0; - pose_transform.position.y = 2.0; - pose_transform.position.z = 3.0; - pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const geometry_msgs::msg::Point p_transformed = transformPoint(p, pose_transform); - - EXPECT_DOUBLE_EQ(p_transformed.x, 3.1919872981077804); - EXPECT_DOUBLE_EQ(p_transformed.y, 3.5334936490538906); - EXPECT_DOUBLE_EQ(p_transformed.z, 5.6160254037844393); - } - - { - geometry_msgs::msg::Point32 p; - p.x = 1.0; - p.y = 2.0; - p.z = 3.0; - - geometry_msgs::msg::Pose pose_transform; - pose_transform.position.x = 1.0; - pose_transform.position.y = 2.0; - pose_transform.position.z = 3.0; - pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const geometry_msgs::msg::Point32 p_transformed = transformPoint(p, pose_transform); - - EXPECT_DOUBLE_EQ(p_transformed.x, 3.1919872760772705); - EXPECT_DOUBLE_EQ(p_transformed.y, 3.5334937572479248); - EXPECT_DOUBLE_EQ(p_transformed.z, 5.616025447845459); - } -} - -TEST(geometry, transformPose) -{ - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::transformPose; - - geometry_msgs::msg::Pose pose; - pose.position.x = 2.0; - pose.position.y = 4.0; - pose.position.z = 6.0; - pose.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(20), deg2rad(30)); - - // with transform - { - geometry_msgs::msg::Transform transform; - transform.translation.x = 1.0; - transform.translation.y = 2.0; - transform.translation.z = 3.0; - transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const geometry_msgs::msg::Pose pose_transformed = transformPose(pose, transform); - - EXPECT_NEAR(pose_transformed.position.x, 5.3839745962155598, epsilon); - EXPECT_NEAR(pose_transformed.position.y, 5.0669872981077804, epsilon); - EXPECT_NEAR(pose_transformed.position.z, 8.2320508075688785, epsilon); - EXPECT_NEAR(pose_transformed.orientation.x, 0.24304508436548405, epsilon); - EXPECT_NEAR(pose_transformed.orientation.y, 0.4296803495383052, epsilon); - EXPECT_NEAR(pose_transformed.orientation.z, 0.40981009820187703, epsilon); - EXPECT_NEAR(pose_transformed.orientation.w, 0.76704600096616271, epsilon); - } - - // with pose_transform - { - geometry_msgs::msg::Pose pose_transform; - pose_transform.position.x = 1.0; - pose_transform.position.y = 2.0; - pose_transform.position.z = 3.0; - pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const geometry_msgs::msg::Pose pose_transformed = transformPose(pose, pose_transform); - - EXPECT_NEAR(pose_transformed.position.x, 5.3839745962155598, epsilon); - EXPECT_NEAR(pose_transformed.position.y, 5.0669872981077804, epsilon); - EXPECT_NEAR(pose_transformed.position.z, 8.2320508075688785, epsilon); - EXPECT_NEAR(pose_transformed.orientation.x, 0.24304508436548405, epsilon); - EXPECT_NEAR(pose_transformed.orientation.y, 0.4296803495383052, epsilon); - EXPECT_NEAR(pose_transformed.orientation.z, 0.40981009820187703, epsilon); - EXPECT_NEAR(pose_transformed.orientation.w, 0.76704600096616271, epsilon); - } -} - -TEST(geometry, inverseTransformPose) -{ - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::inverseTransformPose; - - geometry_msgs::msg::Pose pose; - pose.position.x = 2.0; - pose.position.y = 4.0; - pose.position.z = 6.0; - pose.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(20), deg2rad(30)); - - // with transform - { - geometry_msgs::msg::Transform transform; - transform.translation.x = 1.0; - transform.translation.y = 2.0; - transform.translation.z = 3.0; - transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const geometry_msgs::msg::Pose pose_transformed = inverseTransformPose(pose, transform); - - EXPECT_NEAR(pose_transformed.position.x, 0.11602540378443926, epsilon); - EXPECT_NEAR(pose_transformed.position.y, 2.8325317547305482, epsilon); - EXPECT_NEAR(pose_transformed.position.z, 2.4419872981077804, epsilon); - EXPECT_NEAR(pose_transformed.orientation.x, -0.17298739392508941, epsilon); - EXPECT_NEAR(pose_transformed.orientation.y, -0.08189960831908924, epsilon); - EXPECT_NEAR(pose_transformed.orientation.z, 0.029809019626209146, epsilon); - EXPECT_NEAR(pose_transformed.orientation.w, 0.98106026219040698, epsilon); - } - - // with pose_transform - { - geometry_msgs::msg::Pose pose_transform; - pose_transform.position.x = 1.0; - pose_transform.position.y = 2.0; - pose_transform.position.z = 3.0; - pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const geometry_msgs::msg::Pose pose_transformed = inverseTransformPose(pose, pose_transform); - - EXPECT_NEAR(pose_transformed.position.x, 0.11602540378443926, epsilon); - EXPECT_NEAR(pose_transformed.position.y, 2.8325317547305482, epsilon); - EXPECT_NEAR(pose_transformed.position.z, 2.4419872981077804, epsilon); - EXPECT_NEAR(pose_transformed.orientation.x, -0.17298739392508941, epsilon); - EXPECT_NEAR(pose_transformed.orientation.y, -0.08189960831908924, epsilon); - EXPECT_NEAR(pose_transformed.orientation.z, 0.029809019626209146, epsilon); - EXPECT_NEAR(pose_transformed.orientation.w, 0.98106026219040698, epsilon); - } -} - -TEST(geometry, inverseTransformPoint) -{ - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::inverseTransformPoint; - using tier4_autoware_utils::inverseTransformPose; - - geometry_msgs::msg::Pose pose_transform; - pose_transform.position.x = 1.0; - pose_transform.position.y = 2.0; - pose_transform.position.z = 3.0; - pose_transform.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - // calc expected values - geometry_msgs::msg::Pose pose; - pose.position.x = 2.0; - pose.position.y = 4.0; - pose.position.z = 6.0; - pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - const geometry_msgs::msg::Pose pose_transformed = inverseTransformPose(pose, pose_transform); - const geometry_msgs::msg::Point expected_p = pose_transformed.position; - - geometry_msgs::msg::Point p; - p.x = 2.0; - p.y = 4.0; - p.z = 6.0; - const geometry_msgs::msg::Point p_transformed = inverseTransformPoint(p, pose_transform); - EXPECT_NEAR(p_transformed.x, expected_p.x, epsilon); - EXPECT_NEAR(p_transformed.y, expected_p.y, epsilon); - EXPECT_NEAR(p_transformed.z, expected_p.z, epsilon); -} - -TEST(geometry, transformVector) -{ - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::MultiPoint3d; - using tier4_autoware_utils::transformVector; - - { - const MultiPoint3d ps{{1.0, 2.0, 3.0}, {2.0, 3.0, 4.0}}; - - geometry_msgs::msg::Transform transform; - transform.translation.x = 1.0; - transform.translation.y = 2.0; - transform.translation.z = 3.0; - transform.rotation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); - - const MultiPoint3d ps_transformed = transformVector(ps, transform); - - EXPECT_DOUBLE_EQ(ps_transformed.at(0).x(), 3.1919872981077804); - EXPECT_DOUBLE_EQ(ps_transformed.at(0).y(), 3.5334936490538906); - EXPECT_DOUBLE_EQ(ps_transformed.at(0).z(), 5.6160254037844393); - - EXPECT_DOUBLE_EQ(ps_transformed.at(1).x(), 4.350480947161671); - EXPECT_DOUBLE_EQ(ps_transformed.at(1).y(), 4.625); - EXPECT_DOUBLE_EQ(ps_transformed.at(1).z(), 6.299038105676658); - } -} - -TEST(geometry, calcCurvature) -{ - using tier4_autoware_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); - 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); - 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); - 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); - 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); - 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); - ASSERT_ANY_THROW(calcCurvature(p1, p1, p1)); - ASSERT_ANY_THROW(calcCurvature(p1, p1, p2)); - ASSERT_ANY_THROW(calcCurvature(p1, p2, p1)); - ASSERT_ANY_THROW(calcCurvature(p1, p2, p2)); - } -} - -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; - - // Only translation - { - geometry_msgs::msg::Pose p_in; - p_in.position = createPoint(1.0, 2.0, 3.0); - p_in.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); - - const auto p_out = calcOffsetPose(p_in, 1.0, 1.0, 1.0); - - EXPECT_DOUBLE_EQ(p_out.position.x, 2.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 3.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 4.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0); - } - - { - geometry_msgs::msg::Pose p_in; - p_in.position = createPoint(2.0, 3.0, 1.0); - p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(90)); - - const auto p_out = calcOffsetPose(p_in, 2.0, 1.0, 3.0); - - EXPECT_DOUBLE_EQ(p_out.position.x, 1.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 5.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 4.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.70710678118654757); - EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.70710678118654757); - } - - { - geometry_msgs::msg::Pose p_in; - p_in.position = createPoint(2.0, 1.0, 1.0); - p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30)); - - const auto p_out = calcOffsetPose(p_in, 2.0, 0.0, -1.0); - - EXPECT_DOUBLE_EQ(p_out.position.x, 3.73205080756887729); - EXPECT_DOUBLE_EQ(p_out.position.y, 2.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.25881904510252068); - EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.96592582628906831); - } - - // Only rotation - { - geometry_msgs::msg::Pose p_in; - p_in.position = createPoint(2.0, 1.0, 1.0); - p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30)); - - const auto p_out = calcOffsetPose(p_in, 0.0, 0.0, 0.0, deg2rad(20)); - - EXPECT_DOUBLE_EQ(p_out.position.x, 2.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 1.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 1.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); - EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon); - EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon); - } - - // Both translation and rotation - { - geometry_msgs::msg::Pose p_in; - p_in.position = createPoint(2.0, 1.0, 1.0); - p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30)); - - const auto p_out = calcOffsetPose(p_in, 2.0, 0.0, -1.0, deg2rad(20)); - - EXPECT_DOUBLE_EQ(p_out.position.x, 3.73205080756887729); - EXPECT_DOUBLE_EQ(p_out.position.y, 2.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); - EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon); - EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon); - } -} - -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; - - const double epsilon = 1e-3; - - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(3.0, 0.0, 0.0); - dst_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); - - EXPECT_TRUE(isDrivingForward(src_pose, dst_pose)); - } - - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(180)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(3.0, 0.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(180)); - - EXPECT_FALSE(isDrivingForward(src_pose, dst_pose)); - } - - // Boundary Condition - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(3.0, 0.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90)); - - EXPECT_TRUE(isDrivingForward(src_pose, dst_pose)); - } - - // Boundary Condition - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90 + epsilon)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(3.0, 0.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90 + epsilon)); - - EXPECT_FALSE(isDrivingForward(src_pose, dst_pose)); - } -} - -TEST(geometry, calcInterpolatedPoint) -{ - using tier4_autoware_utils::calcInterpolatedPoint; - using tier4_autoware_utils::createPoint; - - { - const auto src_point = createPoint(0.0, 0.0, 0.0); - const auto dst_point = createPoint(3.0, 0.0, 0.0); - - const double epsilon = 1e-3; - for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { - const auto p_out = calcInterpolatedPoint(src_point, dst_point, ratio); - - EXPECT_DOUBLE_EQ(p_out.x, 3.0 * ratio); - EXPECT_DOUBLE_EQ(p_out.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.z, 0.0); - } - } - - // Same points are given - { - const auto src_point = createPoint(0.0, 0.0, 0.0); - const auto dst_point = createPoint(0.0, 0.0, 0.0); - - const double epsilon = 1e-3; - for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { - const auto p_out = calcInterpolatedPoint(src_point, dst_point, ratio); - - EXPECT_DOUBLE_EQ(p_out.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.z, 0.0); - } - } - - // Boundary Condition (Negative Ratio) - { - const auto src_point = createPoint(0.0, 0.0, 0.0); - const auto dst_point = createPoint(3.0, 0.0, 0.0); - - const auto p_out = calcInterpolatedPoint(src_point, dst_point, -10.0); - EXPECT_DOUBLE_EQ(p_out.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.z, 0.0); - } - - // Boundary Condition (Positive Ratio larger than 1.0) - { - const auto src_point = createPoint(0.0, 0.0, 0.0); - const auto dst_point = createPoint(3.0, 0.0, 0.0); - - const auto p_out = calcInterpolatedPoint(src_point, dst_point, 10.0); - EXPECT_DOUBLE_EQ(p_out.x, 3.0); - EXPECT_DOUBLE_EQ(p_out.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.z, 0.0); - } -} - -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; - const double epsilon = 1e-3; - - // Position Interpolation - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(3.0, 0.0, 0.0); - dst_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); - - for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); - - EXPECT_DOUBLE_EQ(p_out.position.x, 3.0 * ratio); - EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0); - } - } - - // Boundary Condition (Negative Ratio) - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(1.0, 1.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); - - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, -10.0); - - const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45)); - EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); - EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); - EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); - EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); - } - - // Boundary Condition (Positive Ratio larger than 1.0) - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(1.0, 1.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); - - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, 10.0); - - const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); - EXPECT_DOUBLE_EQ(p_out.position.x, 1.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 1.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); - EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); - EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); - EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); - } - - // Quaternion Interpolation - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(1.0, 1.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); - - for (double ratio = 0.0; ratio < 1.0 - epsilon; ratio += 0.1) { - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); - - const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45)); - EXPECT_DOUBLE_EQ(p_out.position.x, 1.0 * ratio); - EXPECT_DOUBLE_EQ(p_out.position.y, 1.0 * ratio); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); - EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); - EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); - EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); - } - - // Boundary Condition (ratio = 1.0) - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, 1.0); - - const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); - EXPECT_DOUBLE_EQ(p_out.position.x, 1.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 1.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); - EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); - EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); - EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); - } - - // Same poses are given - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(0.0, 0.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); - - EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0); - } - } - - // Same points are given (different orientation) - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(0.0, 0.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45)); - - for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); - - const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45)); - EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); - EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); - EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); - EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); - } - } - - // Driving Backward - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(5.0, 0.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180)); - - for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio); - - const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180)); - EXPECT_DOUBLE_EQ(p_out.position.x, 5.0 * ratio); - EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); - EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); - EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); - EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); - } - } -} - -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; - const double epsilon = 1e-3; - - // Position Interpolation - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(3.0, 0.0, 0.0); - dst_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); - - for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false); - - EXPECT_DOUBLE_EQ(p_out.position.x, 3.0 * ratio); - EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0); - } - } - - // Boundary Condition (Negative Ratio) - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(1.0, 1.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); - - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, -10.0, false); - - const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); - EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); - EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); - EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); - } - - // Boundary Condition (Positive Ratio larger than 1.0) - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(1.0, 1.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); - - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, 10.0, false); - - const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); - EXPECT_DOUBLE_EQ(p_out.position.x, 1.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 1.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); - EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); - EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); - EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); - } - - // Quaternion Interpolation1 - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(0.0, 0.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(90)); - - for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false); - - const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(90 * ratio)); - EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); - EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); - EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); - EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); - } - } - - // Quaternion Interpolation2 - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(1.0, 1.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60)); - - for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false); - - const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60 * ratio)); - EXPECT_DOUBLE_EQ(p_out.position.x, 1.0 * ratio); - EXPECT_DOUBLE_EQ(p_out.position.y, 1.0 * ratio); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x); - EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y); - EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z); - EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w); - } - } - - // Same points are given - { - geometry_msgs::msg::Pose src_pose; - src_pose.position = createPoint(0.0, 0.0, 0.0); - src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - geometry_msgs::msg::Pose dst_pose; - dst_pose.position = createPoint(0.0, 0.0, 0.0); - dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0)); - - for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) { - const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio, false); - - EXPECT_DOUBLE_EQ(p_out.position.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0); - EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0); - } - } -} - -TEST(geometry, getTwist) -{ - using tier4_autoware_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); - - geometry_msgs::msg::Twist twist = geometry_msgs::build() - .linear(createVector3(1.0, 2.0, 3.0)) - .angular(createVector3(0.1, 0.2, 0.3)); - - 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)); - - // getTwist - { - auto t_out = tier4_autoware_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); - EXPECT_DOUBLE_EQ(t_out.angular.x, twist.angular.x); - EXPECT_DOUBLE_EQ(t_out.angular.y, twist.angular.y); - EXPECT_DOUBLE_EQ(t_out.angular.z, twist.angular.z); - } -} - -TEST(geometry, getTwistNorm) -{ - using tier4_autoware_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); -} - -TEST(geometry, isTwistCovarianceValid) -{ - using tier4_autoware_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); - - twist_with_covariance.covariance.at(0) = 1.0; - EXPECT_EQ(tier4_autoware_utils::isTwistCovarianceValid(twist_with_covariance), true); -} - -TEST(geometry, intersect) -{ - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::intersect; - - { // Normally crossing - const auto p1 = createPoint(0.0, -1.0, 0.0); - const auto p2 = createPoint(0.0, 1.0, 0.0); - const auto p3 = createPoint(-1.0, 0.0, 0.0); - const auto p4 = createPoint(1.0, 0.0, 0.0); - const auto result = intersect(p1, p2, p3, p4); - - EXPECT_TRUE(result); - EXPECT_NEAR(result->x, 0.0, epsilon); - EXPECT_NEAR(result->y, 0.0, epsilon); - EXPECT_NEAR(result->z, 0.0, epsilon); - } - - { // No crossing - const auto p1 = createPoint(0.0, -1.0, 0.0); - const auto p2 = createPoint(0.0, 1.0, 0.0); - const auto p3 = createPoint(1.0, 0.0, 0.0); - const auto p4 = createPoint(3.0, 0.0, 0.0); - const auto result = intersect(p1, p2, p3, p4); - - EXPECT_FALSE(result); - } - - { // One segment is the point on the other's segment - const auto p1 = createPoint(0.0, -1.0, 0.0); - const auto p2 = createPoint(0.0, 1.0, 0.0); - const auto p3 = createPoint(0.0, 0.0, 0.0); - const auto p4 = createPoint(0.0, 0.0, 0.0); - const auto result = intersect(p1, p2, p3, p4); - - EXPECT_FALSE(result); - } - - { // One segment is the point not on the other's segment - const auto p1 = createPoint(0.0, -1.0, 0.0); - const auto p2 = createPoint(0.0, 1.0, 0.0); - const auto p3 = createPoint(1.0, 0.0, 0.0); - const auto p4 = createPoint(1.0, 0.0, 0.0); - const auto result = intersect(p1, p2, p3, p4); - - EXPECT_FALSE(result); - } - - { // Both segments are the points which are the same position - const auto p1 = createPoint(0.0, 0.0, 0.0); - const auto p2 = createPoint(0.0, 0.0, 0.0); - const auto p3 = createPoint(0.0, 0.0, 0.0); - const auto p4 = createPoint(0.0, 0.0, 0.0); - const auto result = intersect(p1, p2, p3, p4); - - EXPECT_FALSE(result); - } - - { // Both segments are the points which are different position - const auto p1 = createPoint(0.0, 1.0, 0.0); - const auto p2 = createPoint(0.0, 1.0, 0.0); - const auto p3 = createPoint(1.0, 0.0, 0.0); - const auto p4 = createPoint(1.0, 0.0, 0.0); - const auto result = intersect(p1, p2, p3, p4); - - EXPECT_FALSE(result); - } - - { // Segments are the same - const auto p1 = createPoint(0.0, -1.0, 0.0); - const auto p2 = createPoint(0.0, 1.0, 0.0); - const auto p3 = createPoint(0.0, -1.0, 0.0); - const auto p4 = createPoint(0.0, 1.0, 0.0); - const auto result = intersect(p1, p2, p3, p4); - - EXPECT_FALSE(result); - } - - { // One's edge is on the other's segment - const auto p1 = createPoint(0.0, -1.0, 0.0); - const auto p2 = createPoint(0.0, 1.0, 0.0); - const auto p3 = createPoint(0.0, 0.0, 0.0); - const auto p4 = createPoint(1.0, 0.0, 0.0); - const auto result = intersect(p1, p2, p3, p4); - - EXPECT_TRUE(result); - EXPECT_NEAR(result->x, 0.0, epsilon); - EXPECT_NEAR(result->y, 0.0, epsilon); - EXPECT_NEAR(result->z, 0.0, epsilon); - } - - { // One's edge is the same as the other's edge. - const auto p1 = createPoint(0.0, -1.0, 0.0); - const auto p2 = createPoint(0.0, 1.0, 0.0); - const auto p3 = createPoint(0.0, -1.0, 0.0); - const auto p4 = createPoint(2.0, -1.0, 0.0); - const auto result = intersect(p1, p2, p3, p4); - - EXPECT_TRUE(result); - EXPECT_NEAR(result->x, 0.0, epsilon); - EXPECT_NEAR(result->y, -1.0, epsilon); - EXPECT_NEAR(result->z, 0.0, epsilon); - } -} diff --git a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp b/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp deleted file mode 100644 index 379418539841c..0000000000000 --- a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp +++ /dev/null @@ -1,42 +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 "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/trigonometry.hpp" - -#include - -#include - -TEST(trigonometry, sin) -{ - float x = 4.f * tier4_autoware_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); - } -} - -TEST(trigonometry, cos) -{ - float x = 4.f * tier4_autoware_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); - } -} diff --git a/common/tier4_autoware_utils/test/src/transform/test_transform.cpp b/common/tier4_autoware_utils/test/src/transform/test_transform.cpp deleted file mode 100644 index aae8cc22ea9fb..0000000000000 --- a/common/tier4_autoware_utils/test/src/transform/test_transform.cpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "tier4_autoware_utils/transform/transforms.hpp" - -#include -#include -#include - -#include - -TEST(system, transform_point_cloud) -{ - pcl::PointCloud cloud; - cloud.push_back(pcl::PointXYZI(10.055880, -42.758892, -10.636949, 4)); - cloud.push_back(pcl::PointXYZI(23.282284, -29.485722, -11.468469, 5)); - - Eigen::Matrix transform; - transform << 0.834513, -0.550923, -0.008474, 89571.148438, 0.550986, 0.834372, 0.015428, - 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); - - pcl::PointXYZI pt1_gt(89603.187500, 42270.878906, -13.056946, 4); - - constexpr float float_error = 0.0001; - EXPECT_NEAR(cloud_transformed[0].x, pt1_gt.x, float_error); - EXPECT_NEAR(cloud_transformed[0].y, pt1_gt.y, float_error); - EXPECT_NEAR(cloud_transformed[0].z, pt1_gt.z, float_error); - EXPECT_EQ(cloud_transformed[0].intensity, pt1_gt.intensity); -} - -TEST(system, empty_point_cloud) -{ - pcl::PointCloud cloud; - - Eigen::Matrix transform; - transform << 0.834513, -0.550923, -0.008474, 89571.148438, 0.550986, 0.834372, 0.015428, - 42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000; - - pcl::PointCloud cloud_transformed; - - EXPECT_NO_THROW(tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform)); - EXPECT_NO_FATAL_FAILURE( - tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform)); - EXPECT_EQ(cloud_transformed.size(), 0ul); -} diff --git a/common/tier4_calibration_rviz_plugin/CMakeLists.txt b/common/tier4_calibration_rviz_plugin/CMakeLists.txt deleted file mode 100644 index 6b03fe92da0ee..0000000000000 --- a/common/tier4_calibration_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tier4_calibration_rviz_plugin) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) -set(CMAKE_AUTOMOC ON) -set(CMAKE_INCLUDE_CURRENT_DIR ON) -add_definitions(-DQT_NO_KEYWORDS) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/accel_brake_map_calibrator_button_panel.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${QT_LIBRARIES} -) - -# Export the plugin to be imported by rviz2 -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - plugins -) diff --git a/common/tier4_calibration_rviz_plugin/package.xml b/common/tier4_calibration_rviz_plugin/package.xml deleted file mode 100644 index f422847d8cb70..0000000000000 --- a/common/tier4_calibration_rviz_plugin/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - tier4_calibration_rviz_plugin - 0.1.0 - The accel_brake_map_calibrator_button_panel package - Tomoya Kimura - Apache License 2.0 - - Tomoya Kimura - - ament_cmake_auto - autoware_cmake - - libqt5-core - libqt5-widgets - qtbase5-dev - rviz_common - std_msgs - tier4_vehicle_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/common/tier4_calibration_rviz_plugin/plugins/plugin_description.xml b/common/tier4_calibration_rviz_plugin/plugins/plugin_description.xml deleted file mode 100644 index 82f35b934208c..0000000000000 --- a/common/tier4_calibration_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - execution button of accel brake map calibration. - - - diff --git a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp deleted file mode 100644 index d89f13ce74d02..0000000000000 --- a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp +++ /dev/null @@ -1,185 +0,0 @@ -// -// 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 "accel_brake_map_calibrator_button_panel.hpp" - -#include "QFileDialog" -#include "QHBoxLayout" -#include "QLineEdit" -#include "QPainter" -#include "QPushButton" -#include "pluginlib/class_list_macros.hpp" -#include "rviz_common/display_context.hpp" - -#include -#include -#include -#include - -namespace tier4_calibration_rviz_plugin -{ -AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel(QWidget * parent) -: rviz_common::Panel(parent) -{ - topic_label_ = new QLabel("topic: "); - topic_label_->setAlignment(Qt::AlignCenter); - - topic_edit_ = - new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/output/update_suggest"); - connect(topic_edit_, SIGNAL(textEdited(QString)), SLOT(editTopic())); - - service_label_ = new QLabel("service: "); - service_label_->setAlignment(Qt::AlignCenter); - - service_edit_ = new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/update_map_dir"); - connect(service_edit_, SIGNAL(textEdited(QString)), SLOT(editService())); - - calibration_button_ = new QPushButton("Wait for subscribe topic"); - calibration_button_->setEnabled(false); - connect(calibration_button_, SIGNAL(clicked(bool)), SLOT(pushCalibrationButton())); - - status_label_ = new QLabel("Not Ready"); - status_label_->setAlignment(Qt::AlignCenter); - status_label_->setStyleSheet("QLabel { background-color : darkgray;}"); - - QSizePolicy q_size_policy(QSizePolicy::Expanding, QSizePolicy::Expanding); - calibration_button_->setSizePolicy(q_size_policy); - - auto * topic_layout = new QHBoxLayout; - topic_layout->addWidget(topic_label_); - topic_layout->addWidget(topic_edit_); - - auto * service_layout = new QHBoxLayout; - service_layout->addWidget(service_label_); - service_layout->addWidget(service_edit_); - - auto * v_layout = new QVBoxLayout; - v_layout->addLayout(topic_layout); - v_layout->addLayout(service_layout); - v_layout->addWidget(calibration_button_); - v_layout->addWidget(status_label_); - - setLayout(v_layout); -} - -void AccelBrakeMapCalibratorButtonPanel::onInitialize() -{ - rclcpp::Node::SharedPtr raw_node = - this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - update_suggest_sub_ = raw_node->create_subscription( - topic_edit_->text().toStdString(), 10, - std::bind( - &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1)); - - client_ = raw_node->create_client( - service_edit_->text().toStdString()); -} - -void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest( - const std_msgs::msg::Bool::ConstSharedPtr msg) -{ - if (after_calib_) { - return; - } - - if (!client_ || !client_->service_is_ready()) { - calibration_button_->setText("wait for service"); - calibration_button_->setEnabled(false); - return; - } - - if (msg->data) { - status_label_->setText("Ready"); - status_label_->setStyleSheet("QLabel { background-color : white;}"); - } else { - status_label_->setText("Ready (not recommended)"); - status_label_->setStyleSheet("QLabel { background-color : darkgray;}"); - } - calibration_button_->setText("push: start to accel/brake map calibration"); - calibration_button_->setEnabled(true); -} - -void AccelBrakeMapCalibratorButtonPanel::editTopic() -{ - rclcpp::Node::SharedPtr raw_node = - this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - try { - update_suggest_sub_.reset(); - update_suggest_sub_ = raw_node->create_subscription( - topic_edit_->text().toStdString(), 10, - std::bind( - &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1)); - } catch (const rclcpp::exceptions::InvalidTopicNameError & e) { - RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what()); - } - calibration_button_->setText("Wait for subscribe topic"); - calibration_button_->setEnabled(false); -} - -void AccelBrakeMapCalibratorButtonPanel::editService() -{ - rclcpp::Node::SharedPtr raw_node = - this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - try { - client_.reset(); - client_ = raw_node->create_client( - service_edit_->text().toStdString()); - } catch (const rclcpp::exceptions::InvalidServiceNameError & e) { - RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what()); - } -} - -void AccelBrakeMapCalibratorButtonPanel::pushCalibrationButton() -{ - // lock button - calibration_button_->setEnabled(false); - - status_label_->setStyleSheet("QLabel { background-color : blue;}"); - status_label_->setText("executing calibration..."); - - std::thread thread([this] { - if (!client_->wait_for_service(std::chrono::seconds(1))) { - status_label_->setStyleSheet("QLabel { background-color : red;}"); - status_label_->setText("service server not found"); - - } else { - auto req = std::make_shared(); - req->path = ""; - client_->async_send_request( - req, [this]([[maybe_unused]] rclcpp::Client< - tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>::SharedFuture result) {}); - - status_label_->setStyleSheet("QLabel { background-color : lightgreen;}"); - status_label_->setText("OK!!!"); - } - - // wait 3 second - after_calib_ = true; - rclcpp::Rate(1.0 / 3.0).sleep(); - after_calib_ = false; - - // unlock button - calibration_button_->setEnabled(true); - }); - - thread.detach(); -} - -} // namespace tier4_calibration_rviz_plugin - -PLUGINLIB_EXPORT_CLASS( - tier4_calibration_rviz_plugin::AccelBrakeMapCalibratorButtonPanel, rviz_common::Panel) diff --git a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp deleted file mode 100644 index 70ebe0631fa21..0000000000000 --- a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// -// 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 ACCEL_BRAKE_MAP_CALIBRATOR_BUTTON_PANEL_HPP_ -#define ACCEL_BRAKE_MAP_CALIBRATOR_BUTTON_PANEL_HPP_ - -#include "QLabel" -#include "QLineEdit" -#include "QPushButton" -#include "QSettings" - -#include -#ifndef Q_MOC_RUN - -#include "rclcpp/rclcpp.hpp" -#include "rviz_common/panel.hpp" -#include "rviz_common/properties/ros_topic_property.hpp" -#endif - -#include "std_msgs/msg/bool.hpp" -#include "tier4_vehicle_msgs/srv/update_accel_brake_map.hpp" - -namespace tier4_calibration_rviz_plugin -{ -class AccelBrakeMapCalibratorButtonPanel : public rviz_common::Panel -{ - Q_OBJECT - -public: - explicit AccelBrakeMapCalibratorButtonPanel(QWidget * parent = nullptr); - void onInitialize() override; - void callbackUpdateSuggest(const std_msgs::msg::Bool::ConstSharedPtr msg); - -public Q_SLOTS: // NOLINT for Qt - void editTopic(); - void editService(); - void pushCalibrationButton(); - -protected: - rclcpp::Subscription::SharedPtr update_suggest_sub_; - rclcpp::Client::SharedPtr client_; - - bool after_calib_ = false; - - QLabel * topic_label_; - QLineEdit * topic_edit_; - QLabel * service_label_; - QLineEdit * service_edit_; - QPushButton * calibration_button_; - QLabel * status_label_; -}; - -} // end namespace tier4_calibration_rviz_plugin - -#endif // ACCEL_BRAKE_MAP_CALIBRATOR_BUTTON_PANEL_HPP_ diff --git a/common/tier4_control_rviz_plugin/CMakeLists.txt b/common/tier4_control_rviz_plugin/CMakeLists.txt deleted file mode 100644 index 95fff455991ba..0000000000000 --- a/common/tier4_control_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tier4_control_rviz_plugin) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) - -set(CMAKE_AUTOMOC ON) -set(CMAKE_INCLUDE_CURRENT_DIR ON) -add_definitions(-DQT_NO_KEYWORDS) - -set(HEADERS - src/tools/manual_controller.hpp -) - -## Declare a C++ library -ament_auto_add_library(tier4_control_rviz_plugin SHARED - src/tools/manual_controller.cpp - ${HEADERS} -) - -target_link_libraries(tier4_control_rviz_plugin - ${QT_LIBRARIES} -) - -# Export the plugin to be imported by rviz2 -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - plugins -) diff --git a/common/tier4_control_rviz_plugin/README.md b/common/tier4_control_rviz_plugin/README.md deleted file mode 100644 index 8bca77771eee2..0000000000000 --- a/common/tier4_control_rviz_plugin/README.md +++ /dev/null @@ -1,40 +0,0 @@ -# tier4_control_rviz_plugin - -This package is to mimic external control for simulation. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------- | ----------------------- | -| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Current GATE mode | -| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | Current velocity status | -| `/api/autoware/get/engage` | `tier4_external_api_msgs::srv::Engage` | Getting Engage | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The state of GEAR | - -### Output - -| Name | Type | Description | -| -------------------------------- | ---------------------------------------------------------- | ----------------------- | -| `/control/gate_mode_cmd` | `tier4_control_msgs::msg::GateMode` | GATE mode | -| `/external/selected/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | AckermannControlCommand | -| `/external/selected/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | GEAR | - -## Usage - -1. Start rviz and select Panels. - - ![select_panels](./images/select_panels.png) - -2. Select tier4_control_rviz_plugin/ManualController and press OK. - - ![select_manual_controller](./images/select_manual_controller.png) - -3. Enter velocity in "Set Cruise Velocity" and Press the button to confirm. You can notice that GEAR shows D (DRIVE). - - ![manual_controller_not_ready](./images/manual_controller_not_ready.png) - -4. Press "Enable Manual Control" and you can notice that "GATE" and "Engage" turn "Ready" and the vehicle starts! - - ![manual_controller_ready](./images/manual_controller_ready.png) diff --git a/common/tier4_control_rviz_plugin/images/manual_controller_not_ready.png b/common/tier4_control_rviz_plugin/images/manual_controller_not_ready.png deleted file mode 100644 index e4a8ddb0b9bb1..0000000000000 Binary files a/common/tier4_control_rviz_plugin/images/manual_controller_not_ready.png and /dev/null differ diff --git a/common/tier4_control_rviz_plugin/images/manual_controller_ready.png b/common/tier4_control_rviz_plugin/images/manual_controller_ready.png deleted file mode 100644 index d5da7f0644051..0000000000000 Binary files a/common/tier4_control_rviz_plugin/images/manual_controller_ready.png and /dev/null differ diff --git a/common/tier4_control_rviz_plugin/images/select_manual_controller.png b/common/tier4_control_rviz_plugin/images/select_manual_controller.png deleted file mode 100644 index 5027ec571339c..0000000000000 Binary files a/common/tier4_control_rviz_plugin/images/select_manual_controller.png and /dev/null differ diff --git a/common/tier4_control_rviz_plugin/images/select_panels.png b/common/tier4_control_rviz_plugin/images/select_panels.png deleted file mode 100644 index a691602c42c3c..0000000000000 Binary files a/common/tier4_control_rviz_plugin/images/select_panels.png and /dev/null differ diff --git a/common/tier4_control_rviz_plugin/package.xml b/common/tier4_control_rviz_plugin/package.xml deleted file mode 100644 index 73562a766ce0b..0000000000000 --- a/common/tier4_control_rviz_plugin/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - tier4_control_rviz_plugin - 0.1.0 - The tier4_vehicle_rviz_plugin package - Taiki Tanaka - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_vehicle_msgs - libqt5-core - libqt5-gui - libqt5-widgets - qtbase5-dev - rviz_common - rviz_default_plugins - rviz_ogre_vendor - tier4_autoware_utils - tier4_control_msgs - tier4_external_api_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/tier4_control_rviz_plugin/plugins/plugin_description.xml b/common/tier4_control_rviz_plugin/plugins/plugin_description.xml deleted file mode 100644 index 068bbcd73f214..0000000000000 --- a/common/tier4_control_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - ManualController - - diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp deleted file mode 100644 index 8bbb096f728ec..0000000000000 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ /dev/null @@ -1,261 +0,0 @@ -// -// Copyright 2022 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 "manual_controller.hpp" - -#include -#include -#include -#include -#include - -#include -#include - -using std::placeholders::_1; - -namespace rviz_plugins -{ - -ManualController::ManualController(QWidget * parent) : rviz_common::Panel(parent) -{ - auto * state_layout = new QHBoxLayout; - { - // Enable Button - enable_button_ptr_ = new QPushButton("Enable Manual Control"); - connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickEnableButton())); - state_layout->addWidget(enable_button_ptr_); - - // Gate Mode - auto * gate_prefix_label_ptr = new QLabel("GATE: "); - gate_prefix_label_ptr->setAlignment(Qt::AlignRight); - gate_mode_label_ptr_ = new QLabel("INIT"); - gate_mode_label_ptr_->setAlignment(Qt::AlignCenter); - state_layout->addWidget(gate_prefix_label_ptr); - state_layout->addWidget(gate_mode_label_ptr_); - - // Engage Status - auto * engage_prefix_label_ptr = new QLabel("Engage: "); - engage_prefix_label_ptr->setAlignment(Qt::AlignRight); - engage_status_label_ptr_ = new QLabel("INIT"); - engage_status_label_ptr_->setAlignment(Qt::AlignCenter); - state_layout->addWidget(engage_prefix_label_ptr); - state_layout->addWidget(engage_status_label_ptr_); - - // Gear - auto * gear_prefix_label_ptr = new QLabel("GEAR: "); - gear_prefix_label_ptr->setAlignment(Qt::AlignRight); - gear_label_ptr_ = new QLabel("INIT"); - gear_label_ptr_->setAlignment(Qt::AlignCenter); - state_layout->addWidget(gear_prefix_label_ptr); - state_layout->addWidget(gear_label_ptr_); - } - - auto * cruise_velocity_layout = new QHBoxLayout(); - // Velocity Limit - { - cruise_velocity_button_ptr_ = new QPushButton("Set Cruise Velocity"); - cruise_velocity_input_ = new QSpinBox(); - cruise_velocity_input_->setRange(-100.0, 100.0); - cruise_velocity_input_->setValue(0.0); - cruise_velocity_input_->setSingleStep(5.0); - connect(cruise_velocity_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickCruiseVelocity())); - cruise_velocity_layout->addWidget(cruise_velocity_button_ptr_); - cruise_velocity_layout->addWidget(cruise_velocity_input_); - cruise_velocity_layout->addWidget(new QLabel(" [km/h]")); - } - - steering_slider_ptr_ = new QDial(); - steering_slider_ptr_->setRange(-90, 90); - steering_slider_ptr_->setValue(0.0); - connect(steering_slider_ptr_, SIGNAL(valueChanged(int)), this, SLOT(onManualSteering())); - steering_angle_ptr_ = new QLabel(); - cruise_velocity_layout->addWidget(new QLabel("steering ")); - cruise_velocity_layout->addWidget(steering_slider_ptr_); - cruise_velocity_layout->addWidget(steering_angle_ptr_); - cruise_velocity_layout->addWidget(new QLabel(" [deg]")); - - // Layout - auto * v_layout = new QVBoxLayout; - v_layout->addLayout(state_layout); - v_layout->addLayout(cruise_velocity_layout); - setLayout(v_layout); - - auto * timer = new QTimer(this); - connect(timer, &QTimer::timeout, this, &ManualController::update); - timer->start(30); -} - -void ManualController::update() -{ - if (!raw_node_) return; - AckermannControlCommand ackermann; - { - ackermann.stamp = raw_node_->get_clock()->now(); - ackermann.lateral.steering_tire_angle = steering_angle_; - ackermann.longitudinal.speed = cruise_velocity_; - /** - * @brief Calculate desired acceleration by simple BackSteppingControl - * V = 0.5*(v-v_des)^2 >= 0 - * D[V] = (D[v] - a_des)*(v-v_des) <=0 - * a_des = k_const *(v - v_des) + a (k < 0 ) - */ - const double k = -0.5; - const double v = current_velocity_; - const double v_des = cruise_velocity_; - const double a = current_acceleration_; - const double a_des = k * (v - v_des) + a; - ackermann.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0); - } - GearCommand gear_cmd; - { - const double eps = 0.001; - if (ackermann.longitudinal.speed > eps && current_velocity_ > -eps) { - gear_cmd.command = GearCommand::DRIVE; - } else if (ackermann.longitudinal.speed < -eps && current_velocity_ < eps) { - gear_cmd.command = GearCommand::REVERSE; - ackermann.longitudinal.acceleration *= -1.0; - } else { - gear_cmd.command = GearCommand::PARK; - } - } - pub_control_command_->publish(ackermann); - pub_gear_cmd_->publish(gear_cmd); -} - -void ManualController::onManualSteering() -{ - const double scale_factor = -0.25; - steering_angle_ = scale_factor * steering_slider_ptr_->sliderPosition() * M_PI / 180.0; - const QString steering_string = - QString::fromStdString(std::to_string(steering_angle_ * 180.0 / M_PI)); - steering_angle_ptr_->setText(steering_string); -} - -void ManualController::onClickCruiseVelocity() -{ - cruise_velocity_ = cruise_velocity_input_->value() / 3.6; -} - -void ManualController::onInitialize() -{ - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - sub_gate_mode_ = raw_node_->create_subscription( - "/control/current_gate_mode", 10, std::bind(&ManualController::onGateMode, this, _1)); - - sub_velocity_ = raw_node_->create_subscription( - "/vehicle/status/velocity_status", 1, std::bind(&ManualController::onVelocity, this, _1)); - - sub_engage_ = raw_node_->create_subscription( - "/api/autoware/get/engage", 10, std::bind(&ManualController::onEngageStatus, this, _1)); - - sub_gear_ = raw_node_->create_subscription( - "/vehicle/status/gear_status", 10, std::bind(&ManualController::onGear, this, _1)); - - client_engage_ = raw_node_->create_client("/api/autoware/set/engage"); - - pub_gate_mode_ = raw_node_->create_publisher("/control/gate_mode_cmd", rclcpp::QoS(1)); - - pub_control_command_ = raw_node_->create_publisher( - "/external/selected/control_cmd", rclcpp::QoS(1)); - - pub_gear_cmd_ = raw_node_->create_publisher("/external/selected/gear_cmd", 1); -} - -void ManualController::onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg) -{ - switch (msg->data) { - case tier4_control_msgs::msg::GateMode::AUTO: - gate_mode_label_ptr_->setText("Not Ready"); - gate_mode_label_ptr_->setStyleSheet("background-color: #00FF00;"); - break; - - case tier4_control_msgs::msg::GateMode::EXTERNAL: - gate_mode_label_ptr_->setText("Ready"); - gate_mode_label_ptr_->setStyleSheet("background-color: #FFFF00;"); - break; - - default: - gate_mode_label_ptr_->setText("UNKNOWN"); - gate_mode_label_ptr_->setStyleSheet("background-color: #FF0000;"); - break; - } -} -void ManualController::onEngageStatus(const Engage::ConstSharedPtr msg) -{ - current_engage_ = msg->engage; - if (current_engage_) { - engage_status_label_ptr_->setText(QString::fromStdString("Ready")); - engage_status_label_ptr_->setStyleSheet("background-color: #FFFF00;"); - } else { - engage_status_label_ptr_->setText(QString::fromStdString("Not Ready")); - engage_status_label_ptr_->setStyleSheet("background-color: #00FF00;"); - } -} - -void ManualController::onVelocity(const VelocityReport::ConstSharedPtr msg) -{ - current_velocity_ = msg->longitudinal_velocity; -} - -void ManualController::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg) -{ - current_acceleration_ = msg->accel.accel.linear.x; -} - -void ManualController::onGear(const GearReport::ConstSharedPtr msg) -{ - switch (msg->report) { - case GearReport::PARK: - gear_label_ptr_->setText("P"); - break; - case GearReport::REVERSE: - gear_label_ptr_->setText("R"); - break; - case GearReport::DRIVE: - gear_label_ptr_->setText("D"); - break; - case GearReport::LOW: - gear_label_ptr_->setText("L"); - break; - } -} - -void ManualController::onClickEnableButton() -{ - // gate mode - { - pub_gate_mode_->publish(tier4_control_msgs::build().data(GateMode::EXTERNAL)); - } - // engage - { - auto req = std::make_shared(); - req->engage = true; - RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); - if (!client_engage_->service_is_ready()) { - RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); - return; - } - client_engage_->async_send_request( - req, []([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); - } -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::ManualController, rviz_common::Panel) diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp deleted file mode 100644 index aaa625bff911e..0000000000000 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp +++ /dev/null @@ -1,104 +0,0 @@ -// -// Copyright 2022 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 TOOLS__MANUAL_CONTROLLER_HPP_ -#define TOOLS__MANUAL_CONTROLLER_HPP_ - -#include -#include -#include -#include -#include -#include - -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" -#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include -#include -#include -#include -#include -#include - -#include - -namespace rviz_plugins -{ -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_vehicle_msgs::msg::GearCommand; -using autoware_auto_vehicle_msgs::msg::VelocityReport; -using geometry_msgs::msg::AccelWithCovarianceStamped; -using geometry_msgs::msg::Twist; -using tier4_control_msgs::msg::GateMode; -using EngageSrv = tier4_external_api_msgs::srv::Engage; -using autoware_auto_vehicle_msgs::msg::Engage; -using autoware_auto_vehicle_msgs::msg::GearReport; - -class ManualController : public rviz_common::Panel -{ - Q_OBJECT - -public: - explicit ManualController(QWidget * parent = nullptr); - void onInitialize() override; - -public Q_SLOTS: // NOLINT for Qt - void onClickCruiseVelocity(); - void onClickEnableButton(); - void onManualSteering(); - void update(); - -protected: - // Timer - rclcpp::TimerBase::SharedPtr timer_; - void onTimer(); - void onPublishControlCommand(); - void onGateMode(const GateMode::ConstSharedPtr msg); - void onVelocity(const VelocityReport::ConstSharedPtr msg); - void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); - void onEngageStatus(const Engage::ConstSharedPtr msg); - void onGear(const GearReport::ConstSharedPtr msg); - rclcpp::Node::SharedPtr raw_node_; - rclcpp::Subscription::SharedPtr sub_gate_mode_; - rclcpp::Subscription::SharedPtr sub_velocity_; - rclcpp::Subscription::SharedPtr sub_engage_; - rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_control_command_; - rclcpp::Publisher::SharedPtr pub_gear_cmd_; - rclcpp::Client::SharedPtr client_engage_; - rclcpp::Subscription::SharedPtr sub_gear_; - - double cruise_velocity_{0.0}; - double steering_angle_{0.0}; - double current_velocity_{0.0}; - double current_acceleration_{0.0}; - - QLabel * gate_mode_label_ptr_; - QLabel * gear_label_ptr_; - QLabel * engage_status_label_ptr_; - QPushButton * enable_button_ptr_; - QPushButton * cruise_velocity_button_ptr_; - QSpinBox * cruise_velocity_input_; - QDial * steering_slider_ptr_; - QLabel * steering_angle_ptr_; - - bool current_engage_{false}; -}; - -} // namespace rviz_plugins - -#endif // TOOLS__MANUAL_CONTROLLER_HPP_ diff --git a/common/tier4_debug_rviz_plugin/CMakeLists.txt b/common/tier4_debug_rviz_plugin/CMakeLists.txt deleted file mode 100644 index 02e65ad759ed6..0000000000000 --- a/common/tier4_debug_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tier4_debug_rviz_plugin) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) -set(CMAKE_AUTOMOC ON) -add_definitions(-DQT_NO_KEYWORDS) - -ament_auto_add_library(tier4_debug_rviz_plugin SHARED - include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp - include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp - include/tier4_debug_rviz_plugin/string_stamped.hpp - src/float32_multi_array_stamped_pie_chart.cpp - src/string_stamped.cpp - src/jsk_overlay_utils.cpp -) - -target_link_libraries(tier4_debug_rviz_plugin - ${QT_LIBRARIES} -) - -# Export the plugin to be imported by rviz2 -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - icons - plugins -) diff --git a/common/tier4_debug_rviz_plugin/README.md b/common/tier4_debug_rviz_plugin/README.md deleted file mode 100644 index 9189816141632..0000000000000 --- a/common/tier4_debug_rviz_plugin/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# tier4_debug_rviz_plugin - -This package is including jsk code. -Note that jsk_overlay_utils.cpp and jsk_overlay_utils.hpp are BSD license. - -## Plugins - -### Float32MultiArrayStampedPieChart - -Pie chart from `tier4_debug_msgs::msg::Float32MultiArrayStamped`. - -![float32_multi_array_stamped_pie_chart](./images/float32_multi_array_stamped_pie_chart.png) diff --git a/common/tier4_debug_rviz_plugin/icons/classes/Float32MultiArrayStampedPieChart.png b/common/tier4_debug_rviz_plugin/icons/classes/Float32MultiArrayStampedPieChart.png deleted file mode 100644 index 6a67573717ae1..0000000000000 Binary files a/common/tier4_debug_rviz_plugin/icons/classes/Float32MultiArrayStampedPieChart.png and /dev/null differ diff --git a/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png deleted file mode 100644 index 6a67573717ae1..0000000000000 Binary files a/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png and /dev/null differ diff --git a/common/tier4_debug_rviz_plugin/images/float32_multi_array_stamped_pie_chart.png b/common/tier4_debug_rviz_plugin/images/float32_multi_array_stamped_pie_chart.png deleted file mode 100644 index 7cd7ca753f386..0000000000000 Binary files a/common/tier4_debug_rviz_plugin/images/float32_multi_array_stamped_pie_chart.png and /dev/null differ diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp deleted file mode 100644 index c8267c7051d9d..0000000000000 --- a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp +++ /dev/null @@ -1,172 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// Copyright (c) 2014, JSK Lab -// All rights reserved. -// -// Software License Agreement (BSD License) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#ifndef TIER4_DEBUG_RVIZ_PLUGIN__FLOAT32_MULTI_ARRAY_STAMPED_PIE_CHART_HPP_ -#define TIER4_DEBUG_RVIZ_PLUGIN__FLOAT32_MULTI_ARRAY_STAMPED_PIE_CHART_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -namespace rviz_plugins -{ -class Float32MultiArrayStampedPieChartDisplay : public rviz_common::Display -{ - Q_OBJECT -public: - Float32MultiArrayStampedPieChartDisplay(); - virtual ~Float32MultiArrayStampedPieChartDisplay(); - - // methods for OverlayPickerTool - virtual bool isInRegion(int x, int y); - virtual void movePosition(int x, int y); - virtual void setPosition(int x, int y); - virtual int getX() { return left_; } - virtual int getY() { return top_; } - -protected: - virtual void subscribe(); - virtual void unsubscribe(); - virtual void onEnable(); - virtual void onDisable(); - virtual void onInitialize(); - virtual void processMessage( - const tier4_debug_msgs::msg::Float32MultiArrayStamped::ConstSharedPtr msg); - virtual void drawPlot(double val); - virtual void update(float wall_dt, float ros_dt); - // properties - rviz_common::properties::StringProperty * update_topic_property_; - rviz_common::properties::IntProperty * data_index_property_; - rviz_common::properties::IntProperty * size_property_; - rviz_common::properties::IntProperty * left_property_; - rviz_common::properties::IntProperty * top_property_; - rviz_common::properties::ColorProperty * fg_color_property_; - rviz_common::properties::ColorProperty * bg_color_property_; - rviz_common::properties::ColorProperty * text_color_property_; - rviz_common::properties::FloatProperty * fg_alpha_property_; - rviz_common::properties::FloatProperty * fg_alpha2_property_; - rviz_common::properties::FloatProperty * bg_alpha_property_; - rviz_common::properties::FloatProperty * text_alpha_property_; - rviz_common::properties::IntProperty * text_size_property_; - rviz_common::properties::FloatProperty * max_value_property_; - rviz_common::properties::FloatProperty * min_value_property_; - rviz_common::properties::BoolProperty * show_caption_property_; - rviz_common::properties::BoolProperty * auto_color_change_property_; - rviz_common::properties::ColorProperty * max_color_property_; - rviz_common::properties::ColorProperty * med_color_property_; - rviz_common::properties::FloatProperty * max_color_threshold_property_; - rviz_common::properties::FloatProperty * med_color_threshold_property_; - rviz_common::properties::BoolProperty * clockwise_rotate_property_; - - rclcpp::Subscription::SharedPtr sub_; - int left_; - int top_; - uint16_t texture_size_; - QColor fg_color_; - QColor bg_color_; - QColor max_color_; - QColor med_color_; - int text_size_; - bool show_caption_; - bool auto_color_change_; - int caption_offset_; - double fg_alpha_; - double fg_alpha2_; - double bg_alpha_; - double max_value_; - double min_value_; - double max_color_threshold_; - double med_color_threshold_; - bool update_required_; - bool first_time_; - float data_; - int data_index_{0}; - jsk_rviz_plugins::OverlayObject::Ptr overlay_; - bool clockwise_rotate_; - - std::mutex mutex_; - -protected Q_SLOTS: - void updateTopic(); - void updateDataIndex(); - void updateSize(); - void updateTop(); - void updateLeft(); - void updateBGColor(); - void updateTextSize(); - void updateFGColor(); - void updateFGAlpha(); - void updateFGAlpha2(); - void updateBGAlpha(); - void updateMinValue(); - void updateMaxValue(); - void updateShowCaption(); - void updateAutoColorChange(); - void updateMaxColor(); - void updateMedColor(); - void updateMaxColorThreshold(); - void updateMedColorThreshold(); - void updateClockwiseRotate(); - -private: -}; - -} // namespace rviz_plugins - -#endif // TIER4_DEBUG_RVIZ_PLUGIN__FLOAT32_MULTI_ARRAY_STAMPED_PIE_CHART_HPP_ diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp deleted file mode 100644 index 37bf743e35f6a..0000000000000 --- a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp +++ /dev/null @@ -1,143 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// Copyright (c) 2014, JSK Lab -// All rights reserved. -// -// Software License Agreement (BSD License) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#ifndef TIER4_DEBUG_RVIZ_PLUGIN__JSK_OVERLAY_UTILS_HPP_ -#define TIER4_DEBUG_RVIZ_PLUGIN__JSK_OVERLAY_UTILS_HPP_ - -#include -#include -#include -#include -#include -#include - -#include -#include -// see OGRE/OgrePrerequisites.h -// #define OGRE_VERSION -// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) -#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) -#include -#include -#include -#include -#else -#include -#include -#include -#include -#include -#endif - -#include -#include -#include -#include -#include -#include - -namespace jsk_rviz_plugins -{ -class OverlayObject; - -class ScopedPixelBuffer -{ -public: - explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); - virtual ~ScopedPixelBuffer(); - virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); - virtual QImage getQImage(unsigned int width, unsigned int height); - virtual QImage getQImage(OverlayObject & overlay); - virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); - virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); - -protected: - Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; - -private: -}; - -// this is a class for put overlay object on rviz 3D panel. -// This class suppose to be instantiated in onInitialize method -// of rviz::Display class. -class OverlayObject -{ -public: - typedef std::shared_ptr Ptr; - - OverlayObject(Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name); - virtual ~OverlayObject(); - - virtual std::string getName(); - virtual void hide(); - virtual void show(); - virtual bool isTextureReady(); - virtual void updateTextureSize(unsigned int width, unsigned int height); - virtual ScopedPixelBuffer getBuffer(); - virtual void setPosition(double left, double top); - virtual void setDimensions(double width, double height); - virtual bool isVisible(); - virtual unsigned int getTextureWidth(); - virtual unsigned int getTextureHeight(); - -protected: - const std::string name_; - rclcpp::Logger logger_; - Ogre::Overlay * overlay_; - Ogre::PanelOverlayElement * panel_; - Ogre::MaterialPtr panel_material_; - Ogre::TexturePtr texture_; - -private: -}; - -// Ogre::Overlay* createOverlay(std::string name); -// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); -// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); -} // namespace jsk_rviz_plugins - -#endif // TIER4_DEBUG_RVIZ_PLUGIN__JSK_OVERLAY_UTILS_HPP_ diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp deleted file mode 100644 index 0960875d7eee2..0000000000000 --- a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp +++ /dev/null @@ -1,107 +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. - -// Copyright (c) 2014, JSK Lab -// All rights reserved. -// -// Software License Agreement (BSD License) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#ifndef TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ -#define TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ - -#include -#include - -#ifndef Q_MOC_RUN -#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" - -#include -#include -#include -#include - -#endif - -#include - -namespace rviz_plugins -{ -class StringStampedOverlayDisplay -: public rviz_common::RosTopicDisplay - -{ - Q_OBJECT - -public: - StringStampedOverlayDisplay(); - ~StringStampedOverlayDisplay() override; - - void onInitialize() override; - void onEnable() override; - void onDisable() override; - -private Q_SLOTS: - void updateVisualization(); - -protected: - void update(float wall_dt, float ros_dt) override; - void processMessage(const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override; - jsk_rviz_plugins::OverlayObject::Ptr overlay_; - rviz_common::properties::ColorProperty * property_text_color_; - rviz_common::properties::IntProperty * property_left_; - rviz_common::properties::IntProperty * property_top_; - rviz_common::properties::IntProperty * property_value_height_offset_; - rviz_common::properties::FloatProperty * property_value_scale_; - rviz_common::properties::IntProperty * property_font_size_; - rviz_common::properties::IntProperty * property_max_letter_num_; - // QImage hud_; - -private: - static constexpr int line_width_ = 2; - static constexpr int hand_width_ = 4; - - std::mutex mutex_; - tier4_debug_msgs::msg::StringStamped::ConstSharedPtr last_msg_ptr_; -}; -} // namespace rviz_plugins - -#endif // TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ diff --git a/common/tier4_debug_rviz_plugin/package.xml b/common/tier4_debug_rviz_plugin/package.xml deleted file mode 100644 index 45b73d5b9815d..0000000000000 --- a/common/tier4_debug_rviz_plugin/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - tier4_debug_rviz_plugin - 0.1.0 - The tier4_debug_rviz_plugin package - Takayuki Murooka - Apache License 2.0 - - ament_cmake - autoware_cmake - - libqt5-core - libqt5-gui - libqt5-widgets - qtbase5-dev - rclcpp - rviz_common - rviz_default_plugins - rviz_rendering - tier4_debug_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml deleted file mode 100644 index e18900afc8d84..0000000000000 --- a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - Display drivable area of tier4_debug_msgs::msg::Float32MultiArrayStamped - - - Display drivable area of tier4_debug_msgs::msg::StringStamped - - diff --git a/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp b/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp deleted file mode 100644 index 0187cc3e22de0..0000000000000 --- a/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp +++ /dev/null @@ -1,479 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// Copyright (c) 2014, JSK Lab -// All rights reserved. -// -// Software License Agreement (BSD License) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include -#include - -namespace rviz_plugins -{ - -Float32MultiArrayStampedPieChartDisplay::Float32MultiArrayStampedPieChartDisplay() -: rviz_common::Display(), update_required_(false), first_time_(true), data_(0.0) -{ - update_topic_property_ = new rviz_common::properties::StringProperty( - "Topic", "", "tier4_debug_msgs::msg::Float32MultiArrayStamped topic to subscribe to.", this, - SLOT(updateTopic()), this); - data_index_property_ = new rviz_common::properties::IntProperty( - "data index", 0, "data index in message to visualize", this, SLOT(updateDataIndex())); - size_property_ = new rviz_common::properties::IntProperty( - "size", 128, "size of the plotter window", this, SLOT(updateSize())); - left_property_ = new rviz_common::properties::IntProperty( - "left", 128, "left of the plotter window", this, SLOT(updateLeft())); - top_property_ = new rviz_common::properties::IntProperty( - "top", 128, "top of the plotter window", this, SLOT(updateTop())); - fg_color_property_ = new rviz_common::properties::ColorProperty( - "foreground color", QColor(25, 255, 240), "color to draw line", this, SLOT(updateFGColor())); - fg_alpha_property_ = new rviz_common::properties::FloatProperty( - "foreground alpha", 0.7, "alpha blending value for foreground", this, SLOT(updateFGAlpha())); - fg_alpha2_property_ = new rviz_common::properties::FloatProperty( - "foreground alpha 2", 0.4, "alpha blending value for foreground for indicator", this, - SLOT(updateFGAlpha2())); - bg_color_property_ = new rviz_common::properties::ColorProperty( - "background color", QColor(0, 0, 0), "background color", this, SLOT(updateBGColor())); - bg_alpha_property_ = new rviz_common::properties::FloatProperty( - "background alpha", 0.0, "alpha blending value for background", this, SLOT(updateBGAlpha())); - text_size_property_ = new rviz_common::properties::IntProperty( - "text size", 14, "text size", this, SLOT(updateTextSize())); - show_caption_property_ = new rviz_common::properties::BoolProperty( - "show caption", false, "show caption", this, SLOT(updateShowCaption())); - max_value_property_ = new rviz_common::properties::FloatProperty( - "max value", 1.0, "max value of pie chart", this, SLOT(updateMaxValue())); - min_value_property_ = new rviz_common::properties::FloatProperty( - "min value", 0.0, "min value of pie chart", this, SLOT(updateMinValue())); - auto_color_change_property_ = new rviz_common::properties::BoolProperty( - "auto color change", false, "change the color automatically", this, - SLOT(updateAutoColorChange())); - max_color_property_ = new rviz_common::properties::ColorProperty( - "max color", QColor(255, 0, 0), "only used if auto color change is set to True.", this, - SLOT(updateMaxColor())); - - med_color_property_ = new rviz_common::properties::ColorProperty( - "med color", QColor(255, 0, 0), "only used if auto color change is set to True.", this, - SLOT(updateMedColor())); - - max_color_threshold_property_ = new rviz_common::properties::FloatProperty( - "max color change threshold", 0, "change the max color at threshold", this, - SLOT(updateMaxColorThreshold())); - - med_color_threshold_property_ = new rviz_common::properties::FloatProperty( - "med color change threshold", 0, "change the med color at threshold ", this, - SLOT(updateMedColorThreshold())); - - clockwise_rotate_property_ = new rviz_common::properties::BoolProperty( - "clockwise rotate direction", false, "change the rotate direction", this, - SLOT(updateClockwiseRotate())); -} - -Float32MultiArrayStampedPieChartDisplay::~Float32MultiArrayStampedPieChartDisplay() -{ - if (overlay_->isVisible()) { - overlay_->hide(); - } - delete update_topic_property_; - delete fg_color_property_; - delete bg_color_property_; - delete fg_alpha_property_; - delete fg_alpha2_property_; - delete bg_alpha_property_; - delete top_property_; - delete left_property_; - delete size_property_; - delete min_value_property_; - delete max_value_property_; - delete max_color_property_; - delete med_color_property_; - delete text_size_property_; - delete show_caption_property_; -} - -void Float32MultiArrayStampedPieChartDisplay::onInitialize() -{ - static int count = 0; - rviz_common::UniformStringStream ss; - ss << "Float32MultiArrayStampedPieChart" << count++; - auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); - overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); - onEnable(); - updateSize(); - updateLeft(); - updateTop(); - updateFGColor(); - updateBGColor(); - updateFGAlpha(); - updateFGAlpha2(); - updateBGAlpha(); - updateMinValue(); - updateMaxValue(); - updateTextSize(); - updateShowCaption(); - updateAutoColorChange(); - updateMaxColor(); - updateMedColor(); - updateMaxColorThreshold(); - updateMedColorThreshold(); - updateClockwiseRotate(); - overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_); - overlay_->hide(); -} - -void Float32MultiArrayStampedPieChartDisplay::update( - [[maybe_unused]] float wall_dt, [[maybe_unused]] float ros_dt) -{ - if (update_required_) { - update_required_ = false; - overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_); - overlay_->setPosition(left_, top_); - overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); - drawPlot(data_); - } -} - -void Float32MultiArrayStampedPieChartDisplay::processMessage( - const tier4_debug_msgs::msg::Float32MultiArrayStamped::ConstSharedPtr msg) -{ - std::lock_guard lock(mutex_); - - if (!overlay_->isVisible()) { - return; - } - if (data_index_ < 0 || static_cast(msg->data.size()) <= data_index_) { - return; - } - - if (data_ != msg->data.at(data_index_) || first_time_) { - first_time_ = false; - data_ = msg->data.at(data_index_); - update_required_ = true; - } -} - -void Float32MultiArrayStampedPieChartDisplay::drawPlot(double val) -{ - QColor fg_color(fg_color_); - - if (auto_color_change_) { - double r = std::min(1.0, fabs((val - min_value_) / (max_value_ - min_value_))); - if (r > 0.6) { - double r2 = (r - 0.6) / 0.4; - fg_color.setRed((max_color_.red() - fg_color_.red()) * r2 + fg_color_.red()); - fg_color.setGreen((max_color_.green() - fg_color_.green()) * r2 + fg_color_.green()); - fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r2 + fg_color_.blue()); - } - if (max_color_threshold_ != 0) { - if (r > max_color_threshold_) { - fg_color.setRed(max_color_.red()); - fg_color.setGreen(max_color_.green()); - fg_color.setBlue(max_color_.blue()); - } - } - if (med_color_threshold_ != 0) { - if (max_color_threshold_ > r && r > med_color_threshold_) { - fg_color.setRed(med_color_.red()); - fg_color.setGreen(med_color_.green()); - fg_color.setBlue(med_color_.blue()); - } - } - } - - QColor fg_color2(fg_color); - QColor bg_color(bg_color_); - fg_color.setAlpha(fg_alpha_); - fg_color2.setAlpha(fg_alpha2_); - bg_color.setAlpha(bg_alpha_); - int width = overlay_->getTextureWidth(); - int height = overlay_->getTextureHeight(); - { - jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); - QImage Hud = buffer.getQImage(*overlay_, bg_color); - QPainter painter(&Hud); - painter.setRenderHint(QPainter::Antialiasing, true); - - const int outer_line_width = 5; - const int value_line_width = 10; - const int value_indicator_line_width = 2; - const int value_padding = 5; - - const int value_offset = outer_line_width + value_padding + value_line_width / 2; - - painter.setPen(QPen(fg_color, outer_line_width, Qt::SolidLine)); - - painter.drawEllipse( - outer_line_width / 2, outer_line_width / 2, width - outer_line_width, - height - outer_line_width - caption_offset_); - - painter.setPen(QPen(fg_color2, value_indicator_line_width, Qt::SolidLine)); - painter.drawEllipse( - value_offset, value_offset, width - value_offset * 2, - height - value_offset * 2 - caption_offset_); - - const double ratio = (val - min_value_) / (max_value_ - min_value_); - const double rotate_direction = clockwise_rotate_ ? -1.0 : 1.0; - const double ratio_angle = ratio * 360.0 * rotate_direction; - const double start_angle_offset = -90; - painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine)); - painter.drawArc( - QRectF( - value_offset, value_offset, width - value_offset * 2, - height - value_offset * 2 - caption_offset_), - start_angle_offset * 16, ratio_angle * 16); - QFont font = painter.font(); - font.setPointSize(text_size_); - font.setBold(true); - painter.setFont(font); - painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine)); - std::ostringstream s; - s << std::fixed << std::setprecision(2) << val; - painter.drawText( - 0, 0, width, height - caption_offset_, Qt::AlignCenter | Qt::AlignVCenter, s.str().c_str()); - - // caption - if (show_caption_) { - painter.drawText( - 0, height - caption_offset_, width, caption_offset_, Qt::AlignCenter | Qt::AlignVCenter, - getName()); - } - - // done - painter.end(); - // Unlock the pixel buffer - } -} - -void Float32MultiArrayStampedPieChartDisplay::subscribe() -{ - std::string topic_name = update_topic_property_->getStdString(); - - // NOTE: Remove all spaces since topic name filled with only spaces will crash - topic_name.erase(std::remove(topic_name.begin(), topic_name.end(), ' '), topic_name.end()); - - if (topic_name.length() > 0 && topic_name != "/") { - rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - sub_ = raw_node->create_subscription( - topic_name, 1, - std::bind( - &Float32MultiArrayStampedPieChartDisplay::processMessage, this, std::placeholders::_1)); - } -} - -void Float32MultiArrayStampedPieChartDisplay::unsubscribe() -{ - sub_.reset(); -} - -void Float32MultiArrayStampedPieChartDisplay::onEnable() -{ - subscribe(); - overlay_->show(); - first_time_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::onDisable() -{ - unsubscribe(); - overlay_->hide(); -} - -void Float32MultiArrayStampedPieChartDisplay::updateSize() -{ - std::lock_guard lock(mutex_); - - texture_size_ = size_property_->getInt(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateTop() -{ - top_ = top_property_->getInt(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateLeft() -{ - left_ = left_property_->getInt(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateBGColor() -{ - bg_color_ = bg_color_property_->getColor(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateFGColor() -{ - fg_color_ = fg_color_property_->getColor(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateFGAlpha() -{ - fg_alpha_ = fg_alpha_property_->getFloat() * 255.0; - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateFGAlpha2() -{ - fg_alpha2_ = fg_alpha2_property_->getFloat() * 255.0; - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateBGAlpha() -{ - bg_alpha_ = bg_alpha_property_->getFloat() * 255.0; - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateMinValue() -{ - min_value_ = min_value_property_->getFloat(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateMaxValue() -{ - max_value_ = max_value_property_->getFloat(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateTextSize() -{ - std::lock_guard lock(mutex_); - - text_size_ = text_size_property_->getInt(); - QFont font; - font.setPointSize(text_size_); - caption_offset_ = QFontMetrics(font).height(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateShowCaption() -{ - show_caption_ = show_caption_property_->getBool(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateTopic() -{ - unsubscribe(); - subscribe(); -} - -void Float32MultiArrayStampedPieChartDisplay::updateDataIndex() -{ - data_index_ = data_index_property_->getInt(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateAutoColorChange() -{ - auto_color_change_ = auto_color_change_property_->getBool(); - if (auto_color_change_) { - max_color_property_->show(); - med_color_property_->show(); - max_color_threshold_property_->show(); - med_color_threshold_property_->show(); - } else { - max_color_property_->hide(); - med_color_property_->hide(); - max_color_threshold_property_->hide(); - med_color_threshold_property_->hide(); - } - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateMaxColor() -{ - max_color_ = max_color_property_->getColor(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateMedColor() -{ - med_color_ = med_color_property_->getColor(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateMaxColorThreshold() -{ - max_color_threshold_ = max_color_threshold_property_->getFloat(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateMedColorThreshold() -{ - med_color_threshold_ = med_color_threshold_property_->getFloat(); - update_required_ = true; -} - -void Float32MultiArrayStampedPieChartDisplay::updateClockwiseRotate() -{ - clockwise_rotate_ = clockwise_rotate_property_->getBool(); - update_required_ = true; -} - -bool Float32MultiArrayStampedPieChartDisplay::isInRegion(int x, int y) -{ - return (top_ < y && top_ + texture_size_ > y && left_ < x && left_ + texture_size_ > x); -} - -void Float32MultiArrayStampedPieChartDisplay::movePosition(int x, int y) -{ - top_ = y; - left_ = x; -} - -void Float32MultiArrayStampedPieChartDisplay::setPosition(int x, int y) -{ - top_property_->setValue(y); - left_property_->setValue(x); -} -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::Float32MultiArrayStampedPieChartDisplay, rviz_common::Display) diff --git a/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp b/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp deleted file mode 100644 index b1933ebb3e157..0000000000000 --- a/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp +++ /dev/null @@ -1,225 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// Copyright (c) 2014, JSK Lab -// All rights reserved. -// -// Software License Agreement (BSD License) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include - -#include - -namespace jsk_rviz_plugins -{ -ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) -: pixel_buffer_(pixel_buffer) -{ - pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); -} - -ScopedPixelBuffer::~ScopedPixelBuffer() -{ - pixel_buffer_->unlock(); -} - -Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() -{ - return pixel_buffer_; -} - -QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) -{ - const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); - Ogre::uint8 * pDest = static_cast(pixelBox.data); - memset(pDest, 0, width * height); - return QImage(pDest, width, height, QImage::Format_ARGB32); -} - -QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) -{ - QImage Hud = getQImage(width, height); - for (unsigned int i = 0; i < width; i++) { - for (unsigned int j = 0; j < height; j++) { - Hud.setPixel(i, j, bg_color.rgba()); - } - } - return Hud; -} - -QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) -{ - return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); -} - -QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) -{ - return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); -} - -OverlayObject::OverlayObject( - Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name) -: name_(name), logger_(logger) -{ - rviz_rendering::RenderSystem::get()->prepareOverlays(manager); - std::string material_name = name_ + "Material"; - Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); - overlay_ = mOverlayMgr->create(name_); - panel_ = static_cast( - mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); - panel_->setMetricsMode(Ogre::GMM_PIXELS); - - panel_material_ = Ogre::MaterialManager::getSingleton().create( - material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - panel_->setMaterialName(panel_material_->getName()); - overlay_->add2D(panel_); -} - -OverlayObject::~OverlayObject() -{ - hide(); - panel_material_->unload(); - Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); - // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); - // mOverlayMgr->destroyOverlayElement(panel_); - // delete panel_; - // delete overlay_; -} - -std::string OverlayObject::getName() -{ - return name_; -} - -void OverlayObject::hide() -{ - if (overlay_->isVisible()) { - overlay_->hide(); - } -} - -void OverlayObject::show() -{ - if (!overlay_->isVisible()) { - overlay_->show(); - } -} - -bool OverlayObject::isTextureReady() -{ - return static_cast(texture_); -} - -void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) -{ - const std::string texture_name = name_ + "Texture"; - if (width == 0) { - RCLCPP_WARN(logger_, "width=0 is specified as texture size"); - width = 1; - } - if (height == 0) { - RCLCPP_WARN(logger_, "height=0 is specified as texture size"); - height = 1; - } - if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { - if (isTextureReady()) { - Ogre::TextureManager::getSingleton().remove(texture_name); - panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); - } - texture_ = Ogre::TextureManager::getSingleton().createManual( - texture_name, // name - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - Ogre::TEX_TYPE_2D, // type - width, height, // width & height of the render window - 0, // number of mipmaps - Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use - Ogre::TU_DEFAULT // usage - ); - panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); - - panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - } -} - -ScopedPixelBuffer OverlayObject::getBuffer() -{ - if (isTextureReady()) { - return ScopedPixelBuffer(texture_->getBuffer()); - } else { - return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); - } -} - -void OverlayObject::setPosition(double left, double top) -{ - panel_->setPosition(left, top); -} - -void OverlayObject::setDimensions(double width, double height) -{ - panel_->setDimensions(width, height); -} - -bool OverlayObject::isVisible() -{ - return overlay_->isVisible(); -} - -unsigned int OverlayObject::getTextureWidth() -{ - if (isTextureReady()) { - return texture_->getWidth(); - } else { - return 0; - } -} - -unsigned int OverlayObject::getTextureHeight() -{ - if (isTextureReady()) { - return texture_->getHeight(); - } else { - return 0; - } -} - -} // namespace jsk_rviz_plugins diff --git a/common/tier4_debug_rviz_plugin/src/string_stamped.cpp b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp deleted file mode 100644 index 538dc0cbe7069..0000000000000 --- a/common/tier4_debug_rviz_plugin/src/string_stamped.cpp +++ /dev/null @@ -1,198 +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. - -// Copyright (c) 2014, JSK Lab -// All rights reserved. -// -// Software License Agreement (BSD License) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include "tier4_debug_rviz_plugin/string_stamped.hpp" - -#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" - -#include -#include - -#include - -#include -#include -#include -#include - -namespace rviz_plugins -{ -StringStampedOverlayDisplay::StringStampedOverlayDisplay() -{ - const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); - - constexpr float hight_4k = 2160.0; - const float scale = static_cast(screen_info->height) / hight_4k; - const auto left = static_cast(std::round(1024 * scale)); - const auto top = static_cast(std::round(128 * scale)); - - property_text_color_ = new rviz_common::properties::ColorProperty( - "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); - property_left_ = new rviz_common::properties::IntProperty( - "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); - property_left_->setMin(0); - property_top_ = new rviz_common::properties::IntProperty( - "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); - property_top_->setMin(0); - - property_value_height_offset_ = new rviz_common::properties::IntProperty( - "Value height offset", 0, "Height offset of the plotter window", this, - SLOT(updateVisualization())); - property_font_size_ = new rviz_common::properties::IntProperty( - "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); - property_font_size_->setMin(1); - property_max_letter_num_ = new rviz_common::properties::IntProperty( - "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); - property_max_letter_num_->setMin(10); -} - -StringStampedOverlayDisplay::~StringStampedOverlayDisplay() -{ - if (initialized()) { - overlay_->hide(); - } -} - -void StringStampedOverlayDisplay::onInitialize() -{ - RTDClass::onInitialize(); - - static int count = 0; - rviz_common::UniformStringStream ss; - ss << "StringOverlayDisplayObject" << count++; - auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); - overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); - - overlay_->show(); - - const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); - overlay_->updateTextureSize(texture_size, texture_size); - overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); - overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); -} - -void StringStampedOverlayDisplay::onEnable() -{ - subscribe(); - overlay_->show(); -} - -void StringStampedOverlayDisplay::onDisable() -{ - unsubscribe(); - reset(); - overlay_->hide(); -} - -void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) -{ - (void)wall_dt; - (void)ros_dt; - - std::lock_guard message_lock(mutex_); - if (!last_msg_ptr_) { - return; - } - - // Display - QColor background_color; - background_color.setAlpha(0); - jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); - QImage hud = buffer.getQImage(*overlay_); - hud.fill(background_color); - - QPainter painter(&hud); - painter.setRenderHint(QPainter::Antialiasing, true); - - const int w = overlay_->getTextureWidth() - line_width_; - const int h = overlay_->getTextureHeight() - line_width_; - - // text - QColor text_color(property_text_color_->getColor()); - text_color.setAlpha(255); - painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); - QFont font = painter.font(); - font.setPixelSize(property_font_size_->getInt()); - font.setBold(true); - painter.setFont(font); - - // same as above, but align on right side - painter.drawText( - 0, std::min(property_value_height_offset_->getInt(), h - 1), w, - std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, - last_msg_ptr_->data.c_str()); - painter.end(); - updateVisualization(); -} - -void StringStampedOverlayDisplay::processMessage( - const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) -{ - if (!isEnabled()) { - return; - } - - { - std::lock_guard message_lock(mutex_); - last_msg_ptr_ = msg_ptr; - } - - queueRender(); -} - -void StringStampedOverlayDisplay::updateVisualization() -{ - const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); - overlay_->updateTextureSize(texture_size, texture_size); - overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); - overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::StringStampedOverlayDisplay, rviz_common::Display) diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index f2482b2d45a3b..1c1c4ad6d8ba1 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -11,6 +11,7 @@ ament_cmake_auto autoware_cmake + autoware_vehicle_info_utils geometry_msgs libqt5-core libqt5-gui @@ -20,7 +21,6 @@ rviz_common rviz_default_plugins tf2_ros - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp index c4233bd6305e3..cd32df498bfd4 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp @@ -172,7 +172,7 @@ void PoseHistoryFootprint::updateFootprint() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_THROTTLE( diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp index d957054bd479e..44625ad7deb41 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp @@ -15,8 +15,8 @@ #ifndef POSE_HISTORY_FOOTPRINT__DISPLAY_HPP_ #define POSE_HISTORY_FOOTPRINT__DISPLAY_HPP_ +#include #include -#include #include @@ -39,8 +39,8 @@ class BoolProperty; namespace rviz_plugins { -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfo; +using autoware::vehicle_info_utils::VehicleInfoUtils; class PoseHistoryFootprint : public rviz_common::MessageFilterDisplay diff --git a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt deleted file mode 100644 index cc7da7e322d19..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tier4_logging_level_configure_rviz_plugin) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) -set(CMAKE_AUTOMOC ON) -add_definitions(-DQT_NO_KEYWORDS) - -ament_auto_add_library(tier4_logging_level_configure_rviz_plugin SHARED - include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp - src/logging_level_configure.cpp -) - -target_link_libraries(tier4_logging_level_configure_rviz_plugin - ${QT_LIBRARIES} -) - -# Export the plugin to be imported by rviz2 -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - plugins - config -) diff --git a/common/tier4_logging_level_configure_rviz_plugin/README.md b/common/tier4_logging_level_configure_rviz_plugin/README.md deleted file mode 100644 index ed400e521e6aa..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/README.md +++ /dev/null @@ -1,72 +0,0 @@ -# tier4_logging_level_configure_rviz_plugin - -This package provides an rviz_plugin that can easily change the logger level of each node. - -![tier4_logging_level_configure_rviz_plugin](tier4_logging_level_configure_rviz_plugin.png) - -This plugin dispatches services to the "logger name" associated with "nodes" specified in YAML, adjusting the logger level. - -!!! Warning - - It is highly recommended to use this plugin when you're attempting to print any debug information. Furthermore, it is strongly advised to avoid using the logging level INFO, as it might flood the terminal with your information, potentially causing other useful information to be missed. - -!!! note - - To add your logger to the list, simply include the `node_name` and `logger_name` in the [logger_config.yaml](https://github.com/autowarefoundation/autoware.universe/blob/main/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml) under the corresponding component or module. If the relevant component or module is not listed, you may add them yourself. - -!!! note - - As of November 2023, in ROS 2 Humble, users are required to initiate a service server in the node to use this feature. (This might be integrated into ROS standards in the future.) For easy service server generation, you can use the [LoggerLevelConfigure](https://github.com/autowarefoundation/autoware.universe/blob/main/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp) utility. - -## How to use the plugin - -In RVIZ2, go to Panels and add LoggingLevelConfigureRVizPlugin. Then, search for the node you're interested in and select the corresponding logging level to print the logs. - -## How to add or find your logger name - -Because there are no available ROS 2 CLI commands to list loggers, there isn't a straightforward way to check your logger name. Additionally, the following assumes that you already know which node you're working with. - -### For logger as a class member variable - -If your class doesn't have an `rclcpp::Logger` member variable, you can start by including one yourself: - -```c++ -mutable rclcpp::Logger logger_; -``` - -If your node already has a logger, it should, under normal circumstances, be similar to the node's name. - -For instance, if the node name is `/some_component/some_node/node_child`, the `logger_name` would be `some_component.some_node.node_child`. - -Should your log not print as expected, one approach is to initially set your logging level in the code to info, like so: - -```c++ -RCLCPP_INFO(logger_, "Print something here."); -``` - -This will result in something like the following being printed in the terminal: - -```shell -[component_container_mt-36] [INFO 1711949149.735437551] [logger_name]: Print something here. (func() at /path/to/code:line_number) -``` - -Afterward, you can simply copy the `logger_name`. - -!!! warning - - Remember to revert your code to the appropriate logging level after testing. - ```c++ - RCLCPP_DEBUG(logger_, "Print something here."); - ``` - -### For libraries - -When dealing with libraries, such as utility functions, you may need to add the logger manually. Here's an example: - -```c++ -RCLCPP_WARN( - rclcpp::get_logger("some_component").get_child("some_child").get_child("some_child2"), - "Print something here."); -``` - -In this scenario, the `logger_name` would be `some_component.some_child.some_child2`. diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml deleted file mode 100644 index 6b1214b133af4..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ /dev/null @@ -1,132 +0,0 @@ -# logger_config.yaml - -# ============================================================ -# localization -# ============================================================ -Localization: - ndt_scan_matcher: - - node_name: /localization/pose_estimator/ndt_scan_matcher - logger_name: localization.pose_estimator.ndt_scan_matcher - - gyro_odometer: - - node_name: /localization/twist_estimator/gyro_odometer - logger_name: localization.twist_estimator.gyro_odometer - - pose_initializer: - - node_name: /localization/util/pose_initializer_node - logger_name: localization.util.pose_initializer_node - - ekf_localizer: - - node_name: /localization/pose_twist_fusion_filter/ekf_localizer - logger_name: localization.pose_twist_fusion_filter.ekf_localizer - - localization_error_monitor: - - node_name: /localization/localization_error_monitor - logger_name: localization.localization_error_monitor - -# ============================================================ -# planning -# ============================================================ -Planning: - behavior_path_planner: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: tier4_autoware_utils - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: behavior_path_planner.path_shifter - - behavior_path_planner_avoidance: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance.utils - - behavior_path_planner_goal_planner: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.goal_planner - - behavior_path_planner_start_planner: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.start_planner - - behavior_path_avoidance_by_lane_change: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: lane_change.AVOIDANCE_BY_LANE_CHANGE - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: lane_change.utils - - behavior_path_lane_change: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: lane_change.NORMAL - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: lane_change.utils - - behavior_velocity_planner: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner - logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner - logger_name: tier4_autoware_utils - - behavior_velocity_planner_intersection: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner - logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.intersection - - motion_obstacle_avoidance: - - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner - logger_name: planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner - - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner - logger_name: tier4_autoware_utils - - motion_velocity_smoother: - - node_name: /planning/scenario_planning/motion_velocity_smoother - logger_name: planning.scenario_planning.motion_velocity_smoother - - node_name: /planning/scenario_planning/motion_velocity_smoother - logger_name: tier4_autoware_utils - - route_handler: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: route_handler - -# ============================================================ -# control -# ============================================================ -Control: - lateral_controller: - - node_name: /control/trajectory_follower/controller_node_exe - logger_name: control.trajectory_follower.controller_node_exe.lateral_controller - - node_name: /control/trajectory_follower/controller_node_exe - logger_name: tier4_autoware_utils - - longitudinal_controller: - - node_name: /control/trajectory_follower/controller_node_exe - logger_name: control.trajectory_follower.controller_node_exe.longitudinal_controller - - node_name: /control/trajectory_follower/controller_node_exe - logger_name: tier4_autoware_utils - - vehicle_cmd_gate: - - node_name: /control/vehicle_cmd_gate - logger_name: control.vehicle_cmd_gate - - node_name: /control/vehicle_cmd_gate - logger_name: tier4_autoware_utils - -# ============================================================ -# common -# ============================================================ - -Common: - tier4_autoware_utils: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: tier4_autoware_utils - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner - logger_name: tier4_autoware_utils - - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner - logger_name: tier4_autoware_utils - - node_name: /planning/scenario_planning/lane_driving/motion_planning/path_smoother - logger_name: tier4_autoware_utils - - node_name: /planning/scenario_planning/motion_velocity_smoother - logger_name: tier4_autoware_utils - - node_name: /control/trajectory_follower/controller_node_exe - logger_name: tier4_autoware_utils - - node_name: /control/vehicle_cmd_gate - logger_name: tier4_autoware_utils diff --git a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp deleted file mode 100644 index 37d70b494c74e..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp +++ /dev/null @@ -1,90 +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 TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ -#define TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ - -#include "logging_demo/srv/config_logger.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace rviz_plugin -{ -struct LoggerInfo -{ - QString node_name; - QString logger_name; -}; -struct ButtonInfo -{ - QString button_name; - std::vector logger_info_vec; -}; -struct LoggerNamespaceInfo -{ - QString ns; // group namespace - std::vector button_info_vec; -}; -class LoggingLevelConfigureRvizPlugin : public rviz_common::Panel -{ - Q_OBJECT // This macro is needed for Qt to handle slots and signals - - public : LoggingLevelConfigureRvizPlugin(QWidget * parent = nullptr); - void onInitialize() override; - void save(rviz_common::Config config) const override; - void load(const rviz_common::Config & config) override; - -private: - QMap buttonGroups_; - rclcpp::Node::SharedPtr raw_node_; - - std::vector display_info_vec_; - - // client_map_[node_name] = service_client - std::unordered_map::SharedPtr> - client_map_; - - // button_map_[button_name][logging_level] = Q_button_pointer - std::unordered_map> button_map_; - - QStringList getNodeList(); - int getMaxModuleNameWidth(QLabel * containerLabel); - void setLoggerNodeMap(); - - ButtonInfo getButtonInfoFromNamespace(const QString & ns); - std::vector getNodeLoggerNameFromButtonName(const QString button_name); - -private Q_SLOTS: - void onButtonClick(QPushButton * button, const QString & name, const QString & level); - void updateButtonColors( - const QString & target_module_name, QPushButton * active_button, const QString & level); - void changeLogLevel(const QString & container, const QString & level); -}; - -} // namespace rviz_plugin - -#endif // TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ diff --git a/common/tier4_logging_level_configure_rviz_plugin/package.xml b/common/tier4_logging_level_configure_rviz_plugin/package.xml deleted file mode 100644 index 7849e6049a077..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - tier4_logging_level_configure_rviz_plugin - 0.1.0 - The tier4_logging_level_configure_rviz_plugin package - Takamasa Horibe - Satoshi Ota - Kosuke Takeuchi - Apache License 2.0 - - ament_cmake - autoware_cmake - - libqt5-core - libqt5-gui - libqt5-widgets - logging_demo - qtbase5-dev - rclcpp - rviz_common - rviz_default_plugins - rviz_rendering - yaml-cpp - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml b/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml deleted file mode 100644 index ce5cbd13fc131..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - tier4_logging_level_configure_rviz_plugin - - diff --git a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp deleted file mode 100644 index 72ecf361c96d5..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp +++ /dev/null @@ -1,258 +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 "yaml-cpp/yaml.h" - -#include -#include -#include -#include -#include -#include - -#include - -namespace rviz_plugin -{ - -LoggingLevelConfigureRvizPlugin::LoggingLevelConfigureRvizPlugin(QWidget * parent) -: rviz_common::Panel(parent) -{ -} - -void LoggingLevelConfigureRvizPlugin::onInitialize() -{ - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - setLoggerNodeMap(); - - QVBoxLayout * mainLayout = new QVBoxLayout; - - QStringList levels = {"DEBUG", "INFO", "WARN", "ERROR", "FATAL"}; - - constexpr int height = 20; - - // Iterate over the namespaces - for (const auto & ns_group_info : display_info_vec_) { - // Create a group box for each namespace - QGroupBox * groupBox = new QGroupBox(ns_group_info.ns); - QVBoxLayout * groupLayout = new QVBoxLayout; - - // Iterate over the node/logger pairs within this namespace - for (const auto & button_info : ns_group_info.button_info_vec) { - const auto & button_name = button_info.button_name; - - QHBoxLayout * hLayout = new QHBoxLayout; - - // Create a QLabel to display the node name - QLabel * label = new QLabel(button_name); - label->setFixedHeight(height); - label->setFixedWidth(getMaxModuleNameWidth(label)); - - hLayout->addWidget(label); - - // Create a button group for each node - QButtonGroup * buttonGroup = new QButtonGroup(this); - - // Create buttons for each logging level - for (const QString & level : levels) { - QPushButton * button = new QPushButton(level); - button->setFixedHeight(height); - hLayout->addWidget(button); - buttonGroup->addButton(button); - button_map_[button_name][level] = button; - connect(button, &QPushButton::clicked, this, [this, button, button_name, level]() { - this->onButtonClick(button, button_name, level); - }); - } - - // Set the "INFO" button as checked by default and change its color - updateButtonColors(button_name, button_map_[button_name]["INFO"], "INFO"); - - buttonGroups_[button_name] = buttonGroup; - groupLayout->addLayout(hLayout); // Add the horizontal layout to the group layout - } - - groupBox->setLayout(groupLayout); // Set the group layout to the group box - mainLayout->addWidget(groupBox); // Add the group box to the main layout - } - - // Create a QWidget to hold the main layout - QWidget * containerWidget = new QWidget; - containerWidget->setLayout(mainLayout); - - // Create a QScrollArea to make the layout scrollable - QScrollArea * scrollArea = new QScrollArea; - scrollArea->setWidget(containerWidget); - scrollArea->setWidgetResizable(true); - - // Set the QScrollArea as the layout of the main widget - QVBoxLayout * scrollLayout = new QVBoxLayout; - scrollLayout->addWidget(scrollArea); - setLayout(scrollLayout); - - // Setup service clients - const auto & nodes = getNodeList(); - for (const QString & node : nodes) { - const auto client = raw_node_->create_client( - node.toStdString() + "/config_logger"); - client_map_[node] = client; - } -} - -// Calculate the maximum width among all target_module_name. -int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * label) -{ - int max_width = 0; - QFontMetrics metrics(label->font()); - for (const auto & ns_info : display_info_vec_) { - for (const auto & b : ns_info.button_info_vec) { - max_width = std::max(metrics.horizontalAdvance(b.button_name), max_width); - } - } - return max_width; -} - -// create node list in node_logger_map_ without -QStringList LoggingLevelConfigureRvizPlugin::getNodeList() -{ - QStringList nodes; - for (const auto & d : display_info_vec_) { - for (const auto & b : d.button_info_vec) { - for (const auto & info : b.logger_info_vec) { - if (!nodes.contains(info.node_name)) { - nodes.append(info.node_name); - } - } - } - } - return nodes; -} - -// Modify the signature of the onButtonClick function: -void LoggingLevelConfigureRvizPlugin::onButtonClick( - QPushButton * button, const QString & target_module_name, const QString & level) -{ - if (button) { - const auto callback = - [&](rclcpp::Client::SharedFuture future) { - std::cerr << "change logging level: " - << std::string(future.get()->success ? "success!" : "failed...") << std::endl; - }; - - const auto node_logger_vec = getNodeLoggerNameFromButtonName(target_module_name); - for (const auto & data : node_logger_vec) { - const auto req = std::make_shared(); - - req->logger_name = data.logger_name.toStdString(); - req->level = level.toStdString(); - std::cerr << "logger level of " << req->logger_name << " is set to " << req->level - << std::endl; - client_map_[data.node_name]->async_send_request(req, callback); - } - - updateButtonColors( - target_module_name, button, level); // Modify updateButtonColors to accept QPushButton only. - } -} - -void LoggingLevelConfigureRvizPlugin::updateButtonColors( - const QString & target_module_name, QPushButton * active_button, const QString & level) -{ - std::unordered_map colorMap = { - {"DEBUG", "rgb(181, 255, 20)"}, /* green */ - {"INFO", "rgb(200, 255, 255)"}, /* light blue */ - {"WARN", "rgb(255, 255, 0)"}, /* yellow */ - {"ERROR", "rgb(255, 0, 0)"}, /* red */ - {"FATAL", "rgb(139, 0, 0)"}, /* dark red */ - {"OFF", "rgb(211, 211, 211)"} /* gray */ - }; - - const QString LIGHT_GRAY_TEXT = "rgb(180, 180, 180)"; - - const QString color = colorMap.count(level) ? colorMap[level] : colorMap["OFF"]; - - for (const auto & button : button_map_[target_module_name]) { - if (button.second == active_button) { - button.second->setStyleSheet("background-color: " + color + "; color: black"); - } else { - button.second->setStyleSheet( - "background-color: " + colorMap["OFF"] + "; color: " + LIGHT_GRAY_TEXT); - } - } -} -void LoggingLevelConfigureRvizPlugin::save(rviz_common::Config config) const -{ - Panel::save(config); -} - -void LoggingLevelConfigureRvizPlugin::load(const rviz_common::Config & config) -{ - Panel::load(config); -} - -void LoggingLevelConfigureRvizPlugin::setLoggerNodeMap() -{ - const std::string package_share_directory = - ament_index_cpp::get_package_share_directory("tier4_logging_level_configure_rviz_plugin"); - const std::string default_config_path = package_share_directory + "/config/logger_config.yaml"; - - const auto filename = - raw_node_->declare_parameter("config_filename", default_config_path); - RCLCPP_INFO(raw_node_->get_logger(), "load config file: %s", filename.c_str()); - - YAML::Node config = YAML::LoadFile(filename); - - for (YAML::const_iterator it = config.begin(); it != config.end(); ++it) { - const auto ns = QString::fromStdString(it->first.as()); - const YAML::Node ns_config = it->second; - - LoggerNamespaceInfo display_data; - display_data.ns = ns; - - for (YAML::const_iterator ns_it = ns_config.begin(); ns_it != ns_config.end(); ++ns_it) { - const auto key = QString::fromStdString(ns_it->first.as()); - ButtonInfo button_data; - button_data.button_name = key; - const YAML::Node values = ns_it->second; - for (size_t i = 0; i < values.size(); i++) { - LoggerInfo data; - data.node_name = QString::fromStdString(values[i]["node_name"].as()); - data.logger_name = QString::fromStdString(values[i]["logger_name"].as()); - button_data.logger_info_vec.push_back(data); - } - display_data.button_info_vec.push_back(button_data); - } - display_info_vec_.push_back(display_data); - } -} - -std::vector LoggingLevelConfigureRvizPlugin::getNodeLoggerNameFromButtonName( - const QString button_name) -{ - for (const auto & ns_level : display_info_vec_) { - for (const auto & button : ns_level.button_info_vec) { - if (button.button_name == button_name) { - return button.logger_info_vec; - } - } - } - RCLCPP_ERROR( - raw_node_->get_logger(), "Failed to find target name: %s", button_name.toStdString().c_str()); - return {}; -} - -} // namespace rviz_plugin -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugin::LoggingLevelConfigureRvizPlugin, rviz_common::Panel) diff --git a/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png b/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png deleted file mode 100644 index a93aff724bb19..0000000000000 Binary files a/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png and /dev/null differ diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp index 4fc6aeb71771d..7dbbb65c94560 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp @@ -53,8 +53,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class CarInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp index fc40b08be822b..c1ec65a0488bd 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -82,8 +82,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class InteractiveObject diff --git a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp index 195ec8e2a3a6d..642159aceaf3d 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp @@ -53,8 +53,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class PedestrianInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp index 2bc3129b51321..3f1a9b55c30ab 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp @@ -48,8 +48,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class UnknownInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_planning_rviz_plugin/README.md b/common/tier4_planning_rviz_plugin/README.md index ef9df25f2b657..5b6286139af0d 100644 --- a/common/tier4_planning_rviz_plugin/README.md +++ b/common/tier4_planning_rviz_plugin/README.md @@ -11,11 +11,11 @@ This plugin displays the path, trajectory, and maximum speed. ### Input -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------------- | ------------------------------------------ | -| `/input/path` | `autoware_auto_planning_msgs::msg::Path` | The topic on which to subscribe path | -| `/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | The topic on which to subscribe trajectory | -| `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs/msg/VelocityLimit` | The topic on which to publish max velocity | +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------------- | ------------------------------------------ | +| `/input/path` | `autoware_planning_msgs::msg::Path` | The topic on which to subscribe path | +| `/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | The topic on which to subscribe trajectory | +| `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs/msg/VelocityLimit` | The topic on which to publish max velocity | ### Output 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 66b4a31f0993f..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 @@ -17,9 +17,9 @@ #include "tier4_planning_rviz_plugin/path/display_base.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -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); } @@ -188,7 +188,7 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay }; class AutowarePathWithLaneIdDisplay -: public AutowarePathWithDrivableAreaDisplay +: public AutowarePathWithDrivableAreaDisplay { Q_OBJECT public: @@ -198,9 +198,9 @@ class AutowarePathWithLaneIdDisplay private: void preProcessMessageDetail() override; void preVisualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; void visualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) override; rviz_common::properties::BoolProperty property_lane_id_view_; @@ -212,7 +212,7 @@ class AutowarePathWithLaneIdDisplay }; class AutowarePathDisplay -: public AutowarePathWithDrivableAreaDisplay +: public AutowarePathWithDrivableAreaDisplay { Q_OBJECT private: @@ -220,7 +220,7 @@ class AutowarePathDisplay }; class AutowareTrajectoryDisplay -: public AutowarePathBaseDisplay +: public AutowarePathBaseDisplay { Q_OBJECT private: 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 4a59006e3bb96..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 @@ -15,6 +15,7 @@ #ifndef TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ #define TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ +#include #include #include #include @@ -25,9 +26,8 @@ #include #include #include -#include -#include +#include #include #include @@ -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; } } @@ -97,8 +97,8 @@ bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) namespace rviz_plugins { -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfo; +using autoware::vehicle_info_utils::VehicleInfoUtils; template class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay { @@ -316,6 +316,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->detachAllObjects(); node->removeAndDestroyAllChildren(); this->scene_manager_->destroySceneNode(node); + + rviz_rendering::MovableText * text = velocity_texts_.at(i); + delete text; } velocity_texts_.resize(msg_ptr->points.size()); velocity_text_nodes_.resize(msg_ptr->points.size()); @@ -339,6 +342,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->detachAllObjects(); node->removeAndDestroyAllChildren(); this->scene_manager_->destroySceneNode(node); + + rviz_rendering::MovableText * text = slope_texts_.at(i); + delete text; } slope_texts_.resize(msg_ptr->points.size()); slope_text_nodes_.resize(msg_ptr->points.size()); @@ -352,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()) { @@ -448,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; @@ -460,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; @@ -504,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 4af4a371ef651..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_auto_planning_msgs + 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,9 +25,7 @@ rviz_rendering tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_planning_msgs - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml index 0a80327199606..dac8c41670f3c 100644 --- a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml @@ -2,12 +2,12 @@ - Display path points of autoware_auto_planning_msg::Path + Display path points of autoware_planning_msg::Path - Display path_with_lane_id of autoware_auto_planning_msg::PathWithLaneId + Display path_with_lane_id of autoware_planning_msg::PathWithLaneId - Display trajectory points of autoware_auto_planning_msg::Trajectory + Display trajectory points of autoware_planning_msg::Trajectory ( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( @@ -51,7 +51,7 @@ AutowarePathWithLaneIdDisplay::~AutowarePathWithLaneIdDisplay() } void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) { const size_t size = msg_ptr->points.size(); // clear previous text @@ -73,8 +73,7 @@ void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail( } void AutowarePathWithLaneIdDisplay::visualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, - const size_t p_idx) + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) { const auto & point = msg_ptr->points.at(p_idx); @@ -108,7 +107,7 @@ void AutowarePathDisplay::preProcessMessageDetail() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( @@ -125,7 +124,7 @@ void AutowareTrajectoryDisplay::preProcessMessageDetail() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( diff --git a/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt b/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt deleted file mode 100644 index 5b6e205bafed5..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tier4_screen_capture_rviz_plugin) - -find_package(autoware_cmake REQUIRED) -autoware_package() -find_package(OpenCV REQUIRED) -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) -set(CMAKE_AUTOMOC ON) -set(CMAKE_INCLUDE_CURRENT_DIR ON) - -include_directories( - ${OpenCV_INCLUDE_DIRS} -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/screen_capture_panel.hpp - src/screen_capture_panel.cpp -) -target_link_libraries(${PROJECT_NAME} - ${QT_LIBRARIES} - ${OpenCV_LIBRARIES} -) -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - plugins -) diff --git a/common/tier4_screen_capture_rviz_plugin/README.md b/common/tier4_screen_capture_rviz_plugin/README.md deleted file mode 100644 index d16c19c343017..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/README.md +++ /dev/null @@ -1,23 +0,0 @@ -# tier4_screen_capture_rviz_plugin - -## Purpose - -This plugin captures the screen of rviz. - -## Interface - -| Name | Type | Description | -| ---------------------------- | ------------------------ | ---------------------------------- | -| `/debug/capture/video` | `std_srvs::srv::Trigger` | Trigger to start screen capturing. | -| `/debug/capture/screen_shot` | `std_srvs::srv::Trigger` | Trigger to capture screen shot. | - -## Assumptions / Known limits - -This is only for debug or analyze. -The `capture screen` button is still beta version which can slow frame rate. -set lower frame rate according to PC spec. - -## Usage - -1. Start rviz and select panels/Add new panel. - ![select_panel](./images/select_panels.png) diff --git a/common/tier4_screen_capture_rviz_plugin/images/select_panels.png b/common/tier4_screen_capture_rviz_plugin/images/select_panels.png deleted file mode 100644 index a691602c42c3c..0000000000000 Binary files a/common/tier4_screen_capture_rviz_plugin/images/select_panels.png and /dev/null differ diff --git a/common/tier4_screen_capture_rviz_plugin/package.xml b/common/tier4_screen_capture_rviz_plugin/package.xml deleted file mode 100644 index 180bf9eedc9ca..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - tier4_screen_capture_rviz_plugin - 0.0.0 - The screen capture package - Taiki, Tanaka - Satoshi Ota - Kyoichi Sugahara - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - libopencv-dev - libqt5-core - libqt5-gui - libqt5-widgets - qtbase5-dev - rclcpp - rviz_common - rviz_rendering - std_srvs - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/common/tier4_screen_capture_rviz_plugin/plugins/plugin_description.xml b/common/tier4_screen_capture_rviz_plugin/plugins/plugin_description.xml deleted file mode 100644 index fdf105d646497..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - AutowareScreenCapturePanel - - - diff --git a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp deleted file mode 100644 index cad828903e0d3..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp +++ /dev/null @@ -1,220 +0,0 @@ -// Copyright 2021 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 "screen_capture_panel.hpp" - -#include - -#include -#include -#include - -using std::placeholders::_1; -using std::placeholders::_2; - -void setFormatDate(QLabel * line, double time) -{ - char buffer[128]; - auto seconds = static_cast(time); - strftime(buffer, sizeof(buffer), "%Y-%m-%d-%H-%M-%S", localtime(&seconds)); - line->setText(QString("- ") + QString(buffer)); -} - -AutowareScreenCapturePanel::AutowareScreenCapturePanel(QWidget * parent) -: rviz_common::Panel(parent) -{ - std::filesystem::create_directory("capture"); - auto * v_layout = new QVBoxLayout; - // screen capture - auto * cap_layout = new QHBoxLayout; - { - ros_time_label_ = new QLabel; - screen_capture_button_ptr_ = new QPushButton("Capture Screen Shot"); - connect(screen_capture_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickScreenCapture())); - file_name_prefix_ = new QLineEdit("cap"); - connect(file_name_prefix_, SIGNAL(valueChanged(std::string)), this, SLOT(onPrefixChanged())); - cap_layout->addWidget(screen_capture_button_ptr_); - cap_layout->addWidget(file_name_prefix_); - cap_layout->addWidget(ros_time_label_); - // initialize file name system clock is better for identification. - setFormatDate(ros_time_label_, rclcpp::Clock().now().seconds()); - } - - // video capture - auto * video_cap_layout = new QHBoxLayout; - { - capture_to_mp4_button_ptr_ = new QPushButton("Capture Screen"); - connect(capture_to_mp4_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVideoCapture())); - capture_hz_ = new QSpinBox(); - capture_hz_->setRange(1, 10); - capture_hz_->setValue(10); - capture_hz_->setSingleStep(1); - connect(capture_hz_, SIGNAL(valueChanged(int)), this, SLOT(onRateChanged())); - // video cap layout - video_cap_layout->addWidget(capture_to_mp4_button_ptr_); - video_cap_layout->addWidget(capture_hz_); - video_cap_layout->addWidget(new QLabel(" [Hz]")); - } - - // consider layout - { - v_layout->addLayout(cap_layout); - v_layout->addLayout(video_cap_layout); - setLayout(v_layout); - } - auto * timer = new QTimer(this); - connect(timer, &QTimer::timeout, this, &AutowareScreenCapturePanel::update); - timer->start(1000); - capture_timer_ = new QTimer(this); - connect(capture_timer_, &QTimer::timeout, this, &AutowareScreenCapturePanel::onTimer); - state_ = State::WAITING_FOR_CAPTURE; -} - -void AutowareScreenCapturePanel::onCaptureVideoTrigger( - [[maybe_unused]] const std_srvs::srv::Trigger::Request::SharedPtr req, - const std_srvs::srv::Trigger::Response::SharedPtr res) -{ - onClickVideoCapture(); - res->success = true; - res->message = stateToString(state_); -} - -void AutowareScreenCapturePanel::onCaptureScreenShotTrigger( - [[maybe_unused]] const std_srvs::srv::Trigger::Request::SharedPtr req, - const std_srvs::srv::Trigger::Response::SharedPtr res) -{ - onClickScreenCapture(); - res->success = true; - res->message = stateToString(state_); -} - -void AutowareScreenCapturePanel::onInitialize() -{ - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - capture_video_srv_ = raw_node_->create_service( - "/debug/capture/video", - std::bind(&AutowareScreenCapturePanel::onCaptureVideoTrigger, this, _1, _2)); - capture_screen_shot_srv_ = raw_node_->create_service( - "/debug/capture/screen_shot", - std::bind(&AutowareScreenCapturePanel::onCaptureScreenShotTrigger, this, _1, _2)); -} - -void onPrefixChanged() -{ -} - -void AutowareScreenCapturePanel::onRateChanged() -{ -} - -void AutowareScreenCapturePanel::onClickScreenCapture() -{ - const std::string time_text = - "capture/" + file_name_prefix_->text().toStdString() + ros_time_label_->text().toStdString(); - getDisplayContext()->getViewManager()->getRenderPanel()->getRenderWindow()->captureScreenShot( - time_text + ".png"); -} - -void AutowareScreenCapturePanel::onClickVideoCapture() -{ - const int clock = static_cast(1e3 / capture_hz_->value()); - try { - const QWidgetList top_level_widgets = QApplication::topLevelWidgets(); - for (QWidget * widget : top_level_widgets) { - auto * main_window_candidate = qobject_cast(widget); - if (main_window_candidate) { - main_window_ = main_window_candidate; - } - } - } catch (...) { - return; - } - if (!main_window_) return; - switch (state_) { - case State::WAITING_FOR_CAPTURE: - // initialize setting - { - capture_file_name_ = ros_time_label_->text().toStdString(); - } - capture_to_mp4_button_ptr_->setText("capturing rviz screen"); - capture_to_mp4_button_ptr_->setStyleSheet("background-color: #FF0000;"); - { - int fourcc = cv::VideoWriter::fourcc('h', '2', '6', '4'); // mp4 - QScreen * screen = QGuiApplication::primaryScreen(); - const auto q_size = screen->grabWindow(main_window_->winId()) - .toImage() - .convertToFormat(QImage::Format_RGB888) - .rgbSwapped() - .size(); - current_movie_size_ = cv::Size(q_size.width(), q_size.height()); - writer_.open( - "capture/" + file_name_prefix_->text().toStdString() + capture_file_name_ + ".mp4", - fourcc, capture_hz_->value(), current_movie_size_); - } - capture_timer_->start(clock); - state_ = State::CAPTURING; - break; - case State::CAPTURING: - writer_.release(); - capture_timer_->stop(); - capture_to_mp4_button_ptr_->setText("waiting for capture"); - capture_to_mp4_button_ptr_->setStyleSheet("background-color: #00FF00;"); - state_ = State::WAITING_FOR_CAPTURE; - break; - } -} - -void AutowareScreenCapturePanel::onTimer() -{ - if (!main_window_) return; - // this is deprecated but only way to capture nicely - QScreen * screen = QGuiApplication::primaryScreen(); - QPixmap original_pixmap = screen->grabWindow(main_window_->winId()); - const auto q_image = - original_pixmap.toImage().convertToFormat(QImage::Format_RGB888).rgbSwapped(); - const int h = q_image.height(); - const int w = q_image.width(); - cv::Size size = cv::Size(w, h); - cv::Mat image( - size, CV_8UC3, const_cast(q_image.bits()), - static_cast(q_image.bytesPerLine())); - if (size != current_movie_size_) { - cv::Mat new_image; - cv::resize(image, new_image, current_movie_size_); - writer_.write(new_image); - } else { - writer_.write(image); - } - cv::waitKey(0); -} - -void AutowareScreenCapturePanel::update() -{ - setFormatDate(ros_time_label_, rclcpp::Clock().now().seconds()); -} - -void AutowareScreenCapturePanel::save(rviz_common::Config config) const -{ - Panel::save(config); -} - -void AutowareScreenCapturePanel::load(const rviz_common::Config & config) -{ - Panel::load(config); -} - -AutowareScreenCapturePanel::~AutowareScreenCapturePanel() = default; - -#include -PLUGINLIB_EXPORT_CLASS(AutowareScreenCapturePanel, rviz_common::Panel) diff --git a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp deleted file mode 100644 index 5c4d16f57da82..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2021 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 SCREEN_CAPTURE_PANEL_HPP_ -#define SCREEN_CAPTURE_PANEL_HPP_ - -// Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// rviz -#include -#include -#include -#include -#include -#include -#include - -// ros -#include - -#include -#include -#include - -class QLineEdit; - -class AutowareScreenCapturePanel : public rviz_common::Panel -{ - Q_OBJECT - -public: - explicit AutowareScreenCapturePanel(QWidget * parent = nullptr); - ~AutowareScreenCapturePanel() override; - void update(); - void onInitialize() override; - void createWallTimer(); - void onTimer(); - void save(rviz_common::Config config) const override; - void load(const rviz_common::Config & config) override; - void onCaptureVideoTrigger( - const std_srvs::srv::Trigger::Request::SharedPtr req, - const std_srvs::srv::Trigger::Response::SharedPtr res); - void onCaptureScreenShotTrigger( - const std_srvs::srv::Trigger::Request::SharedPtr req, - const std_srvs::srv::Trigger::Response::SharedPtr res); - -public Q_SLOTS: - void onClickScreenCapture(); - void onClickVideoCapture(); - void onPrefixChanged(); - void onRateChanged(); - -private: - QLabel * ros_time_label_; - QPushButton * screen_capture_button_ptr_; - QPushButton * capture_to_mp4_button_ptr_; - QLineEdit * file_name_prefix_; - QSpinBox * capture_hz_; - QTimer * capture_timer_; - QMainWindow * main_window_{nullptr}; - enum class State { WAITING_FOR_CAPTURE, CAPTURING }; - State state_; - std::string capture_file_name_; - bool is_capture_{false}; - cv::VideoWriter writer_; - cv::Size current_movie_size_; - std::vector image_vec_; - - static std::string stateToString(const State & state) - { - if (state == State::WAITING_FOR_CAPTURE) { - return "waiting for capture"; - } - if (state == State::CAPTURING) { - return "capturing"; - } - return ""; - } - -protected: - rclcpp::Service::SharedPtr capture_video_srv_; - rclcpp::Service::SharedPtr capture_screen_shot_srv_; - rclcpp::Node::SharedPtr raw_node_; -}; - -#endif // SCREEN_CAPTURE_PANEL_HPP_ diff --git a/common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt b/common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt deleted file mode 100644 index 020766340016e..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tier4_simulated_clock_rviz_plugin) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) -set(CMAKE_AUTOMOC ON) -set(CMAKE_INCLUDE_CURRENT_DIR ON) -add_definitions(-DQT_NO_KEYWORDS) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/simulated_clock_panel.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${QT_LIBRARIES} -) - -# Export the plugin to be imported by rviz2 -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - icons - plugins -) diff --git a/common/tier4_simulated_clock_rviz_plugin/README.md b/common/tier4_simulated_clock_rviz_plugin/README.md deleted file mode 100644 index a6218f32b988c..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/README.md +++ /dev/null @@ -1,46 +0,0 @@ -# tier4_simulated_clock_rviz_plugin - -## Purpose - -This plugin allows publishing and controlling the simulated ROS time. - -## Output - -| Name | Type | Description | -| -------- | --------------------------- | -------------------------- | -| `/clock` | `rosgraph_msgs::msg::Clock` | the current simulated time | - -## How to use the plugin - -1. Launch [planning simulator](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/#1-launch-autoware) with `use_sim_time:=true`. - - ```shell - ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit use_sim_time:=true - ``` - - > Warning - > If you launch the planning simulator without adding the `tier4_simulated_clock_rviz_plugin`, your simulation will not be running. You'll not even be able to place the initial and the goal poses. - -2. Start rviz and select panels/Add new panel. - - ![select_panel](./images/select_panels.png) - -3. Select tier4_clock_rviz_plugin/SimulatedClock and press OK. - - ![select_clock_plugin](./images/select_clock_plugin.png) - -4. Use the added panel to control how the simulated clock is published. - - ![use_clock_plugin](./images/use_clock_plugin.png) - -
    -
  1. Pause button: pause/resume the clock.
    2. -
  2. Speed: speed of the clock relative to the system clock.
    3. -
  3. Rate: publishing rate of the clock.
    4. -
  4. Step button: advance the clock by the specified time step.
    5. -
  5. Time step: value used to advance the clock when pressing the step button d).
    6. -
  6. Time unit: time unit associated with the value from e).
    7. -
- - > Warning - > If you set the time step too large, your simulation will go haywire. diff --git a/common/tier4_simulated_clock_rviz_plugin/icons/classes/SimulatedClockPanel.png b/common/tier4_simulated_clock_rviz_plugin/icons/classes/SimulatedClockPanel.png deleted file mode 100644 index 9431c2d422363..0000000000000 Binary files a/common/tier4_simulated_clock_rviz_plugin/icons/classes/SimulatedClockPanel.png and /dev/null differ diff --git a/common/tier4_simulated_clock_rviz_plugin/images/select_clock_plugin.png b/common/tier4_simulated_clock_rviz_plugin/images/select_clock_plugin.png deleted file mode 100644 index 8bf5e3a903751..0000000000000 Binary files a/common/tier4_simulated_clock_rviz_plugin/images/select_clock_plugin.png and /dev/null differ diff --git a/common/tier4_simulated_clock_rviz_plugin/images/select_panels.png b/common/tier4_simulated_clock_rviz_plugin/images/select_panels.png deleted file mode 100644 index a691602c42c3c..0000000000000 Binary files a/common/tier4_simulated_clock_rviz_plugin/images/select_panels.png and /dev/null differ diff --git a/common/tier4_simulated_clock_rviz_plugin/images/use_clock_plugin.png b/common/tier4_simulated_clock_rviz_plugin/images/use_clock_plugin.png deleted file mode 100644 index 39c3669c2ea31..0000000000000 Binary files a/common/tier4_simulated_clock_rviz_plugin/images/use_clock_plugin.png and /dev/null differ diff --git a/common/tier4_simulated_clock_rviz_plugin/package.xml b/common/tier4_simulated_clock_rviz_plugin/package.xml deleted file mode 100644 index d80b18a5895b0..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - tier4_simulated_clock_rviz_plugin - 0.0.1 - Rviz plugin to publish and control the /clock topic - Maxime CLEMENT - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - libqt5-core - libqt5-gui - libqt5-widgets - qtbase5-dev - rclcpp - rosgraph_msgs - rviz_common - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/common/tier4_simulated_clock_rviz_plugin/plugins/plugin_description.xml b/common/tier4_simulated_clock_rviz_plugin/plugins/plugin_description.xml deleted file mode 100644 index 00caf2e5d49e0..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - Panel that publishes a simulated clock to the /clock topic and provides an interface to pause the clock and modify its speed. - - - diff --git a/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.cpp b/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.cpp deleted file mode 100644 index ad698d925fdff..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.cpp +++ /dev/null @@ -1,153 +0,0 @@ -// -// Copyright 2022 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 "simulated_clock_panel.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace rviz_plugins -{ -SimulatedClockPanel::SimulatedClockPanel(QWidget * parent) : rviz_common::Panel(parent) -{ - pause_button_ = new QPushButton("Pause"); - pause_button_->setToolTip("Freeze ROS time."); - pause_button_->setCheckable(true); - - publishing_rate_input_ = new QSpinBox(); - publishing_rate_input_->setRange(1, 1000); - publishing_rate_input_->setSingleStep(1); - publishing_rate_input_->setValue(100); - publishing_rate_input_->setSuffix("Hz"); - - clock_speed_input_ = new QDoubleSpinBox(); - clock_speed_input_->setRange(0.0, 10.0); - clock_speed_input_->setSingleStep(0.1); - clock_speed_input_->setValue(1.0); - clock_speed_input_->setSuffix(" X real time"); - - step_button_ = new QPushButton("Step"); - step_button_->setToolTip("Pause and steps the simulation clock"); - step_time_input_ = new QSpinBox(); - step_time_input_->setRange(1, 999); - step_time_input_->setValue(1); - step_unit_combo_ = new QComboBox(); - step_unit_combo_->addItems({"s", "ms", "µs", "ns"}); - - auto * layout = new QGridLayout(this); - auto * step_layout = new QHBoxLayout(); - auto * clock_layout = new QHBoxLayout(); - auto * clock_box = new QWidget(); - auto * step_box = new QWidget(); - clock_box->setLayout(clock_layout); - step_box->setLayout(step_layout); - layout->addWidget(pause_button_, 0, 0); - layout->addWidget(step_button_, 1, 0); - clock_layout->addWidget(new QLabel("Speed:")); - clock_layout->addWidget(clock_speed_input_); - clock_layout->addWidget(new QLabel("Rate:")); - clock_layout->addWidget(publishing_rate_input_); - step_layout->addWidget(step_time_input_); - step_layout->addWidget(step_unit_combo_); - layout->addWidget(clock_box, 0, 1, 1, 2); - layout->addWidget(step_box, 1, 1, 1, 2); - layout->setContentsMargins(0, 0, 20, 0); - prev_published_time_ = std::chrono::system_clock::now(); - - connect(publishing_rate_input_, SIGNAL(valueChanged(int)), this, SLOT(onRateChanged(int))); - connect(step_button_, SIGNAL(clicked()), this, SLOT(onStepClicked())); -} - -void SimulatedClockPanel::onInitialize() -{ - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - clock_pub_ = raw_node_->create_publisher("/clock", rclcpp::QoS(1)); - createWallTimer(); -} - -void SimulatedClockPanel::onRateChanged(int new_rate) -{ - (void)new_rate; - pub_timer_->cancel(); - createWallTimer(); -} - -void SimulatedClockPanel::onStepClicked() -{ - using std::chrono::duration_cast, std::chrono::seconds, std::chrono::milliseconds, - std::chrono::microseconds, std::chrono::nanoseconds; - pause_button_->setChecked(true); - const auto step_time = step_time_input_->value(); - const auto unit = step_unit_combo_->currentText(); - nanoseconds step_duration_ns{}; - if (unit == "s") { - step_duration_ns += duration_cast(seconds(step_time)); - } else if (unit == "ms") { - step_duration_ns += duration_cast(milliseconds(step_time)); - } else if (unit == "µs") { - step_duration_ns += duration_cast(microseconds(step_time)); - } else if (unit == "ns") { - step_duration_ns += duration_cast(nanoseconds(step_time)); - } - addTimeToClock(step_duration_ns); -} - -void SimulatedClockPanel::createWallTimer() -{ - // convert rate from Hz to milliseconds - const auto period = - std::chrono::milliseconds(static_cast(1e3 / publishing_rate_input_->value())); - pub_timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); }); -} - -void SimulatedClockPanel::onTimer() -{ - if (!pause_button_->isChecked()) { - const auto duration_since_prev_clock = std::chrono::system_clock::now() - prev_published_time_; - const auto speed_adjusted_duration = duration_since_prev_clock * clock_speed_input_->value(); - addTimeToClock(std::chrono::duration_cast(speed_adjusted_duration)); - } - clock_pub_->publish(clock_msg_); - prev_published_time_ = std::chrono::system_clock::now(); -} - -void SimulatedClockPanel::addTimeToClock(std::chrono::nanoseconds time_to_add_ns) -{ - constexpr auto one_sec = std::chrono::seconds(1); - constexpr auto one_sec_ns = std::chrono::nanoseconds(one_sec); - while (time_to_add_ns >= one_sec) { - time_to_add_ns -= one_sec; - clock_msg_.clock.sec += 1; - } - clock_msg_.clock.nanosec += time_to_add_ns.count(); - if (clock_msg_.clock.nanosec >= one_sec_ns.count()) { - clock_msg_.clock.sec += 1; - clock_msg_.clock.nanosec = clock_msg_.clock.nanosec - one_sec_ns.count(); - } -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::SimulatedClockPanel, rviz_common::Panel) diff --git a/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.hpp b/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.hpp deleted file mode 100644 index b2ac332107202..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.hpp +++ /dev/null @@ -1,76 +0,0 @@ -// -// Copyright 2022 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 SIMULATED_CLOCK_PANEL_HPP_ -#define SIMULATED_CLOCK_PANEL_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -namespace rviz_plugins -{ -class SimulatedClockPanel : public rviz_common::Panel -{ - Q_OBJECT - -public: - explicit SimulatedClockPanel(QWidget * parent = nullptr); - void onInitialize() override; - -protected Q_SLOTS: - /// @brief callback for when the publishing rate is changed - void onRateChanged(int new_rate); - /// @brief callback for when the step button is clicked - void onStepClicked(); - -protected: - /// @brief creates ROS wall timer to periodically call onTimer() - void createWallTimer(); - void onTimer(); - /// @brief add some time to the clock - /// @input ns time to add in nanoseconds - void addTimeToClock(std::chrono::nanoseconds ns); - - // ROS - rclcpp::Node::SharedPtr raw_node_; - rclcpp::Publisher::SharedPtr clock_pub_; - rclcpp::TimerBase::SharedPtr pub_timer_; - - // GUI - QPushButton * pause_button_; - QPushButton * step_button_; - QSpinBox * publishing_rate_input_; - QDoubleSpinBox * clock_speed_input_; - QSpinBox * step_time_input_; - QComboBox * step_unit_combo_; - - // Clocks - std::chrono::time_point prev_published_time_; - rosgraph_msgs::msg::Clock clock_msg_; -}; - -} // namespace rviz_plugins - -#endif // SIMULATED_CLOCK_PANEL_HPP_ diff --git a/common/tier4_state_rviz_plugin/CMakeLists.txt b/common/tier4_state_rviz_plugin/CMakeLists.txt index afe21f66291b2..6b569ddb6d162 100644 --- a/common/tier4_state_rviz_plugin/CMakeLists.txt +++ b/common/tier4_state_rviz_plugin/CMakeLists.txt @@ -13,8 +13,32 @@ add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED src/autoware_state_panel.cpp src/velocity_steering_factors_panel.cpp + src/custom_toggle_switch.cpp + src/custom_slider.cpp + src/custom_container.cpp + src/custom_button.cpp + src/custom_icon_label.cpp + src/custom_segmented_button.cpp + src/custom_segmented_button_item.cpp + src/custom_label.cpp + src/include/material_colors.hpp + src/include/autoware_state_panel.hpp + src/include/custom_button.hpp + src/include/custom_container.hpp + src/include/custom_icon_label.hpp + src/include/custom_label.hpp + src/include/custom_segmented_button_item.hpp + src/include/custom_segmented_button.hpp + src/include/custom_slider.hpp + src/include/custom_toggle_switch.hpp + src/include/velocity_steering_factors_panel.hpp ) +target_include_directories( + ${PROJECT_NAME} PUBLIC +) + + target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ) @@ -22,6 +46,9 @@ target_link_libraries(${PROJECT_NAME} # Export the plugin to be imported by rviz2 pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + + + ament_auto_package( INSTALL_TO_SHARE icons diff --git a/common/tier4_state_rviz_plugin/README.md b/common/tier4_state_rviz_plugin/README.md index ac1e188fb36cd..e45be3bea13dc 100644 --- a/common/tier4_state_rviz_plugin/README.md +++ b/common/tier4_state_rviz_plugin/README.md @@ -16,7 +16,7 @@ This plugin also can engage from the panel. | `/api/localization/initialization_state` | `autoware_adapi_v1_msgs::msg::LocalizationInitializationState` | The topic represents the state of localization initialization | | `/api/motion/state` | `autoware_adapi_v1_msgs::msg::MotionState` | The topic represents the state of motion | | `/api/autoware/get/emergency` | `tier4_external_api_msgs::msg::Emergency` | The topic represents the state of external emergency | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic represents the state of gear | +| `/vehicle/status/gear_status` | `autoware_vehicle_msgs::msg::GearReport` | The topic represents the state of gear | ### Output diff --git a/common/tier4_state_rviz_plugin/icons/assets/active.png b/common/tier4_state_rviz_plugin/icons/assets/active.png new file mode 100644 index 0000000000000..db9c81211abd5 Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/active.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/crash.png b/common/tier4_state_rviz_plugin/icons/assets/crash.png new file mode 100644 index 0000000000000..18175a823ae4a Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/crash.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/danger.png b/common/tier4_state_rviz_plugin/icons/assets/danger.png new file mode 100644 index 0000000000000..baed346deea2d Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/danger.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/none.png b/common/tier4_state_rviz_plugin/icons/assets/none.png new file mode 100644 index 0000000000000..c44f9f485dbf1 Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/none.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/pending.png b/common/tier4_state_rviz_plugin/icons/assets/pending.png new file mode 100644 index 0000000000000..3925162878fd5 Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/pending.png differ diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 06d57bd947af3..65ccd217d8016 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -6,14 +6,14 @@ The autoware state rviz plugin package Hiroki OTA Takagi, Isamu + Khalil Selyan Apache License 2.0 ament_cmake_auto autoware_cmake autoware_adapi_v1_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_vehicle_msgs libqt5-core libqt5-gui libqt5-widgets diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index ba961b9ac5b00..f9fb7d3dd3958 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -14,15 +14,13 @@ // limitations under the License. // -#include "autoware_state_panel.hpp" +#include "include/autoware_state_panel.hpp" -#include -#include -#include -#include -#include #include +#include +#include + #include #include @@ -35,181 +33,59 @@ namespace rviz_plugins { AutowareStatePanel::AutowareStatePanel(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())); + // Panel Configuration + this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); // 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 * AutowareStatePanel::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 * AutowareStatePanel::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 * AutowareStatePanel::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 * AutowareStatePanel::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 * AutowareStatePanel::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 * AutowareStatePanel::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; + // Create a new container widget + QWidget * containerWidget = new QWidget(this); + containerWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + containerWidget->setStyleSheet( + QString("QWidget { background-color: %1; color: %2; }") + .arg(autoware::state_rviz_plugin::colors::default_colors.background.c_str()) + .arg(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + + auto * containerLayout = new QVBoxLayout(containerWidget); + // Set the alignment of the layout + containerLayout->setAlignment(Qt::AlignTop); + containerLayout->setSpacing(1); + + auto * operation_mode_group = makeOperationModeGroup(); + auto * diagnostic_v_layout = new QVBoxLayout; + auto * localization_group = makeLocalizationGroup(); + auto * motion_group = makeMotionGroup(); + auto * fail_safe_group = makeFailSafeGroup(); + auto * routing_group = makeRoutingGroup(); + auto * velocity_limit_group = makeVelocityLimitGroup(); + // auto * diagnostic_group = makeDiagnosticGroup(); + + diagnostic_v_layout->addLayout(routing_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(localization_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(motion_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(fail_safe_group); + + // containerLayout->addLayout(diagnostic_group); + + containerLayout->addLayout(operation_mode_group); + // containerLayout->addSpacing(5); + containerLayout->addLayout(diagnostic_v_layout); + // main_v_layout->addSpacing(5); + containerLayout->addLayout(velocity_limit_group); + + // Create a QScrollArea + QScrollArea * scrollArea = new QScrollArea(this); + scrollArea->setWidgetResizable(true); + scrollArea->setWidget(containerWidget); + + // Main layout for AutowareStatePanel + QVBoxLayout * mainLayout = new QVBoxLayout(this); + mainLayout->addWidget(scrollArea); + setLayout(mainLayout); } void AutowareStatePanel::onInitialize() @@ -268,9 +144,9 @@ void AutowareStatePanel::onInitialize() "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onMRMState, this, _1)); - // Others - sub_gear_ = raw_node_->create_subscription( - "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1)); + // // Diagnostics + // sub_diag_ = raw_node_->create_subscription( + // "/diagnostics", 10, std::bind(&AutowareStatePanel::onDiagnostics, this, _1)); sub_emergency_ = raw_node_->create_subscription( "/api/autoware/get/emergency", 10, std::bind(&AutowareStatePanel::onEmergencyStatus, this, _1)); @@ -280,174 +156,498 @@ void AutowareStatePanel::onInitialize() pub_velocity_limit_ = raw_node_->create_publisher( "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); -} -void AutowareStatePanel::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); + QObject::connect(segmented_button, &CustomSegmentedButton::buttonClicked, this, [this](int id) { + const QList buttons = segmented_button->getButtonGroup()->buttons(); + + // Check if the button ID is within valid range + if (id < 0 || id >= buttons.size()) { + return; + } + + // Ensure the button is not null + QAbstractButton * abstractButton = segmented_button->getButtonGroup()->button(id); + if (!abstractButton) { + return; + } + + QPushButton * button = qobject_cast(abstractButton); + if (button) { + // Call the corresponding function for each button + if (button == auto_button_ptr_) { + onClickAutonomous(); + } else if (button == local_button_ptr_) { + onClickLocal(); + } else if (button == remote_button_ptr_) { + onClickRemote(); + } else if (button == stop_button_ptr_) { + onClickStop(); + } } else { - deactivateButton(button); + // qDebug() << "Button not found with ID:" << id; } - }; + }); +} - QString text = ""; - QString style_sheet = ""; - // Operation Mode - switch (msg->mode) { - case OperationModeState::AUTONOMOUS: - text = "AUTONOMOUS"; - style_sheet = "background-color: #00FF00;"; // green - break; +QVBoxLayout * AutowareStatePanel::makeOperationModeGroup() +{ + control_mode_switch_ptr_ = new CustomToggleSwitch(this); + connect( + control_mode_switch_ptr_, &QCheckBox::stateChanged, this, + &AutowareStatePanel::onSwitchStateChanged); + + control_mode_label_ptr_ = new QLabel("Autoware Control"); + control_mode_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + CustomContainer * group1 = new CustomContainer(this); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(control_mode_switch_ptr_); + horizontal_layout->addWidget(control_mode_label_ptr_); + + // add switch and label to the container + group1->setContentsMargins(0, 0, 0, 10); + group1->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + + // Create the CustomSegmentedButton + segmented_button = new CustomSegmentedButton(this); + auto_button_ptr_ = segmented_button->addButton("Auto"); + local_button_ptr_ = segmented_button->addButton("Local"); + remote_button_ptr_ = segmented_button->addButton("Remote"); + stop_button_ptr_ = segmented_button->addButton("Stop"); + + QVBoxLayout * groupLayout = new QVBoxLayout; + // set these widgets to show up at the left and not stretch more than needed + groupLayout->setAlignment(Qt::AlignCenter); + groupLayout->setContentsMargins(10, 0, 0, 0); + groupLayout->addWidget(group1); + // groupLayout->addSpacing(5); + groupLayout->addWidget(segmented_button, 0, Qt::AlignCenter); + return groupLayout; +} - case OperationModeState::LOCAL: - text = "LOCAL"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; +QVBoxLayout * AutowareStatePanel::makeRoutingGroup() +{ + auto * group = new QVBoxLayout; - case OperationModeState::REMOTE: - text = "REMOTE"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; + auto * custom_container = new CustomContainer(this); - case OperationModeState::STOP: - text = "STOP"; - style_sheet = "background-color: #FFA500;"; // orange - break; + routing_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } + clear_route_button_ptr_ = new CustomElevatedButton("Clear Route"); + clear_route_button_ptr_->setCheckable(true); + clear_route_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); - if (msg->is_in_transition) { - text += "\n(TRANSITION)"; - } + routing_label_ptr_ = new QLabel("Routing | Unknown"); + routing_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); - updateLabel(operation_mode_label_ptr_, text, style_sheet); + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); - // 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 - } + horizontal_layout->addWidget(routing_icon); + horizontal_layout->addWidget(routing_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(clear_route_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + + return group; +} - // 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); +QVBoxLayout * AutowareStatePanel::makeLocalizationGroup() +{ + auto * group = new QVBoxLayout; + auto * custom_container = new CustomContainer(this); + + init_by_gnss_button_ptr_ = new CustomElevatedButton("Initialize with GNSS"); + init_by_gnss_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); + + localization_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + localization_label_ptr_ = new QLabel("Localization | Unknown"); + localization_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(localization_icon); + horizontal_layout->addWidget(localization_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(init_by_gnss_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + return group; +} + +QVBoxLayout * AutowareStatePanel::makeMotionGroup() +{ + auto * group = new QVBoxLayout; + auto * custom_container = new CustomContainer(this); + + accept_start_button_ptr_ = new CustomElevatedButton("Accept Start"); + accept_start_button_ptr_->setCheckable(true); + accept_start_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); + + motion_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + motion_label_ptr_ = new QLabel("Motion | Unknown"); + motion_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + horizontal_layout->setAlignment(Qt::AlignLeft); + + horizontal_layout->addWidget(motion_icon); + horizontal_layout->addWidget(motion_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(accept_start_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + + return group; +} + +QVBoxLayout * AutowareStatePanel::makeFailSafeGroup() +{ + auto * group = new QVBoxLayout; + auto * v_layout = new QVBoxLayout; + auto * custom_container1 = new CustomContainer(this); + auto * custom_container2 = new CustomContainer(this); + + mrm_state_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + mrm_behavior_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + + mrm_state_label_ptr_ = new QLabel("MRM State | Unknown"); + mrm_behavior_label_ptr_ = new QLabel("MRM Behavior | Unknown"); + + // change text color + mrm_state_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + mrm_behavior_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(mrm_state_icon); + horizontal_layout->addWidget(mrm_state_label_ptr_); + + custom_container1->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + + auto * horizontal_layout2 = new QHBoxLayout; + horizontal_layout2->setSpacing(10); + horizontal_layout2->setContentsMargins(0, 0, 0, 0); + + horizontal_layout2->addWidget(mrm_behavior_icon); + horizontal_layout2->addWidget(mrm_behavior_label_ptr_); + + custom_container2->getLayout()->addLayout(horizontal_layout2, 0, 0, 1, 1, Qt::AlignLeft); + + v_layout->addWidget(custom_container1); + // v_layout->addSpacing(5); + v_layout->addWidget(custom_container2); + + group->setContentsMargins(10, 0, 0, 0); + + group->addLayout(v_layout); + return group; +} + +/* QVBoxLayout * AutowareStatePanel::makeDiagnosticGroup() +{ + auto * group = new QVBoxLayout; + + // Create the scroll area + QScrollArea * scrollArea = new QScrollArea; + scrollArea->setFixedHeight(66); // Adjust the height as needed + scrollArea->setWidgetResizable(true); + scrollArea->setHorizontalScrollBarPolicy(Qt::ScrollBarAsNeeded); + scrollArea->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); + + // Create a widget to contain the layout + QWidget * scrollAreaWidgetContents = new QWidget; + // use layout to contain the diagnostic label and the diagnostic level + diagnostic_layout_ = new QVBoxLayout(); + diagnostic_layout_->setSpacing(5); // Set space between items + diagnostic_layout_->setContentsMargins(5, 5, 5, 5); // Set margins within the layout + + // Add a QLabel to display the title of what this is + auto * tsm_label_title_ptr_ = new QLabel("Topic State Monitor: "); + // Set the layout on the widget + scrollAreaWidgetContents->setLayout(diagnostic_layout_); + + // Set the widget on the scroll area + scrollArea->setWidget(scrollAreaWidgetContents); + + group->addWidget(tsm_label_title_ptr_); + group->addWidget(scrollArea); + + return group; +} */ + +QVBoxLayout * AutowareStatePanel::makeVelocityLimitGroup() +{ + // Velocity Limit + velocity_limit_setter_ptr_ = new QLabel("Set Velocity Limit"); + // set its width to fit the text + velocity_limit_setter_ptr_->setFixedWidth( + velocity_limit_setter_ptr_->fontMetrics().horizontalAdvance("Set Velocity Limit")); + + velocity_limit_value_label_ = new QLabel("0"); + velocity_limit_value_label_->setMaximumWidth( + velocity_limit_value_label_->fontMetrics().horizontalAdvance("0")); + + CustomSlider * pub_velocity_limit_slider_ = new CustomSlider(Qt::Horizontal); + pub_velocity_limit_slider_->setRange(0, 100); + pub_velocity_limit_slider_->setValue(0); + pub_velocity_limit_slider_->setMaximumWidth(300); + + connect(pub_velocity_limit_slider_, &QSlider::sliderPressed, this, [this]() { + sliderIsDragging = true; // User starts dragging the handle + }); + + connect(pub_velocity_limit_slider_, &QSlider::sliderReleased, this, [this]() { + sliderIsDragging = false; // User finished dragging + onClickVelocityLimit(); // Call when handle is released after dragging + }); + + connect(pub_velocity_limit_slider_, &QSlider::valueChanged, this, [this](int value) { + this->velocity_limit_value_label_->setText(QString::number(value)); + velocity_limit_value_label_->setMaximumWidth( + velocity_limit_value_label_->fontMetrics().horizontalAdvance(QString::number(value))); + if (!sliderIsDragging) { // If the value changed without dragging, it's a click on the track + onClickVelocityLimit(); // Call the function immediately since it's not a drag operation + } + }); + + // Emergency Button + emergency_button_ptr_ = new CustomElevatedButton("Set Emergency"); + + emergency_button_ptr_->setCursor(Qt::PointingHandCursor); + // set fixed width to fit the text + connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); + auto * utility_layout = new QVBoxLayout; + auto * velocity_limit_layout = new QHBoxLayout; + QLabel * velocity_limit_label = new QLabel("km/h"); + + velocity_limit_layout->addWidget(pub_velocity_limit_slider_); + velocity_limit_layout->addSpacing(5); + velocity_limit_layout->addWidget(velocity_limit_value_label_); + velocity_limit_layout->addWidget(velocity_limit_label); + + // Velocity Limit layout + utility_layout->addSpacing(15); + utility_layout->addWidget(velocity_limit_setter_ptr_); + utility_layout->addSpacing(10); + utility_layout->addLayout(velocity_limit_layout); + utility_layout->addSpacing(25); + utility_layout->addWidget(emergency_button_ptr_); + + utility_layout->setContentsMargins(15, 0, 15, 0); + + return utility_layout; +} + +void AutowareStatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + auto updateButtonState = []( + CustomSegmentedButtonItem * button, bool is_available, + uint8_t current_mode, uint8_t desired_mode, bool disable) { + bool is_checked = (current_mode == desired_mode); + button->setHovered(false); + + button->setActivated(is_checked); + button->setChecked(is_checked); + button->setDisabledButton(disable || !is_available); + button->setCheckableButton(!disable && is_available && !is_checked); + }; + + bool disable_buttons = msg->is_in_transition; + + updateButtonState( + auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS, + disable_buttons); + updateButtonState( + stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP, + disable_buttons); + updateButtonState( + local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL, + disable_buttons); + updateButtonState( + remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE, + disable_buttons); + + // toggle switch for control mode + auto changeToggleSwitchState = [](CustomToggleSwitch * toggle_switch, const bool is_enabled) { + // Flick the switch without triggering its function + bool old_state = toggle_switch->blockSignals(true); + toggle_switch->setCheckedState(!is_enabled); + toggle_switch->blockSignals(old_state); + }; + + if (!msg->is_in_transition) { + // would cause an on/off/on flicker if in transition + changeToggleSwitchState(control_mode_switch_ptr_, !msg->is_autoware_control_enabled); + } } void AutowareStatePanel::onRoute(const RouteState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString route_state = "Routing | Unknown"; + switch (msg->state) { case RouteState::UNSET: - text = "UNSET"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + route_state = "Routing | Unset"; break; case RouteState::SET: - text = "SET"; - style_sheet = "background-color: #00FF00;"; // green + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + route_state = "Routing | Set"; break; case RouteState::ARRIVED: - text = "ARRIVED"; - style_sheet = "background-color: #FFA500;"; // orange + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + route_state = "Routing | Arrived"; break; case RouteState::CHANGING: - text = "CHANGING"; - style_sheet = "background-color: #FFFF00;"; // yellow + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + state = Pending; + route_state = "Routing | Changing"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(routing_label_ptr_, text, style_sheet); + routing_icon->updateStyle(state, bgColor); + routing_label_ptr_->setText(route_state); if (msg->state == RouteState::SET) { activateButton(clear_route_button_ptr_); } else { + clear_route_button_ptr_->setStyleSheet( + QString("QPushButton {" + "background-color: %1;color: %2;" + "border: 2px solid %3;" + "font-weight: bold;" + "}") + .arg(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()) + .arg(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()) + .arg( + autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str())); deactivateButton(clear_route_button_ptr_); } } void AutowareStatePanel::onLocalization(const LocalizationInitializationState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString localization_state = "Localization | Unknown"; + switch (msg->state) { case LocalizationInitializationState::UNINITIALIZED: - text = "UNINITIALIZED"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + localization_state = "Localization | Uninitialized"; break; - case LocalizationInitializationState::INITIALIZING: - text = "INITIALIZING"; - style_sheet = "background-color: #FFA500;"; // orange + case LocalizationInitializationState::INITIALIZED: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + localization_state = "Localization | Initialized"; break; - case LocalizationInitializationState::INITIALIZED: - text = "INITIALIZED"; - style_sheet = "background-color: #00FF00;"; // green + case LocalizationInitializationState::INITIALIZING: + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + localization_state = "Localization | Initializing"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(localization_label_ptr_, text, style_sheet); + localization_icon->updateStyle(state, bgColor); + localization_label_ptr_->setText(localization_state); } void AutowareStatePanel::onMotion(const MotionState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString motion_state = "Motion | Unknown"; + switch (msg->state) { case MotionState::STARTING: - text = "STARTING"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + motion_state = "Motion | Starting"; break; - case MotionState::STOPPED: - text = "STOPPED"; - style_sheet = "background-color: #FFA500;"; // orange + case MotionState::MOVING: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + motion_state = "Motion | Moving"; break; - case MotionState::MOVING: - text = "MOVING"; - style_sheet = "background-color: #00FF00;"; // green + case MotionState::STOPPED: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + motion_state = "Motion | Stopped"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(motion_label_ptr_, text, style_sheet); + motion_icon->updateStyle(state, bgColor); + motion_label_ptr_->setText(motion_state); if (msg->state == MotionState::STARTING) { activateButton(accept_start_button_ptr_); @@ -458,94 +658,85 @@ void AutowareStatePanel::onMotion(const MotionState::ConstSharedPtr msg) void AutowareStatePanel::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; + IconState state; + QColor bgColor; + QString mrm_state = "MRM State | Unknown"; - case MRMState::MRM_OPERATING: - text = "MRM_OPERATING"; - style_sheet = "background-color: #FFA500;"; // orange - break; + switch (msg->state) { + case MRMState::NONE: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Inactive"; + break; - case MRMState::MRM_SUCCEEDED: - text = "MRM_SUCCEEDED"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; + case MRMState::MRM_OPERATING: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Operating"; + break; - case MRMState::MRM_FAILED: - text = "MRM_FAILED"; - style_sheet = "background-color: #FF0000;"; // red - break; + case MRMState::MRM_SUCCEEDED: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + mrm_state = "MRM State | Successful"; + break; - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } + case MRMState::MRM_FAILED: + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + mrm_state = "MRM State | Failed"; + break; - updateLabel(mrm_state_label_ptr_, text, style_sheet); + default: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Unknown"; + break; } + mrm_state_icon->updateStyle(state, bgColor); + mrm_state_label_ptr_->setText(mrm_state); + // behavior { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString mrm_behavior = "MRM Behavior | Unknown"; + switch (msg->behavior) { case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_behavior = "MRM Behavior | Inactive"; break; case MRMState::PULL_OVER: - text = "PULL_OVER"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + mrm_behavior = "MRM Behavior | Pull Over"; break; case MRMState::COMFORTABLE_STOP: - text = "COMFORTABLE_STOP"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + mrm_behavior = "MRM Behavior | Comfortable Stop"; break; case MRMState::EMERGENCY_STOP: - text = "EMERGENCY_STOP"; - style_sheet = "background-color: #FFA500;"; // orange + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + mrm_behavior = "MRM Behavior | Emergency Stop"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_behavior = "MRM Behavior | Unknown"; break; } - updateLabel(mrm_behavior_label_ptr_, text, style_sheet); - } -} - -void AutowareStatePanel::onShift( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) -{ - switch (msg->report) { - case autoware_auto_vehicle_msgs::msg::GearReport::PARK: - gear_label_ptr_->setText("PARKING"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: - gear_label_ptr_->setText("REVERSE"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::DRIVE: - gear_label_ptr_->setText("DRIVE"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: - gear_label_ptr_->setText("NEUTRAL"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::LOW: - gear_label_ptr_->setText("LOW"); - break; + mrm_behavior_icon->updateStyle(state, bgColor); + mrm_behavior_label_ptr_->setText(mrm_behavior); } } @@ -554,18 +745,37 @@ void AutowareStatePanel::onEmergencyStatus( { current_emergency_ = msg->emergency; if (msg->emergency) { - emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #FF0000;"); + emergency_button_ptr_->updateStyle( + "Clear Emergency", + QColor(autoware::state_rviz_plugin::colors::default_colors.error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.error_container.c_str())); } else { - emergency_button_ptr_->setText(QString::fromStdString("Set Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #00FF00;"); + emergency_button_ptr_->updateStyle( + "Set Emergency", QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_tint.c_str())); + } +} + +void AutowareStatePanel::onSwitchStateChanged(int state) +{ + if (state == 0) { + // call the control mode function + onClickDirectControl(); + } else if (state == 2) { + onClickAutowareControl(); } } void AutowareStatePanel::onClickVelocityLimit() { auto velocity_limit = std::make_shared(); - velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6; + velocity_limit->max_velocity = velocity_limit_value_label_->text().toDouble() / 3.6; pub_velocity_limit_->publish(*velocity_limit); } diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp deleted file mode 100644 index 8f67a90215bd1..0000000000000 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ /dev/null @@ -1,209 +0,0 @@ -// -// 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 AUTOWARE_STATE_PANEL_HPP_ -#define AUTOWARE_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 rviz_plugins -{ -class AutowareStatePanel : 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 AutowareStatePanel(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_auto_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 - //// Gate 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 rviz_plugins - -#endif // AUTOWARE_STATE_PANEL_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/custom_button.cpp b/common/tier4_state_rviz_plugin/src/custom_button.cpp new file mode 100644 index 0000000000000..0ef2628577135 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_button.cpp @@ -0,0 +1,129 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "include/custom_button.hpp" + +#include "src/include/material_colors.hpp" + +CustomElevatedButton::CustomElevatedButton( + const QString & text, const QColor & bgColor, const QColor & textColor, const QColor & hoverColor, + const QColor & disabledBgColor, const QColor & disabledTextColor, QWidget * parent) +: QPushButton(text, parent), + backgroundColor(bgColor), + textColor(textColor), + hoverColor(hoverColor), + disabledBgColor(disabledBgColor), + disabledTextColor(disabledTextColor) +{ + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + setCursor(Qt::PointingHandCursor); + + // set font weight to bold and size to 12 + QFont font = this->font(); + font.setWeight(QFont::Bold); + font.setFamily("Roboto"); + setFont(font); + + // Set up drop shadow effect + QGraphicsDropShadowEffect * shadowEffect = new QGraphicsDropShadowEffect(this); + shadowEffect->setBlurRadius(15); + shadowEffect->setOffset(3, 3); + shadowEffect->setColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.shadow.c_str())); + setGraphicsEffect(shadowEffect); +} + +QSize CustomElevatedButton::sizeHint() const +{ + QFontMetrics fm(font()); + int textWidth = fm.horizontalAdvance(text()); + int textHeight = fm.height(); + int width = textWidth + 45; // Adding padding + int height = textHeight + 25; // Adding padding + return QSize(width, height); +} + +QSize CustomElevatedButton::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomElevatedButton::updateStyle( + const QString & text, const QColor & bgColor, const QColor & textColor, const QColor & hoverColor, + const QColor & disabledBgColor, const QColor & disabledTextColor) +{ + setText(text); + backgroundColor = bgColor; + this->textColor = textColor; + this->hoverColor = hoverColor; + this->disabledBgColor = disabledBgColor; + this->disabledTextColor = disabledTextColor; + update(); // Force repaint +} + +void CustomElevatedButton::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + QStyleOption opt; + opt.initFrom(this); + QRect r = rect(); + + QColor buttonColor; + QColor currentTextColor = textColor; + if (!isEnabled()) { + buttonColor = disabledBgColor; + currentTextColor = disabledTextColor; + } else if (isHovered) { + buttonColor = hoverColor; + } else { + buttonColor = backgroundColor; + } + + int cornerRadius = height() / 2; // Making the corner radius proportional to the height + + // Draw button background + QPainterPath backgroundPath; + backgroundPath.addRoundedRect(r, cornerRadius, cornerRadius); + if (!isEnabled()) { + painter.setBrush( + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + painter.setOpacity(0.12); + } else { + painter.setBrush(buttonColor); + } + painter.setPen(Qt::NoPen); + painter.drawPath(backgroundPath); + + // Draw button text + if (!isEnabled()) { + painter.setOpacity(0.38); + } + painter.setPen(currentTextColor); + painter.drawText(r, Qt::AlignCenter, text()); +} + +void CustomElevatedButton::enterEvent(QEvent * event) +{ + isHovered = true; + update(); + QPushButton::enterEvent(event); +} + +void CustomElevatedButton::leaveEvent(QEvent * event) +{ + isHovered = false; + update(); + QPushButton::leaveEvent(event); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_container.cpp b/common/tier4_state_rviz_plugin/src/custom_container.cpp new file mode 100644 index 0000000000000..0b0aa1ccd6ed5 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_container.cpp @@ -0,0 +1,70 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "include/custom_container.hpp" + +#include "src/include/material_colors.hpp" + +CustomContainer::CustomContainer(QWidget * parent) : QFrame(parent), cornerRadius(15) +{ + setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + setContentsMargins(0, 0, 0, 0); + layout = new QGridLayout(this); + layout->setMargin(0); // Margin between the border and child widgets + layout->setSpacing(0); // Spacing between child widgets + layout->setContentsMargins(10, 0, 10, 0); // Margin between the border and the layout + setLayout(layout); +} + +void CustomContainer::setCornerRadius(int radius) +{ + cornerRadius = radius; + update(); +} + +int CustomContainer::getCornerRadius() const +{ + return cornerRadius; +} + +QGridLayout * CustomContainer::getLayout() const +{ + return layout; // Provide access to the layout +} + +QSize CustomContainer::sizeHint() const +{ + QSize size = layout->sizeHint(); + int width = size.width() + 20; // Adding padding + int height = size.height() + 20; // Adding padding + return QSize(width, height); +} + +QSize CustomContainer::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomContainer::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Draw background + QPainterPath path; + path.addRoundedRect(rect(), height() / 2, height() / 2); // Use height for rounded corners + painter.setPen(Qt::NoPen); + painter.setBrush(QColor( + autoware::state_rviz_plugin::colors::default_colors.surface.c_str())); // Background color + painter.drawPath(path); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp b/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp new file mode 100644 index 0000000000000..6e4d32d40f7fb --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp @@ -0,0 +1,80 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "include/custom_icon_label.hpp" + +#include + +CustomIconLabel::CustomIconLabel(const QColor & bgColor, QWidget * parent) +: QLabel(parent), backgroundColor(bgColor) +{ + loadIcons(); + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); + setAlignment(Qt::AlignCenter); + setFixedSize(35, 35); +} + +void CustomIconLabel::loadIcons() +{ + std::string packagePath = ament_index_cpp::get_package_share_directory("tier4_state_rviz_plugin"); + + iconMap[Active] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/active.png")); + iconMap[Pending] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/pending.png")); + iconMap[Danger] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/danger.png")); + iconMap[None] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/none.png")); + iconMap[Crash] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/crash.png")); + + icon = iconMap[None]; // Default icon +} + +void CustomIconLabel::updateStyle(IconState state, const QColor & bgColor) +{ + icon = iconMap[state]; + backgroundColor = bgColor; + update(); // Force repaint +} + +QSize CustomIconLabel::sizeHint() const +{ + int size = qMax(icon.width(), icon.height()) + 20; // Adding padding + return QSize(size, size); +} + +QSize CustomIconLabel::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomIconLabel::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + int diameter = qMin(width(), height()); + int radius = diameter / 2; + + // Draw background circle + QPainterPath path; + path.addEllipse(width() / 2 - radius, height() / 2 - radius, diameter, diameter); + painter.setPen(Qt::NoPen); + painter.setBrush(backgroundColor); + painter.drawPath(path); + + // Draw icon in the center + if (!icon.isNull()) { + QSize iconSize = icon.size().scaled(diameter * 0.6, diameter * 0.6, Qt::KeepAspectRatio); + QPoint iconPos((width() - iconSize.width()) / 2, (height() - iconSize.height()) / 2); + painter.drawPixmap( + iconPos, icon.scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation)); + } +} diff --git a/common/tier4_state_rviz_plugin/src/custom_label.cpp b/common/tier4_state_rviz_plugin/src/custom_label.cpp new file mode 100644 index 0000000000000..1f96fc0d45095 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_label.cpp @@ -0,0 +1,82 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "include/custom_label.hpp" + +#include "src/include/material_colors.hpp" + +#include +#include +#include +#include +#include +#include + +CustomLabel::CustomLabel(const QString & text, QWidget * parent) : QLabel(text, parent) +{ + setFont(QFont("Roboto", 10, QFont::Bold)); // Set the font as needed + setAlignment(Qt::AlignCenter); + + // Add shadow effect + QGraphicsDropShadowEffect * shadowEffect = new QGraphicsDropShadowEffect(this); + shadowEffect->setBlurRadius(15); // Blur radius for a smoother shadow + shadowEffect->setOffset(3, 3); // Offset for the shadow + shadowEffect->setColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.shadow.c_str())); // Shadow color + setGraphicsEffect(shadowEffect); +} + +QSize CustomLabel::sizeHint() const +{ + QFontMetrics fm(font()); + int textWidth = fm.horizontalAdvance(text()); + int textHeight = fm.height(); + int width = textWidth + 40; // Adding padding + int height = textHeight; // Adding padding + return QSize(width, height); +} + +QSize CustomLabel::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomLabel::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + int cornerRadius = height() / 2; // Making the corner radius proportional to the height + + // Draw background + QPainterPath path; + path.addRoundedRect(rect().adjusted(1, 1, -1, -1), cornerRadius, cornerRadius); + + painter.setPen(Qt::NoPen); + painter.setBrush(backgroundColor); + + painter.drawPath(path); + + // Set text color and draw text + painter.setPen(textColor); + painter.drawText(rect(), Qt::AlignCenter, text()); +} + +void CustomLabel::updateStyle( + const QString & text, const QColor & bg_color, const QColor & text_color) +{ + setText(text); + backgroundColor = bg_color; + textColor = text_color; + update(); // Force repaint +} diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp new file mode 100644 index 0000000000000..8063bdc0fbc28 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp @@ -0,0 +1,75 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "include/custom_segmented_button.hpp" + +#include + +CustomSegmentedButton::CustomSegmentedButton(QWidget * parent) +: QWidget(parent), buttonGroup(new QButtonGroup(this)), layout(new QHBoxLayout(this)) +{ + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + layout->setContentsMargins(0, 0, 0, 0); // Ensure no margins around the layout + layout->setSpacing(0); // Ensure no spacing between buttons + + setLayout(layout); + + buttonGroup->setExclusive(true); + + connect( + buttonGroup, QOverload::of(&QButtonGroup::idClicked), this, + &CustomSegmentedButton::buttonClicked); +} + +CustomSegmentedButtonItem * CustomSegmentedButton::addButton(const QString & text) +{ + CustomSegmentedButtonItem * button = new CustomSegmentedButtonItem(text); + layout->addWidget(button); + buttonGroup->addButton(button, layout->count() - 1); + + return button; +} + +QButtonGroup * CustomSegmentedButton::getButtonGroup() const +{ + return buttonGroup; +} + +QSize CustomSegmentedButton::sizeHint() const +{ + return QSize(400, 40); // Adjust the size hint as needed + + // return QSize( + // layout->count() * (layout->itemAt(0)->widget()->width()), + // layout->itemAt(0)->widget()->height() + 10); +} + +QSize CustomSegmentedButton::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomSegmentedButton::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Draw background + QPainterPath path; + path.addRoundedRect(rect(), height() / 2, height() / 2); + + painter.setPen(Qt::NoPen); + painter.setBrush( + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str())); + painter.drawPath(path); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp new file mode 100644 index 0000000000000..f2d15260c4e41 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp @@ -0,0 +1,186 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "include/custom_segmented_button_item.hpp" + +CustomSegmentedButtonItem::CustomSegmentedButtonItem(const QString & text, QWidget * parent) +: QPushButton(text, parent), + bgColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str())), + checkedBgColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.secondary_container.c_str())), + inactiveTextColor(QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())), + activeTextColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())), + isHovered(false), + isActivated(false), + isDisabled(false) + +{ + setCheckable(true); + setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + setCursor(Qt::PointingHandCursor); +} + +void CustomSegmentedButtonItem::setColors( + const QColor & bg, const QColor & checkedBg, const QColor & activeText, + const QColor & inactiveText) +{ + bgColor = bg; + checkedBgColor = checkedBg; + activeTextColor = activeText; + inactiveTextColor = inactiveText; + update(); +} + +// void CustomSegmentedButtonItem::updateSize() +// { +// QFontMetrics fm(font()); +// int width = fm.horizontalAdvance(text()) + 40; // Add padding +// int height = fm.height() + 20; // Add padding +// setFixedSize(width, height); +// } + +void CustomSegmentedButtonItem::setHovered(bool hovered) +{ + isHovered = hovered; + updateCheckableState(); +} + +void CustomSegmentedButtonItem::setCheckableButton(bool checkable) +{ + setCheckable(checkable); + updateCheckableState(); +} + +void CustomSegmentedButtonItem::updateCheckableState() +{ + setCursor( + isDisabled ? Qt::ForbiddenCursor + : (isCheckable() ? Qt::PointingHandCursor : Qt::ForbiddenCursor)); + update(); +} + +void CustomSegmentedButtonItem::setDisabledButton(bool disabled) +{ + isDisabled = disabled; + updateCheckableState(); +} + +void CustomSegmentedButtonItem::setActivated(bool activated) +{ + isActivated = activated; + update(); +} + +void CustomSegmentedButtonItem::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Adjust opacity for disabled state + if (isDisabled) { + painter.setOpacity(0.3); + } else { + painter.setOpacity(1.0); + } + + // Determine the button's color based on its state + QColor buttonColor; + if (isDisabled) { + buttonColor = disabledBgColor; + } else if (isHovered && !isChecked() && isCheckable()) { + buttonColor = hoverColor; + } else if (isActivated) { + buttonColor = checkedBgColor; + } else { + buttonColor = isChecked() ? checkedBgColor : bgColor; + } + + // Determine if this is the first or last button + bool isFirstButton = false; + bool isLastButton = false; + + QHBoxLayout * parentLayout = qobject_cast(parentWidget()->layout()); + if (parentLayout) { + int index = parentLayout->indexOf(this); + isFirstButton = (index == 0); + isLastButton = (index == parentLayout->count() - 1); + } + + // Draw button background + + QRect buttonRect = rect().adjusted(0, 1, 0, -1); + + if (isFirstButton) { + buttonRect.setLeft(buttonRect.left() + 1); + } + if (isLastButton) { + buttonRect.setRight(buttonRect.right() - 1); + } + + QPainterPath path; + double radius = (height() - 2) / 2; + + path.setFillRule(Qt::WindingFill); + if (isFirstButton) { + path.addRoundedRect(buttonRect, radius, radius); + path.addRect(QRect( + (buttonRect.left() + buttonRect.width() - radius), + buttonRect.top() + buttonRect.height() - radius, radius, + radius)); // Bottom Right + path.addRect(QRect( + (buttonRect.left() + buttonRect.width() - radius), buttonRect.top(), radius, + radius)); // Top Right + } else if (isLastButton) { + path.addRoundedRect(buttonRect, radius, radius); + path.addRect(QRect( + (buttonRect.left()), buttonRect.top() + buttonRect.height() - radius, radius, + radius)); // Bottom left + path.addRect(QRect((buttonRect.left()), buttonRect.top(), radius, + radius)); // Top Left + } else { + path.addRect(buttonRect); + } + painter.fillPath(path, buttonColor); + + // Draw button border + QPen pen(QColor(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()), 1); + pen.setJoinStyle(Qt::RoundJoin); + pen.setCapStyle(Qt::RoundCap); + painter.setPen(pen); + painter.drawPath(path.simplified()); + + // Draw button text + QColor textColor = (isChecked() ? activeTextColor : inactiveTextColor); + painter.setPen(textColor); + painter.drawText(rect(), Qt::AlignCenter, text()); +} + +void CustomSegmentedButtonItem::enterEvent(QEvent * event) +{ + if (isCheckable()) { + isHovered = true; + update(); + } + QPushButton::enterEvent(event); +} + +void CustomSegmentedButtonItem::leaveEvent(QEvent * event) +{ + if (isCheckable()) { + isHovered = false; + update(); + } + QPushButton::leaveEvent(event); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_slider.cpp b/common/tier4_state_rviz_plugin/src/custom_slider.cpp new file mode 100644 index 0000000000000..cf3f7c3d4638f --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_slider.cpp @@ -0,0 +1,102 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "include/custom_slider.hpp" + +CustomSlider::CustomSlider(Qt::Orientation orientation, QWidget * parent) +: QSlider(orientation, parent) +{ + setMinimumHeight(40); // Ensure there's enough space for the custom track +} + +void CustomSlider::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + painter.setPen(Qt::NoPen); + + // Initialize style option + QStyleOptionSlider opt; + initStyleOption(&opt); + + QRect grooveRect = + style()->subControlRect(QStyle::CC_Slider, &opt, QStyle::SC_SliderGroove, this); + int centerY = grooveRect.center().y(); + QRect handleRect = + style()->subControlRect(QStyle::CC_Slider, &opt, QStyle::SC_SliderHandle, this); + + int value = this->value(); + int minValue = this->minimum(); + int maxValue = this->maximum(); + + int trackThickness = 14; + int gap = 8; + + QRect beforeRect( + grooveRect.x(), centerY - trackThickness / 2, handleRect.center().x() - grooveRect.x() - gap, + trackThickness); + + QRect afterRect( + handleRect.center().x() + gap, centerY - trackThickness / 2, + grooveRect.right() - handleRect.center().x() - gap, trackThickness); + + QColor inactiveTrackColor( + autoware::state_rviz_plugin::colors::default_colors.primary_container.c_str()); + QColor activeTrackColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + QColor handleColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + + // only draw the active track if the value is more than the gap from the minimum + if (value > minValue + gap / 2) { + QPainterPath beforePath; + beforePath.moveTo(beforeRect.left(), centerY + trackThickness / 2); // Start from bottom-left + beforePath.quadTo( + beforeRect.left(), centerY - trackThickness / 2, beforeRect.left() + trackThickness * 0.5, + centerY - trackThickness / 2); + beforePath.lineTo(beforeRect.right() - trackThickness * 0.1, centerY - trackThickness / 2); + beforePath.quadTo( + beforeRect.right(), centerY - trackThickness / 2, beforeRect.right(), centerY); + beforePath.quadTo( + beforeRect.right(), centerY + trackThickness / 2, beforeRect.right() - trackThickness * 0.1, + centerY + trackThickness / 2); + beforePath.lineTo(beforeRect.left() + trackThickness * 0.5, centerY + trackThickness / 2); + beforePath.quadTo(beforeRect.left(), centerY + trackThickness / 2, beforeRect.left(), centerY); + painter.fillPath(beforePath, activeTrackColor); + } + + if (value < maxValue - gap / 2) { + QPainterPath afterPath; + afterPath.moveTo(afterRect.left(), centerY + trackThickness / 2); + afterPath.quadTo( + afterRect.left(), centerY - trackThickness / 2, afterRect.left() + trackThickness * 0.1, + centerY - trackThickness / 2); + afterPath.lineTo(afterRect.right() - trackThickness * 0.5, centerY - trackThickness / 2); + afterPath.quadTo(afterRect.right(), centerY - trackThickness / 2, afterRect.right(), centerY); + afterPath.quadTo( + afterRect.right(), centerY + trackThickness / 2, afterRect.right() - trackThickness * 0.5, + centerY + trackThickness / 2); + afterPath.lineTo(afterRect.left() + trackThickness * 0.1, centerY + trackThickness / 2); + afterPath.quadTo(afterRect.left(), centerY + trackThickness / 2, afterRect.left(), centerY); + painter.fillPath(afterPath, inactiveTrackColor); + } + + painter.setBrush(handleColor); + int handleLineHeight = 30; + int handleLineWidth = 4; + int handleLineRadius = 2; + QRect handleLineRect( + handleRect.center().x() - handleLineWidth / 2, centerY - handleLineHeight / 2, handleLineWidth, + handleLineHeight); + QPainterPath handlePath; + handlePath.addRoundedRect(handleLineRect, handleLineRadius, handleLineRadius); + painter.fillPath(handlePath, handleColor); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp b/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp new file mode 100644 index 0000000000000..3b58ded626439 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp @@ -0,0 +1,87 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "include/custom_toggle_switch.hpp" + +CustomToggleSwitch::CustomToggleSwitch(QWidget * parent) : QCheckBox(parent) +{ + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + setCursor(Qt::PointingHandCursor); + + connect(this, &QCheckBox::stateChanged, this, [this]() { + if (!blockSignalsGuard) { + update(); // Force repaint + } + }); +} + +QSize CustomToggleSwitch::sizeHint() const +{ + return QSize(50, 30); // Preferred size of the toggle switch +} + +void CustomToggleSwitch::paintEvent(QPaintEvent *) +{ + QPainter p(this); + p.setRenderHint(QPainter::Antialiasing); + + int margin = 2; + int circleRadius = height() / 2 - margin * 2; + QRect r = rect().adjusted(margin, margin, -margin, -margin); + bool isChecked = this->isChecked(); + + QColor uncheckedIndicatorColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()); + QColor checkedIndicatorColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()); + QColor indicatorColor = isChecked ? checkedIndicatorColor : uncheckedIndicatorColor; + + QColor uncheckedBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor checkedBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + + QColor bgColor = isChecked ? checkedBgColor : uncheckedBgColor; + + QRect borderR = r.adjusted(-margin, -margin, margin, margin); + p.setBrush(bgColor); + p.setPen(Qt::NoPen); + p.drawRoundedRect(borderR, circleRadius + 4, circleRadius + 4); + + p.setBrush(bgColor); + p.setPen(Qt::NoPen); + p.drawRoundedRect(r, circleRadius + 4, circleRadius + 4); + + int minX = r.left() + margin * 2; + int maxX = r.right() - circleRadius * 2 - margin; + int circleX = isChecked ? maxX : minX; + QRect circleRect(circleX, r.top() + margin, circleRadius * 2, circleRadius * 2); + p.setBrush(indicatorColor); + p.drawEllipse(circleRect); +} + +void CustomToggleSwitch::mouseReleaseEvent(QMouseEvent * event) +{ + if (event->button() == Qt::LeftButton) { + setCheckedState(!isChecked()); + } + QCheckBox::mouseReleaseEvent(event); +} + +void CustomToggleSwitch::setCheckedState(bool state) +{ + blockSignalsGuard = true; + setChecked(state); + blockSignalsGuard = false; + update(); // Force repaint +} diff --git a/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp new file mode 100644 index 0000000000000..c1ab9018e12f9 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp @@ -0,0 +1,253 @@ +// +// 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 AUTOWARE_STATE_PANEL_HPP_ +#define AUTOWARE_STATE_PANEL_HPP_ + +#include "custom_button.hpp" +#include "custom_container.hpp" +#include "custom_icon_label.hpp" +#include "custom_label.hpp" +#include "custom_segmented_button.hpp" +#include "custom_segmented_button_item.hpp" +#include "custom_slider.hpp" +#include "custom_toggle_switch.hpp" +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace rviz_plugins +{ +class AutowareStatePanel : 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; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + + Q_OBJECT + +public: + explicit AutowareStatePanel(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(); + void onSwitchStateChanged(int state); + +protected: + // Layout + QVBoxLayout * makeVelocityLimitGroup(); + QVBoxLayout * makeOperationModeGroup(); + QVBoxLayout * makeRoutingGroup(); + QVBoxLayout * makeLocalizationGroup(); + QVBoxLayout * makeMotionGroup(); + QVBoxLayout * makeFailSafeGroup(); + // QVBoxLayout * makeDiagnosticGroup(); + + // 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_; + + QLabel * velocity_limit_value_label_{nullptr}; + bool sliderIsDragging = false; + + // Operation Mode + QLabel * operation_mode_label_ptr_{nullptr}; + CustomSegmentedButtonItem * stop_button_ptr_{nullptr}; + CustomSegmentedButtonItem * auto_button_ptr_{nullptr}; + CustomSegmentedButtonItem * local_button_ptr_{nullptr}; + CustomSegmentedButtonItem * 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 + CustomSegmentedButton * segmented_button; + CustomToggleSwitch * control_mode_switch_ptr_{nullptr}; + 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 + CustomIconLabel * routing_icon{nullptr}; + CustomElevatedButton * clear_route_button_ptr_{nullptr}; + QLabel * routing_label_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Client::SharedPtr client_clear_route_; + + void onRoute(const RouteState::ConstSharedPtr msg); + + // Localization + CustomIconLabel * localization_icon{nullptr}; + CustomElevatedButton * init_by_gnss_button_ptr_{nullptr}; + QLabel * localization_label_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_localization_; + rclcpp::Client::SharedPtr client_init_by_gnss_; + + void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg); + + // Motion + CustomIconLabel * motion_icon{nullptr}; + CustomElevatedButton * accept_start_button_ptr_{nullptr}; + QLabel * motion_label_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_motion_; + rclcpp::Client::SharedPtr client_accept_start_; + + void onMotion(const MotionState::ConstSharedPtr msg); + + // FailSafe + CustomIconLabel * mrm_state_icon{nullptr}; + QLabel * mrm_state_label_ptr_{nullptr}; + CustomIconLabel * mrm_behavior_icon{nullptr}; + QLabel * mrm_behavior_label_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_mrm_; + + void onMRMState(const MRMState::ConstSharedPtr msg); + + // Others + QLabel * velocity_limit_setter_ptr_; + QLabel * gear_label_ptr_; + + QSpinBox * pub_velocity_limit_input_; + CustomElevatedButton * 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 updateCustomLabel( + CustomLabel * label, QString text, QColor bg_color, QColor text_color) + { + label->updateStyle(text, bg_color, text_color); + } + + static void updateButton(QPushButton * button, QString text, QString style_sheet) + { + button->setText(text); + button->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 rviz_plugins + +#endif // AUTOWARE_STATE_PANEL_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_button.hpp b/common/tier4_state_rviz_plugin/src/include/custom_button.hpp new file mode 100644 index 0000000000000..b10663ce09933 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_button.hpp @@ -0,0 +1,69 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 CUSTOM_BUTTON_HPP_ +#define CUSTOM_BUTTON_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include + +class CustomElevatedButton : public QPushButton +{ + Q_OBJECT + +public: + explicit CustomElevatedButton( + const QString & text, + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()), + const QColor & textColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()), + const QColor & hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()), + const QColor & disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()), + const QColor & disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface_variant.c_str()), + QWidget * parent = nullptr); + void updateStyle( + const QString & text, const QColor & bgColor, const QColor & textColor, + const QColor & hoverColor, const QColor & disabledBgColor, const QColor & disabledTextColor); + +protected: + void paintEvent(QPaintEvent * event) override; + void enterEvent(QEvent * event) override; + void leaveEvent(QEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()); + QColor textColor = QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + QColor hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.disabled_elevated_button_bg.c_str()); + QColor disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str()); + bool isHovered = false; +}; + +#endif // CUSTOM_BUTTON_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_container.hpp b/common/tier4_state_rviz_plugin/src/include/custom_container.hpp new file mode 100644 index 0000000000000..5142b409ebc88 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_container.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 CUSTOM_CONTAINER_HPP_ +#define CUSTOM_CONTAINER_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include + +class CustomContainer : public QFrame +{ + Q_OBJECT + +public: + explicit CustomContainer(QWidget * parent = nullptr); + + // Methods to set dimensions and corner radius + void setContainerHeight(int height); + void setContainerWidth(int width); + void setCornerRadius(int radius); + + // Getters + int getContainerHeight() const; + int getContainerWidth() const; + int getCornerRadius() const; + QGridLayout * getLayout() const; // Add a method to access the layout + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QGridLayout * layout; + int cornerRadius; +}; + +#endif // CUSTOM_CONTAINER_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp b/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp new file mode 100644 index 0000000000000..1b3ab9c8e0c6c --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 CUSTOM_ICON_LABEL_HPP_ +#define CUSTOM_ICON_LABEL_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +enum IconState { Active, Pending, Danger, None, Crash }; + +class CustomIconLabel : public QLabel +{ + Q_OBJECT + +public: + explicit CustomIconLabel( + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()), + QWidget * parent = nullptr); + void updateStyle(IconState state, const QColor & bgColor); + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + void loadIcons(); + QPixmap icon; + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()); + QMap iconMap; +}; + +#endif // CUSTOM_ICON_LABEL_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_label.hpp b/common/tier4_state_rviz_plugin/src/include/custom_label.hpp new file mode 100644 index 0000000000000..a328c4de56e3d --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_label.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 CUSTOM_LABEL_HPP_ +#define CUSTOM_LABEL_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +class CustomLabel : public QLabel +{ + Q_OBJECT + +public: + explicit CustomLabel(const QString & text, QWidget * parent = nullptr); + void updateStyle( + const QString & text, + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()), + const QColor & textColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()); + QColor textColor = QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str()); +}; + +#endif // CUSTOM_LABEL_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp new file mode 100644 index 0000000000000..c6c589d31b8d4 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp @@ -0,0 +1,53 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 CUSTOM_SEGMENTED_BUTTON_HPP_ +#define CUSTOM_SEGMENTED_BUTTON_HPP_ + +#include "custom_segmented_button_item.hpp" +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +class CustomSegmentedButton : public QWidget +{ + Q_OBJECT + +public: + explicit CustomSegmentedButton(QWidget * parent = nullptr); + + CustomSegmentedButtonItem * addButton(const QString & text); + QButtonGroup * getButtonGroup() const; + + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +Q_SIGNALS: + void buttonClicked(int id); + +protected: + void paintEvent(QPaintEvent * event) override; + +private: + QButtonGroup * buttonGroup; + QHBoxLayout * layout; +}; + +#endif // CUSTOM_SEGMENTED_BUTTON_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp new file mode 100644 index 0000000000000..33eb9fe16aa31 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp @@ -0,0 +1,64 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ +#define CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include + +class CustomSegmentedButtonItem : public QPushButton +{ + Q_OBJECT + +public: + explicit CustomSegmentedButtonItem(const QString & text, QWidget * parent = nullptr); + + void setColors( + const QColor & bg, const QColor & checkedBg, const QColor & activeText, + const QColor & inactiveText); + void setActivated(bool activated); + void setCheckableButton(bool checkable); + void setDisabledButton(bool disabled); + void setHovered(bool hovered); + +protected: + void paintEvent(QPaintEvent * event) override; + void enterEvent(QEvent * event) override; + void leaveEvent(QEvent * event) override; + +private: + void updateCheckableState(); + + QColor bgColor; + QColor checkedBgColor; + QColor hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor inactiveTextColor; + QColor activeTextColor; + QColor disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_dim.c_str()); + QColor disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface_variant.c_str()); + bool isHovered = false; + bool isActivated = false; + bool isDisabled = false; +}; + +#endif // CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_slider.hpp b/common/tier4_state_rviz_plugin/src/include/custom_slider.hpp new file mode 100644 index 0000000000000..f0dc9f12aedfe --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_slider.hpp @@ -0,0 +1,35 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 CUSTOM_SLIDER_HPP_ +#define CUSTOM_SLIDER_HPP_ +#include "material_colors.hpp" + +#include +#include +#include +#include +#include + +class CustomSlider : public QSlider +{ + Q_OBJECT + +public: + explicit CustomSlider(Qt::Orientation orientation, QWidget * parent = nullptr); + +protected: + void paintEvent(QPaintEvent * event) override; +}; + +#endif // CUSTOM_SLIDER_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp b/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp new file mode 100644 index 0000000000000..107d45af8c3f3 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 CUSTOM_TOGGLE_SWITCH_HPP_ +#define CUSTOM_TOGGLE_SWITCH_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +class CustomToggleSwitch : public QCheckBox +{ + Q_OBJECT + +public: + explicit CustomToggleSwitch(QWidget * parent = nullptr); + QSize sizeHint() const override; + void setCheckedState(bool state); + +protected: + void paintEvent(QPaintEvent * event) override; + void mouseReleaseEvent(QMouseEvent * event) override; + +private: + bool blockSignalsGuard = false; // Guard variable to block signals during updates +}; + +#endif // CUSTOM_TOGGLE_SWITCH_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/material_colors.hpp b/common/tier4_state_rviz_plugin/src/include/material_colors.hpp new file mode 100644 index 0000000000000..d146b599ab600 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/material_colors.hpp @@ -0,0 +1,88 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 MATERIAL_COLORS_HPP_ +#define MATERIAL_COLORS_HPP_ +#include + +namespace autoware +{ +namespace state_rviz_plugin +{ +namespace colors +{ +struct MaterialColors +{ + std::string primary = "#8BD0F0"; + std::string surface_tint = "#8BD0F0"; + std::string on_primary = "#003546"; + std::string primary_container = "#004D64"; + std::string on_primary_container = "#BEE9FF"; + std::string secondary = "#B4CAD6"; + std::string on_secondary = "#1F333C"; + std::string secondary_container = "#354A54"; + std::string on_secondary_container = "#D0E6F2"; + std::string tertiary = "#C6C2EA"; + std::string on_tertiary = "#2F2D4D"; + std::string tertiary_container = "#454364"; + std::string on_tertiary_container = "#E3DFFF"; + std::string error = "#FFB4AB"; + std::string on_error = "#690005"; + std::string error_container = "#93000A"; + std::string on_error_container = "#FFDAD6"; + std::string background = "#0F1417"; + std::string on_background = "#DFE3E7"; + std::string surface = "#0F1417"; + std::string on_surface = "#DFE3E7"; + std::string surface_variant = "#40484C"; + std::string on_surface_variant = "#C0C8CD"; + std::string outline = "#8A9297"; + std::string outline_variant = "#40484C"; + std::string shadow = "#000000"; + std::string scrim = "#000000"; + std::string inverse_surface = "#DFE3E7"; + std::string inverse_on_surface = "#2C3134"; + std::string inverse_primary = "#126682"; + std::string primary_fixed = "#BEE9FF"; + std::string on_primary_fixed = "#001F2A"; + std::string primary_fixed_dim = "#8BD0F0"; + std::string on_primary_fixed_variant = "#004D64"; + std::string secondary_fixed = "#D0E6F2"; + std::string on_secondary_fixed = "#081E27"; + std::string secondary_fixed_dim = "#B4CAD6"; + std::string on_secondary_fixed_variant = "#354A54"; + std::string tertiary_fixed = "#E3DFFF"; + std::string on_tertiary_fixed = "#1A1836"; + std::string tertiary_fixed_dim = "#C6C2EA"; + std::string on_tertiary_fixed_variant = "#454364"; + std::string surface_dim = "#0F1417"; + std::string surface_bright = "#353A3D"; + std::string surface_container_lowest = "#0A0F11"; + std::string surface_container_low = "#171C1F"; + std::string surface_container = "#1B2023"; + std::string surface_container_high = "#262B2E"; + std::string surface_container_highest = "#303538"; + std::string disabled_elevated_button_bg = "#292D30"; + std::string success = "#8DF08B"; + std::string warning = "#EEF08B"; + std::string info = "#8BD0F0"; + std::string danger = "#F08B8B"; +}; + +inline MaterialColors default_colors; +} // namespace colors +} // namespace state_rviz_plugin +} // namespace autoware + +#endif // MATERIAL_COLORS_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp similarity index 100% rename from common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp rename to common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp index 6ea142ed66a1b..90ad18fe5af6c 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp @@ -14,7 +14,7 @@ // limitations under the License. // -#include "velocity_steering_factors_panel.hpp" +#include "include/velocity_steering_factors_panel.hpp" #include #include diff --git a/common/tier4_system_rviz_plugin/README.md b/common/tier4_system_rviz_plugin/README.md index 61260ecfda723..763504c4709a9 100644 --- a/common/tier4_system_rviz_plugin/README.md +++ b/common/tier4_system_rviz_plugin/README.md @@ -6,6 +6,6 @@ This plugin display the Hazard information from Autoware; and output notices whe ## Input -| Name | Type | Description | -| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------ | -| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware | +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ------------------------------------------------------------ | +| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware | diff --git a/common/tier4_system_rviz_plugin/package.xml b/common/tier4_system_rviz_plugin/package.xml index adae997ea07aa..bd888fb085960 100644 --- a/common/tier4_system_rviz_plugin/package.xml +++ b/common/tier4_system_rviz_plugin/package.xml @@ -10,7 +10,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_system_msgs + 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_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp index 61c2bd378dab1..c0db8cc86450b 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp @@ -50,7 +50,7 @@ #include #include -#include +#include #include #include @@ -162,7 +162,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) // MRM summary std::vector mrm_comfortable_stop_summary_list = {}; std::vector mrm_emergency_stop_summary_list = {}; - int hazard_level = autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT; + int hazard_level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; { std::lock_guard message_lock(mutex_); if (last_msg_ptr_) { @@ -207,7 +207,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) std::ostringstream output_text; // Display the MRM Summary only when there is a fault - if (hazard_level != autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT) { + if (hazard_level != autoware_system_msgs::msg::HazardStatus::NO_FAULT) { // Broadcasting the Basic Error Infos int number_of_comfortable_stop_messages = static_cast(mrm_comfortable_stop_summary_list.size()); @@ -243,7 +243,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) } void MrmSummaryOverlayDisplay::processMessage( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp index d0b0e32c3788f..0f59ba5582fed 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp @@ -59,14 +59,14 @@ #include #include -#include +#include #endif namespace rviz_plugins { class MrmSummaryOverlayDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -85,7 +85,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) override; + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; rviz_common::properties::IntProperty * property_left_; @@ -101,7 +101,7 @@ private Q_SLOTS: static constexpr int hand_width_ = 4; std::mutex mutex_; - autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr last_msg_ptr_; + autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt b/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt deleted file mode 100644 index ed458cf924e33..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tier4_target_object_type_rviz_plugin) - -find_package(autoware_cmake REQUIRED) -autoware_package() -find_package(OpenCV REQUIRED) -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) -set(CMAKE_AUTOMOC ON) -set(CMAKE_INCLUDE_CURRENT_DIR ON) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/target_object_type_panel.hpp - src/target_object_type_panel.cpp -) -target_link_libraries(${PROJECT_NAME} - ${QT_LIBRARIES} - ${OpenCV_LIBRARIES} -) -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - plugins -) diff --git a/common/tier4_target_object_type_rviz_plugin/README.md b/common/tier4_target_object_type_rviz_plugin/README.md deleted file mode 100644 index 1296bd3442ab3..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/README.md +++ /dev/null @@ -1,9 +0,0 @@ -# tier4_target_object_type_rviz_plugin - -This plugin allows you to check which types of the dynamic object is being used by each planner. - -![window](./image/window.png) - -## Limitations - -Currently, which parameters of which module to check are hardcoded. In the future, this will be parameterized using YAML. diff --git a/common/tier4_target_object_type_rviz_plugin/image/window.png b/common/tier4_target_object_type_rviz_plugin/image/window.png deleted file mode 100644 index 33aa9a1e5a26e..0000000000000 Binary files a/common/tier4_target_object_type_rviz_plugin/image/window.png and /dev/null differ diff --git a/common/tier4_target_object_type_rviz_plugin/package.xml b/common/tier4_target_object_type_rviz_plugin/package.xml deleted file mode 100644 index 04d2f28d9ba3e..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - tier4_target_object_type_rviz_plugin - 0.0.1 - The tier4_target_object_type_rviz_plugin package - Takamasa Horibe - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - cv_bridge - libqt5-core - libqt5-gui - libqt5-widgets - qtbase5-dev - rclcpp - rviz_common - rviz_rendering - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml b/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml deleted file mode 100644 index eb3350e4333bd..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - TargetObjectTypePanel - - - diff --git a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp deleted file mode 100644 index e014307942bab..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp +++ /dev/null @@ -1,316 +0,0 @@ -// Copyright 2023 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 "target_object_type_panel.hpp" - -#include -#include -#include -#include -#include - -TargetObjectTypePanel::TargetObjectTypePanel(QWidget * parent) : rviz_common::Panel(parent) -{ - node_ = std::make_shared("matrix_display_node"); - - setParameters(); - - matrix_widget_ = new QTableWidget(modules_.size(), targets_.size(), this); - for (size_t i = 0; i < modules_.size(); i++) { - matrix_widget_->setVerticalHeaderItem( - i, new QTableWidgetItem(QString::fromStdString(modules_[i]))); - } - for (size_t j = 0; j < targets_.size(); j++) { - matrix_widget_->setHorizontalHeaderItem( - j, new QTableWidgetItem(QString::fromStdString(targets_[j]))); - } - - updateMatrix(); - - reload_button_ = new QPushButton("Reload", this); - connect( - reload_button_, &QPushButton::clicked, this, &TargetObjectTypePanel::onReloadButtonClicked); - - QVBoxLayout * layout = new QVBoxLayout; - layout->addWidget(matrix_widget_); - layout->addWidget(reload_button_); - setLayout(layout); -} - -void TargetObjectTypePanel::onReloadButtonClicked() -{ - RCLCPP_INFO(node_->get_logger(), "Reload button clicked. Update parameter data."); - updateMatrix(); -} - -void TargetObjectTypePanel::setParameters() -{ - // Parameter will be investigated for these modules: - modules_ = { - "avoidance", - "avoidance_by_lane_change", - "dynamic_avoidance", - "lane_change", - "start_planner", - "goal_planner", - "crosswalk", - "surround_obstacle_checker", - "obstacle_cruise (inside)", - "obstacle_cruise (outside)", - "obstacle_stop", - "obstacle_slowdown"}; - - // Parameter will be investigated for targets in each module - targets_ = {"car", "truck", "bus", "trailer", "unknown", "bicycle", "motorcycle", "pedestrian"}; - - // TODO(Horibe): If the param naming strategy is aligned, this should be done automatically based - // on the modules_ and targets_. - - // default - ParamNameEnableObject default_param_name; - default_param_name.name.emplace("car", "car"); - default_param_name.name.emplace("truck", "truck"); - default_param_name.name.emplace("bus", "bus"); - default_param_name.name.emplace("trailer", "trailer"); - default_param_name.name.emplace("unknown", "unknown"); - default_param_name.name.emplace("bicycle", "bicycle"); - default_param_name.name.emplace("motorcycle", "motorcycle"); - default_param_name.name.emplace("pedestrian", "pedestrian"); - - // avoidance - { - const auto module = "avoidance"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; - param_name.ns = "avoidance.target_filtering.target_type"; - param_name.name = default_param_name.name; - param_names_.emplace(module, param_name); - } - - // avoidance_by_lane_change - { - const auto module = "avoidance_by_lane_change"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; - param_name.ns = "avoidance_by_lane_change.target_filtering.target_type"; - param_name.name = default_param_name.name; - param_names_.emplace(module, param_name); - } - - // lane_change - { - const auto module = "lane_change"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; - param_name.ns = "lane_change.target_object"; - param_name.name = default_param_name.name; - param_names_.emplace(module, param_name); - } - - // start_planner - { - const auto module = "start_planner"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; - param_name.ns = "start_planner.path_safety_check.target_filtering.object_types_to_check"; - param_name.name.emplace("car", "check_car"); - param_name.name.emplace("truck", "check_truck"); - param_name.name.emplace("bus", "check_bus"); - param_name.name.emplace("trailer", "check_trailer"); - param_name.name.emplace("unknown", "check_unknown"); - param_name.name.emplace("bicycle", "check_bicycle"); - param_name.name.emplace("motorcycle", "check_motorcycle"); - param_name.name.emplace("pedestrian", "check_pedestrian"); - param_names_.emplace(module, param_name); - } - - // goal_planner - { - const auto module = "goal_planner"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; - param_name.ns = "goal_planner.path_safety_check.target_filtering.object_types_to_check"; - param_name.name.emplace("car", "check_car"); - param_name.name.emplace("truck", "check_truck"); - param_name.name.emplace("bus", "check_bus"); - param_name.name.emplace("trailer", "check_trailer"); - param_name.name.emplace("unknown", "check_unknown"); - param_name.name.emplace("bicycle", "check_bicycle"); - param_name.name.emplace("motorcycle", "check_motorcycle"); - param_name.name.emplace("pedestrian", "check_pedestrian"); - param_names_.emplace(module, param_name); - } - - // dynamic_avoidance - { - const auto module = "dynamic_avoidance"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; - param_name.ns = "dynamic_avoidance.target_object"; - param_name.name = default_param_name.name; - param_names_.emplace(module, param_name); - } - - // crosswalk - { - const auto module = "crosswalk"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner"; - param_name.ns = "crosswalk.object_filtering.target_object"; - param_name.name = default_param_name.name; - param_names_.emplace(module, param_name); - } - - // obstacle cruise (inside) - { - const auto module = "obstacle_cruise (inside)"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; - param_name.ns = "common.cruise_obstacle_type.inside"; - param_name.name = default_param_name.name; - param_names_.emplace(module, param_name); - } - - // obstacle cruise (outside) - { - const auto module = "obstacle_cruise (outside)"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; - param_name.ns = "common.cruise_obstacle_type.outside"; - param_name.name = default_param_name.name; - param_names_.emplace(module, param_name); - } - - // surround_obstacle_check - { - const auto module = "surround_obstacle_checker"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker"; - param_name.ns = ""; - param_name.name.emplace("car", "car.enable_check"); - param_name.name.emplace("truck", "truck.enable_check"); - param_name.name.emplace("bus", "bus.enable_check"); - param_name.name.emplace("trailer", "trailer.enable_check"); - param_name.name.emplace("unknown", "unknown.enable_check"); - param_name.name.emplace("bicycle", "bicycle.enable_check"); - param_name.name.emplace("motorcycle", "motorcycle.enable_check"); - param_name.name.emplace("pedestrian", "pedestrian.enable_check"); - param_names_.emplace(module, param_name); - } - - // obstacle stop - { - const auto module = "obstacle_stop"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; - param_name.ns = "common.stop_obstacle_type"; - param_name.name = default_param_name.name; - param_names_.emplace(module, param_name); - } - - // obstacle slowdown - { - const auto module = "obstacle_slowdown"; - ParamNameEnableObject param_name; - param_name.node = - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; - param_name.ns = "common.slow_down_obstacle_type"; - param_name.name = default_param_name.name; - param_names_.emplace(module, param_name); - } -} - -void TargetObjectTypePanel::updateMatrix() -{ - // blue base - // const QColor color_in_use("#6eb6cc"); - // const QColor color_no_use("#1d3e48"); - // const QColor color_undefined("#9e9e9e"); - - // green base - const QColor color_in_use("#afff70"); - const QColor color_no_use("#44642b"); - const QColor color_undefined("#9e9e9e"); - - const auto set_undefined = [&](const auto i, const auto j) { - QTableWidgetItem * item = new QTableWidgetItem("N/A"); - item->setForeground(QBrush(Qt::black)); // set the text color to black - item->setBackground(color_undefined); - matrix_widget_->setItem(i, j, item); - }; - - for (size_t i = 0; i < modules_.size(); i++) { - const auto & module = modules_[i]; - - // Check if module exists in param_names_ - if (param_names_.find(module) == param_names_.end()) { - RCLCPP_WARN_STREAM(node_->get_logger(), module << " is not in the param names"); - continue; - } - - const auto & module_params = param_names_.at(module); - auto parameters_client = - std::make_shared(node_, module_params.node); - if (!parameters_client->wait_for_service(std::chrono::microseconds(500))) { - RCLCPP_WARN_STREAM( - node_->get_logger(), "Failed to find parameter service for node: " << module_params.node); - for (size_t j = 0; j < targets_.size(); j++) { - set_undefined(i, j); - } - continue; - } - - for (size_t j = 0; j < targets_.size(); j++) { - const auto & target = targets_[j]; - - // Check if target exists in module's name map - if (module_params.name.find(target) == module_params.name.end()) { - RCLCPP_WARN_STREAM( - node_->get_logger(), target << " parameter is not set in the " << module); - continue; - } - - std::string param_name = - (module_params.ns.empty() ? "" : module_params.ns + ".") + module_params.name.at(target); - auto parameter_result = parameters_client->get_parameters({param_name}); - - if (!parameter_result.empty()) { - bool value = parameter_result[0].as_bool(); - QTableWidgetItem * item = new QTableWidgetItem(value ? "O" : "X"); - item->setForeground(QBrush(value ? Qt::black : Qt::black)); // set the text color to black - item->setBackground(QBrush(value ? color_in_use : color_no_use)); - matrix_widget_->setItem(i, j, item); - } else { - RCLCPP_WARN_STREAM( - node_->get_logger(), - "Failed to get parameter " << module_params.node << " " << param_name); - - set_undefined(i, j); - } - } - } -} - -PLUGINLIB_EXPORT_CLASS(TargetObjectTypePanel, rviz_common::Panel) diff --git a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp deleted file mode 100644 index 1f3934369bfba..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2023 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 TARGET_OBJECT_TYPE_PANEL_HPP_ -#define TARGET_OBJECT_TYPE_PANEL_HPP_ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -class TargetObjectTypePanel : public rviz_common::Panel -{ - Q_OBJECT - -public: - explicit TargetObjectTypePanel(QWidget * parent = 0); - -protected: - QTableWidget * matrix_widget_; - std::shared_ptr node_; - std::vector modules_; - std::vector targets_; - - struct ParamNameEnableObject - { - std::string node; - std::string ns; - std::unordered_map name; - }; - std::unordered_map param_names_; - -private slots: - void onReloadButtonClicked(); - -private: - QPushButton * reload_button_; - - void updateMatrix(); - void setParameters(); -}; - -#endif // TARGET_OBJECT_TYPE_PANEL_HPP_ diff --git a/common/tier4_traffic_light_rviz_plugin/README.md b/common/tier4_traffic_light_rviz_plugin/README.md index 91fb5bc124cb7..cc37d17768d49 100644 --- a/common/tier4_traffic_light_rviz_plugin/README.md +++ b/common/tier4_traffic_light_rviz_plugin/README.md @@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals. ### Output -| Name | Type | Description | -| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals | +| Name | Type | Description | +| ------------------------------------------------------- | ------------------------------------------------------- | ----------------------------- | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | Publish traffic light signals | ## HowToUse diff --git a/common/tier4_traffic_light_rviz_plugin/package.xml b/common/tier4_traffic_light_rviz_plugin/package.xml index 2b118b429f1af..b123f8e2bdc3c 100644 --- a/common/tier4_traffic_light_rviz_plugin/package.xml +++ b/common/tier4_traffic_light_rviz_plugin/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs + autoware_map_msgs autoware_perception_msgs lanelet2_extension libqt5-core diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp index 35c5a88a2f8a6..db33abd72283d 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp @@ -139,66 +139,66 @@ void TrafficLightPublishPanel::onSetTrafficLightState() const auto shape = light_shape_combo_->currentText(); const auto status = light_status_combo_->currentText(); - TrafficSignalElement traffic_light; + TrafficLightElement traffic_light; traffic_light.confidence = traffic_light_confidence_input_->value(); if (color == "RED") { - traffic_light.color = TrafficSignalElement::RED; + traffic_light.color = TrafficLightElement::RED; } else if (color == "AMBER") { - traffic_light.color = TrafficSignalElement::AMBER; + traffic_light.color = TrafficLightElement::AMBER; } else if (color == "GREEN") { - traffic_light.color = TrafficSignalElement::GREEN; + traffic_light.color = TrafficLightElement::GREEN; } else if (color == "WHITE") { - traffic_light.color = TrafficSignalElement::WHITE; + traffic_light.color = TrafficLightElement::WHITE; } else if (color == "UNKNOWN") { - traffic_light.color = TrafficSignalElement::UNKNOWN; + traffic_light.color = TrafficLightElement::UNKNOWN; } if (shape == "CIRCLE") { - traffic_light.shape = TrafficSignalElement::CIRCLE; + traffic_light.shape = TrafficLightElement::CIRCLE; } else if (shape == "LEFT_ARROW") { - traffic_light.shape = TrafficSignalElement::LEFT_ARROW; + traffic_light.shape = TrafficLightElement::LEFT_ARROW; } else if (shape == "RIGHT_ARROW") { - traffic_light.shape = TrafficSignalElement::RIGHT_ARROW; + traffic_light.shape = TrafficLightElement::RIGHT_ARROW; } else if (shape == "UP_ARROW") { - traffic_light.shape = TrafficSignalElement::UP_ARROW; + traffic_light.shape = TrafficLightElement::UP_ARROW; } else if (shape == "DOWN_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_ARROW; } else if (shape == "DOWN_LEFT_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_LEFT_ARROW; } else if (shape == "DOWN_RIGHT_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_RIGHT_ARROW; } else if (shape == "UNKNOWN") { - traffic_light.shape = TrafficSignalElement::UNKNOWN; + traffic_light.shape = TrafficLightElement::UNKNOWN; } if (status == "SOLID_OFF") { - traffic_light.status = TrafficSignalElement::SOLID_OFF; + traffic_light.status = TrafficLightElement::SOLID_OFF; } else if (status == "SOLID_ON") { - traffic_light.status = TrafficSignalElement::SOLID_ON; + traffic_light.status = TrafficLightElement::SOLID_ON; } else if (status == "FLASHING") { - traffic_light.status = TrafficSignalElement::FLASHING; + traffic_light.status = TrafficLightElement::FLASHING; } else if (status == "UNKNOWN") { - traffic_light.status = TrafficSignalElement::UNKNOWN; + traffic_light.status = TrafficLightElement::UNKNOWN; } - TrafficSignal traffic_signal; + TrafficLightGroup traffic_signal; traffic_signal.elements.push_back(traffic_light); - traffic_signal.traffic_signal_id = traffic_light_id; + traffic_signal.traffic_light_group_id = traffic_light_id; - for (auto & signal : extra_traffic_signals_.signals) { - if (signal.traffic_signal_id == traffic_light_id) { + for (auto & signal : extra_traffic_signals_.traffic_light_groups) { + if (signal.traffic_light_group_id == traffic_light_id) { signal = traffic_signal; return; } } - extra_traffic_signals_.signals.push_back(traffic_signal); + extra_traffic_signals_.traffic_light_groups.push_back(traffic_signal); } void TrafficLightPublishPanel::onResetTrafficLightState() { - extra_traffic_signals_.signals.clear(); + extra_traffic_signals_.traffic_light_groups.clear(); enable_publish_ = false; publish_button_->setText("PUBLISH"); @@ -218,10 +218,10 @@ void TrafficLightPublishPanel::onInitialize() using std::placeholders::_1; raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - pub_traffic_signals_ = raw_node_->create_publisher( + pub_traffic_signals_ = raw_node_->create_publisher( "/perception/traffic_light_recognition/traffic_signals", rclcpp::QoS(1)); - sub_vector_map_ = raw_node_->create_subscription( + sub_vector_map_ = raw_node_->create_subscription( "/map/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&TrafficLightPublishPanel::onVectorMap, this, _1)); createWallTimer(); @@ -252,20 +252,20 @@ void TrafficLightPublishPanel::onTimer() pub_traffic_signals_->publish(extra_traffic_signals_); } - traffic_table_->setRowCount(extra_traffic_signals_.signals.size()); + traffic_table_->setRowCount(extra_traffic_signals_.traffic_light_groups.size()); - if (extra_traffic_signals_.signals.empty()) { + if (extra_traffic_signals_.traffic_light_groups.empty()) { return; } - for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) { - const auto & signal = extra_traffic_signals_.signals.at(i); + for (size_t i = 0; i < extra_traffic_signals_.traffic_light_groups.size(); ++i) { + const auto & signal = extra_traffic_signals_.traffic_light_groups.at(i); if (signal.elements.empty()) { continue; } - auto id_label = new QLabel(QString::number(signal.traffic_signal_id)); + auto id_label = new QLabel(QString::number(signal.traffic_light_group_id)); id_label->setAlignment(Qt::AlignCenter); auto color_label = new QLabel(); @@ -273,23 +273,23 @@ void TrafficLightPublishPanel::onTimer() const auto & light = signal.elements.front(); switch (light.color) { - case TrafficSignalElement::RED: + case TrafficLightElement::RED: color_label->setText("RED"); color_label->setStyleSheet("background-color: #FF0000;"); break; - case TrafficSignalElement::AMBER: + case TrafficLightElement::AMBER: color_label->setText("AMBER"); color_label->setStyleSheet("background-color: #FFBF00;"); break; - case TrafficSignalElement::GREEN: + case TrafficLightElement::GREEN: color_label->setText("GREEN"); color_label->setStyleSheet("background-color: #7CFC00;"); break; - case TrafficSignalElement::WHITE: + case TrafficLightElement::WHITE: color_label->setText("WHITE"); color_label->setStyleSheet("background-color: #FFFFFF;"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: color_label->setText("UNKNOWN"); color_label->setStyleSheet("background-color: #808080;"); break; @@ -301,28 +301,28 @@ void TrafficLightPublishPanel::onTimer() shape_label->setAlignment(Qt::AlignCenter); switch (light.shape) { - case TrafficSignalElement::CIRCLE: + case TrafficLightElement::CIRCLE: shape_label->setText("CIRCLE"); break; - case TrafficSignalElement::LEFT_ARROW: + case TrafficLightElement::LEFT_ARROW: shape_label->setText("LEFT_ARROW"); break; - case TrafficSignalElement::RIGHT_ARROW: + case TrafficLightElement::RIGHT_ARROW: shape_label->setText("RIGHT_ARROW"); break; - case TrafficSignalElement::UP_ARROW: + case TrafficLightElement::UP_ARROW: shape_label->setText("UP_ARROW"); break; - case TrafficSignalElement::DOWN_ARROW: + case TrafficLightElement::DOWN_ARROW: shape_label->setText("DOWN_ARROW"); break; - case TrafficSignalElement::DOWN_LEFT_ARROW: + case TrafficLightElement::DOWN_LEFT_ARROW: shape_label->setText("DOWN_LEFT_ARROW"); break; - case TrafficSignalElement::DOWN_RIGHT_ARROW: + case TrafficLightElement::DOWN_RIGHT_ARROW: shape_label->setText("DOWN_RIGHT_ARROW"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: shape_label->setText("UNKNOWN"); break; default: @@ -333,16 +333,16 @@ void TrafficLightPublishPanel::onTimer() status_label->setAlignment(Qt::AlignCenter); switch (light.status) { - case TrafficSignalElement::SOLID_OFF: + case TrafficLightElement::SOLID_OFF: status_label->setText("SOLID_OFF"); break; - case TrafficSignalElement::SOLID_ON: + case TrafficLightElement::SOLID_ON: status_label->setText("SOLID_ON"); break; - case TrafficSignalElement::FLASHING: + case TrafficLightElement::FLASHING: status_label->setText("FLASHING"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: status_label->setText("UNKNOWN"); break; default: @@ -361,7 +361,7 @@ void TrafficLightPublishPanel::onTimer() traffic_table_->update(); } -void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg) +void TrafficLightPublishPanel::onVectorMap(const LaneletMapBin::ConstSharedPtr msg) { if (received_vector_map_) return; // NOTE: examples from map_loader/lanelet2_map_visualization_node.cpp diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp index e54b6a301873b..2979563062fea 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp @@ -26,8 +26,8 @@ #include #include -#include -#include +#include +#include #endif #include @@ -35,10 +35,10 @@ namespace rviz_plugins { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_perception_msgs::msg::TrafficSignal; -using autoware_perception_msgs::msg::TrafficSignalArray; -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_perception_msgs::msg::TrafficLightGroup; +using autoware_perception_msgs::msg::TrafficLightGroupArray; class TrafficLightPublishPanel : public rviz_common::Panel { Q_OBJECT @@ -56,12 +56,12 @@ public Q_SLOTS: protected: void onTimer(); void createWallTimer(); - void onVectorMap(const HADMapBin::ConstSharedPtr msg); + void onVectorMap(const LaneletMapBin::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; rclcpp::TimerBase::SharedPtr pub_timer_; - rclcpp::Publisher::SharedPtr pub_traffic_signals_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Publisher::SharedPtr pub_traffic_signals_; + rclcpp::Subscription::SharedPtr sub_vector_map_; QSpinBox * publishing_rate_input_; QComboBox * traffic_light_id_input_; @@ -74,7 +74,7 @@ public Q_SLOTS: QPushButton * publish_button_; QTableWidget * traffic_table_; - TrafficSignalArray extra_traffic_signals_; + TrafficLightGroupArray extra_traffic_signals_; bool enable_publish_{false}; std::set traffic_light_ids_; diff --git a/common/tier4_vehicle_rviz_plugin/README.md b/common/tier4_vehicle_rviz_plugin/README.md index 09576ac327bec..9560963b1f5c0 100644 --- a/common/tier4_vehicle_rviz_plugin/README.md +++ b/common/tier4_vehicle_rviz_plugin/README.md @@ -11,12 +11,12 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t ### Input -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------------- | ---------------------------------- | -| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | -| `/control/turn_signal_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | -| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | -| `/localization/acceleration` | `geometry_msgs::msg::AccelWithCovarianceStamped` | The topic is the acceleration | +| Name | Type | Description | +| --------------------------------- | -------------------------------------------------- | ---------------------------------- | +| `/vehicle/status/velocity_status` | `autoware_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | +| `/control/turn_signal_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/steering_status` | `autoware_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/localization/acceleration` | `geometry_msgs::msg::AccelWithCovarianceStamped` | The topic is the acceleration | ## Parameter diff --git a/common/tier4_vehicle_rviz_plugin/package.xml b/common/tier4_vehicle_rviz_plugin/package.xml index f6c131fdc99f3..69057701886cb 100644 --- a/common/tier4_vehicle_rviz_plugin/package.xml +++ b/common/tier4_vehicle_rviz_plugin/package.xml @@ -10,7 +10,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_universe_utils + autoware_vehicle_msgs libqt5-core libqt5-gui libqt5-widgets @@ -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 5d3684d0351c6..8f7348c87be3c 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp @@ -164,7 +164,7 @@ void ConsoleMeterDisplay::update(float wall_dt, float ros_dt) } void ConsoleMeterDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; @@ -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 cba0fa16b50fe..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,20 +21,20 @@ #ifndef Q_MOC_RUN #include "jsk_overlay_utils.hpp" +#include +#include #include #include #include #include -#include -#include -#include +#include #endif namespace rviz_plugins { class ConsoleMeterDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -52,7 +52,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; rviz_common::properties::IntProperty * property_left_; @@ -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 @@ -86,7 +86,7 @@ private Q_SLOTS: Arc outer_arc_; std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp index 60a81e45c9c29..ffc2484c3673a 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp @@ -167,7 +167,7 @@ void SteeringAngleDisplay::update(float wall_dt, float ros_dt) } void SteeringAngleDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp index d56b5a8d742b9..867a4c6110bb0 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp @@ -26,13 +26,13 @@ #include #include -#include +#include #endif namespace rviz_plugins { class SteeringAngleDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -50,7 +50,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; @@ -65,7 +65,7 @@ private Q_SLOTS: private: std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp index b9ff54ef44ecd..ec16b693a5974 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp @@ -84,7 +84,7 @@ void TurnSignalDisplay::onDisable() } void TurnSignalDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; @@ -123,19 +123,19 @@ void TurnSignalDisplay::update(float wall_dt, float ros_dt) // turn signal color QColor white_color(Qt::white); white_color.setAlpha(255); - if (signal_type == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT) { + if (signal_type == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT) { painter.setPen(QPen(white_color, static_cast(2), Qt::DotLine)); painter.drawPolygon(left_arrow_polygon_, 7); painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(right_arrow_polygon_, 7); - } else if (signal_type == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT) { + } else if (signal_type == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT) { painter.setPen(QPen(white_color, static_cast(2), Qt::DotLine)); painter.drawPolygon(right_arrow_polygon_, 7); painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(left_arrow_polygon_, 7); - } else if (signal_type == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE) { + } else if (signal_type == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE) { painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(right_arrow_polygon_, 7); diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp index 06a1d2e5f0d54..eb64edd2d595e 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp @@ -24,14 +24,14 @@ #include #include -#include -#include +#include +#include #endif namespace rviz_plugins { class TurnSignalDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -49,7 +49,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::IntProperty * property_left_; rviz_common::properties::IntProperty * property_top_; @@ -62,7 +62,7 @@ private Q_SLOTS: QPointF left_arrow_polygon_[7]; std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp index b5db539f437cb..ccf84d4e64895 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp @@ -102,7 +102,7 @@ void VelocityHistoryDisplay::reset() } bool VelocityHistoryDisplay::validateFloats( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr) { if (!rviz_common::validateFloats(msg_ptr->longitudinal_velocity)) { return false; @@ -120,7 +120,7 @@ void VelocityHistoryDisplay::update(float wall_dt, float ros_dt) } void VelocityHistoryDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp index 9b5819df98bef..96345205289e6 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -36,7 +36,7 @@ namespace rviz_plugins { class VelocityHistoryDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -53,7 +53,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; std::unique_ptr setColorDependsOnVelocity( const double vel_max, const double cmd_vel); std::unique_ptr gradation( @@ -67,11 +67,9 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * property_vel_max_; private: - std::deque< - std::tuple> + std::deque> histories_; - bool validateFloats( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr); + bool validateFloats(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr); std::mutex mutex_; }; diff --git a/common/traffic_light_recognition_marker_publisher/Readme.md b/common/traffic_light_recognition_marker_publisher/Readme.md index 1cafc619ff6fb..d87fc33aefd33 100644 --- a/common/traffic_light_recognition_marker_publisher/Readme.md +++ b/common/traffic_light_recognition_marker_publisher/Readme.md @@ -1,4 +1,4 @@ -# path_distance_calculator +# Traffic Light Recognition Marker Publisher ## Purpose @@ -12,10 +12,10 @@ This node publishes a marker array for visualizing traffic signal recognition re ### Input -| Name | Type | Description | -| ------------------------------------------------------- | -------------------------------------------------------- | ------------------------------------------------- | -| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Vector map for getting traffic signal information | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | The result of traffic signal recognition | +| Name | Type | Description | +| ------------------------------------------------------- | ------------------------------------------------------- | ------------------------------------------------- | +| `/map/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | Vector map for getting traffic signal information | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | The result of traffic signal recognition | ### Output diff --git a/common/traffic_light_recognition_marker_publisher/package.xml b/common/traffic_light_recognition_marker_publisher/package.xml index d12022c01684e..71a3e5238b388 100644 --- a/common/traffic_light_recognition_marker_publisher/package.xml +++ b/common/traffic_light_recognition_marker_publisher/package.xml @@ -12,13 +12,13 @@ ament_cmake autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_perception_msgs + 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/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp index 6076d3fa32f26..5d887a2296137 100644 --- a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp +++ b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp @@ -26,17 +26,17 @@ TrafficLightRecognitionMarkerPublisher::TrafficLightRecognitionMarkerPublisher( { using std::placeholders::_1; - sub_map_ptr_ = this->create_subscription( + sub_map_ptr_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS{1}.transient_local(), std::bind(&TrafficLightRecognitionMarkerPublisher::onMap, this, _1)); - sub_tlr_ptr_ = this->create_subscription( + sub_tlr_ptr_ = this->create_subscription( "~/input/traffic_signals", rclcpp::QoS{1}, std::bind(&TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray, this, _1)); pub_marker_ptr_ = this->create_publisher("~/output/marker", rclcpp::QoS{1}); } -void TrafficLightRecognitionMarkerPublisher::onMap(const HADMapBin::ConstSharedPtr msg_ptr) +void TrafficLightRecognitionMarkerPublisher::onMap(const LaneletMapBin::ConstSharedPtr msg_ptr) { is_map_ready_ = false; lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); @@ -70,10 +70,10 @@ void TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray( } MarkerArray markers; marker_id = 0; - for (const auto & tl : msg_ptr->signals) { - if (tl_position_map_.count(tl.map_primitive_id) == 0) continue; - auto tl_position = tl_position_map_[tl.map_primitive_id]; - for (const auto tl_light : tl.lights) { + for (const auto & tl : msg_ptr->traffic_light_groups) { + if (tl_position_map_.count(tl.traffic_light_group_id) == 0) continue; + auto tl_position = tl_position_map_[tl.traffic_light_group_id]; + for (const auto tl_light : tl.elements) { const auto marker = getTrafficLightMarker(tl_position, tl_light.color, tl_light.shape); markers.markers.emplace_back(marker); marker_id++; diff --git a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp index de9aea14b7e5a..c1cac302647a1 100644 --- a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp +++ b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -32,21 +32,21 @@ class TrafficLightRecognitionMarkerPublisher : public rclcpp::Node { public: - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; - using TrafficSignalArray = autoware_auto_perception_msgs::msg::TrafficSignalArray; - using TrafficLight = autoware_auto_perception_msgs::msg::TrafficLight; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; + using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; + using TrafficLight = autoware_perception_msgs::msg::TrafficLightElement; using MarkerArray = visualization_msgs::msg::MarkerArray; using Pose = geometry_msgs::msg::Pose; explicit TrafficLightRecognitionMarkerPublisher(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_map_ptr_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_map_ptr_; + rclcpp::Subscription::SharedPtr sub_tlr_ptr_; rclcpp::Publisher::SharedPtr pub_marker_ptr_; - void onMap(const HADMapBin::ConstSharedPtr msg_ptr); + void onMap(const LaneletMapBin::ConstSharedPtr msg_ptr); void onTrafficSignalArray(const TrafficSignalArray::ConstSharedPtr msg_ptr); visualization_msgs::msg::Marker getTrafficLightMarker( const Pose & tl_pose, const uint8_t tl_color, const uint8_t tl_shape); diff --git a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp index 9c3acd4f45fa4..cf4ecdd9774b8 100644 --- a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp +++ b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp @@ -15,8 +15,8 @@ #ifndef TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ #define TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ -#include "autoware_perception_msgs/msg/traffic_signal.hpp" -#include "autoware_perception_msgs/msg/traffic_signal_element.hpp" +#include "autoware_perception_msgs/msg/traffic_light_element.hpp" +#include "autoware_perception_msgs/msg/traffic_light_group.hpp" #include "tier4_perception_msgs/msg/traffic_light.hpp" #include "tier4_perception_msgs/msg/traffic_light_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_roi.hpp" @@ -50,7 +50,7 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float c * @return True if a circle-shaped light with the specified color is found, false otherwise. */ bool hasTrafficLightCircleColor( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_color); /** * @brief Checks if a traffic light state includes a light with the specified shape. @@ -62,7 +62,7 @@ bool hasTrafficLightCircleColor( * @return True if a light with the specified shape is found, false otherwise. */ bool hasTrafficLightShape( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_shape); /** * @brief Determines if a traffic signal indicates a stop for the given lanelet. @@ -78,7 +78,7 @@ bool hasTrafficLightShape( */ bool isTrafficSignalStop( const lanelet::ConstLanelet & lanelet, - const autoware_perception_msgs::msg::TrafficSignal & tl_state); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state); tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light); diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index 0bd3d85a9b71f..8aea884510ec2 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -52,11 +52,11 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float c } bool hasTrafficLightCircleColor( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_color) { const auto it_lamp = std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == autoware_perception_msgs::msg::TrafficSignalElement::CIRCLE && + return x.shape == autoware_perception_msgs::msg::TrafficLightElement::CIRCLE && x.color == lamp_color; }); @@ -64,7 +64,7 @@ bool hasTrafficLightCircleColor( } bool hasTrafficLightShape( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_shape) { const auto it_lamp = std::find_if( tl_state.elements.begin(), tl_state.elements.end(), @@ -75,10 +75,10 @@ bool hasTrafficLightShape( bool isTrafficSignalStop( const lanelet::ConstLanelet & lanelet, - const autoware_perception_msgs::msg::TrafficSignal & tl_state) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state) { if (hasTrafficLightCircleColor( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::GREEN)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::GREEN)) { return false; } @@ -90,18 +90,18 @@ bool isTrafficSignalStop( if ( turn_direction == "right" && hasTrafficLightShape( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::RIGHT_ARROW)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW)) { return false; } if ( turn_direction == "left" && hasTrafficLightShape( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::LEFT_ARROW)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::LEFT_ARROW)) { return false; } if ( turn_direction == "straight" && - hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficSignalElement::UP_ARROW)) { + hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficLightElement::UP_ARROW)) { return false; } diff --git a/control/autonomous_emergency_braking/CMakeLists.txt b/control/autonomous_emergency_braking/CMakeLists.txt deleted file mode 100644 index 53a629fafa0cc..0000000000000 --- a/control/autonomous_emergency_braking/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autonomous_emergency_braking) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(PCL REQUIRED) - -include_directories( - include - SYSTEM - ${PCL_INCLUDE_DIRS} -) - -set(AEB_NODE ${PROJECT_NAME}_node) -ament_auto_add_library(${AEB_NODE} SHARED - src/node.cpp -) - -rclcpp_components_register_node(${AEB_NODE} - PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB" - EXECUTABLE ${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/control/autonomous_emergency_braking/README.md b/control/autonomous_emergency_braking/README.md deleted file mode 100644 index 7a7bf212320e0..0000000000000 --- a/control/autonomous_emergency_braking/README.md +++ /dev/null @@ -1,152 +0,0 @@ -# Autonomous Emergency Braking (AEB) - -## Purpose / Role - -`autonomous_emergency_braking` is a module that prevents collisions with obstacles on the predicted path created by a control module or sensor values estimated from the control module. - -### Assumptions - -This module has following assumptions. - -- It is used when driving at low speeds (about 15 km/h). - -- The predicted path of the ego vehicle can be made from either the path created from sensors or the path created from a control module, or both. - -- The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses point cloud as obstacles. - -![aeb_range](./image/range.drawio.svg) - -### Limitations - -- AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud. - -- Longitudinal acceleration information obtained from sensors is not used due to the high amount of noise. - -- The accuracy of the predicted path created from sensor data depends on the accuracy of sensors attached to the ego vehicle. - -## Parameters - -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | -| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | -| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | -| detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | -| detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | -| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | -| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | -| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | -| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | -| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | -| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | -| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | -| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | -| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | -| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | -| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | -| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | -| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | -| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | - -## Inner-workings / Algorithms - -AEB has the following steps before it outputs the emergency stop signal. - -1. Activate AEB if necessary. - -2. Generate a predicted path of the ego vehicle. - -3. Get target obstacles from the input point cloud. - -4. Collision check with target obstacles. - -5. Send emergency stop signals to `/diagnostics`. - -We give more details of each section below. - -### 1. Activate AEB if necessary - -We do not activate AEB module if it satisfies the following conditions. - -- Ego vehicle is not in autonomous driving state - -- When the ego vehicle is not moving (Current Velocity is very low) - -### 2. Generate a predicted path of the ego vehicle - -AEB generates a predicted path based on current velocity and current angular velocity obtained from attached sensors. Note that if `use_imu_path` is `false`, it skips this step. This predicted path is generated as: - -$$ -x_{k+1} = x_k + v cos(\theta_k) dt \\ -y_{k+1} = y_k + v sin(\theta_k) dt \\ -\theta_{k+1} = \theta_k + \omega dt -$$ - -where $v$ and $\omega$ are current longitudinal velocity and angular velocity respectively. $dt$ is time interval that users can define in advance. - -### 3. Get target obstacles from the input point cloud - -After generating the ego predicted path, we select target obstacles from the input point cloud. This obstacle filtering has three major steps, which are rough filtering, noise filtering with clustering and rigorous filtering. - -#### Rough filtering - -In rough filtering step, we select target obstacle with simple filter. Create a search area up to a certain distance (default is half of the ego vehicle width plus the `path_footprint_extra_margin` parameter) away from the predicted path of the ego vehicle and ignore the point cloud that are not within it. The image of the rough filtering is illustrated below. - -![rough_filtering](./image/obstacle_filtering_1.drawio.svg) - -#### Noise filtering with clustering and convex hulls - -To prevent the AEB from considering noisy points, euclidean clustering is performed on the filtered point cloud. The points in the point cloud that are not close enough to other points to form a cluster are discarded. The parameters `cluster_tolerance`, `minimum_cluster_size` and `maximum_cluster_size` can be used to tune the clustering and the size of objects to be ignored, for more information about the clustering method used by the AEB module, please check the official documentation on euclidean clustering of the PCL library: . - -Furthermore, a 2D convex hull is created around each detected cluster, the vertices of each hull represent the most extreme/outside points of the cluster. These vertices are then checked in the next step. - -#### Rigorous filtering - -After Noise filtering, it performs a geometric collision check to determine whether the filtered obstacles/hull vertices actually have possibility to collide with the ego vehicle. In this check, the ego vehicle is represented as a rectangle, and the point cloud obstacles are represented as points. Only the vertices with a possibility of collision are kept. - -![rigorous_filtering](./image/obstacle_filtering_2.drawio.svg) - -Finally, the vertex that is closest to the ego vehicle is chosen as the candidate for collision checking: Since rss distance is used to judge if a collision will happen or not, if the closest vertex to the ego is deemed to be safe, the rest of the vertices (and the points in the clusters) will also be safe. - -#### Obstacle velocity estimation - -Once the position of the closest obstacle/point is determined, the AEB modules uses the history of previously detected objects to estimate the closest object speed using the following equations: - -$$ -d_{t} = o_{time stamp} - prev_{time stamp} -$$ - -$$ -d_{pos} = norm(o_{pos} - prev_{pos}) -$$ - -$$ -v_{norm} = d_{pos} / d_{t} -$$ - -Where $o_{time stamp}$ and $prev_{time stamp}$ are the timestamps of the point clouds used to detect the current closest object and the closest object of the previous point cloud frame, and $o_{pos}$ and $prev_{pos}$ are the positions of those objects, respectively. - -Finally, the velocity vector is compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$: - -$$ -v_{obj} = v_{norm} * Cos(yaw_{diff}) + v_{ego} -$$ - -where $yaw_{diff}$ is the difference in yaw between the ego path and the displacement vector $$v_{pos} = o_{pos} - prev_{pos} $$ and $v_{ego}$ is the ego's speed, which accounts for the movement of points caused by the ego moving and not the object. -All these equations are performed disregarding the z axis (in 2D). - -### 4. Collision check with target obstacles - -In the fourth step, it checks the collision with filtered obstacles using RSS distance. RSS is formulated as: - -$$ -d = v_{ego}*t_{response} + v_{ego}^2/(2*a_{min}) - v_{obj}^2/(2*a_{obj_{min}}) + offset -$$ - -where $v_{ego}$ and $v_{obj}$ is current ego and obstacle velocity, $a_{min}$ and $a_{obj_{min}}$ is ego and object minimum acceleration (maximum deceleration), $t_{response}$ is response time of the ego vehicle to start deceleration. Therefore the distance from the ego vehicle to the obstacle is smaller than this RSS distance $d$, the ego vehicle send emergency stop signals. This is illustrated in the following picture. - -![rss_check](./image/rss_check.drawio.svg) - -### 5. Send emergency stop signals to `/diagnostics` - -If AEB detects collision with point cloud obstacles in the previous step, it sends emergency signal to `/diagnostics` in this step. Note that in order to enable emergency stop, it has to send ERROR level emergency. Moreover, AEB user should modify the setting file to keep the emergency level, otherwise Autoware does not hold the emergency state. diff --git a/control/autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg b/control/autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg deleted file mode 100644 index c1fa2e16ff7fb..0000000000000 --- a/control/autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg +++ /dev/null @@ -1,133 +0,0 @@ - - - - - - - - - - - - - - - -
-
-
- Ego Predicted Path -
-
-
- - Ego Predicted Path - - - - - - - - - - - - - -
-
-
- Pointcloud -
-
-
- - Pointcloud - - - - - - - - -
-
-
- Ignore point outside of the search area -
-
-
- - Ignore point outside of the search area - - - - - - - - - - -
-
-
- Search Area -
-
-
- - Search Area - - - - - - - - -
-
-
- Ignore point behind the ego vehicle -
-
-
- - Ignore point behind the ego vehicle - - - - - - - Text is not SVG - cannot display - - - diff --git a/control/autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg b/control/autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg deleted file mode 100644 index d0340892aa5e9..0000000000000 --- a/control/autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/control/autonomous_emergency_braking/image/rss_check.drawio.svg b/control/autonomous_emergency_braking/image/rss_check.drawio.svg deleted file mode 100644 index 71d6e6ff8932c..0000000000000 --- a/control/autonomous_emergency_braking/image/rss_check.drawio.svg +++ /dev/null @@ -1,76 +0,0 @@ - - - - - - - - - - - - - - - - - - - -
-
-
- obj_dist -
-
-
- - obj_dist - - - - - - - - - - - -
-
-
- - rss_dist -
- -
-
-
- - rss_dist - - - - - - - Text is not SVG - cannot display - - - diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp deleted file mode 100644 index 43fb310b17416..0000000000000 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ /dev/null @@ -1,335 +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 AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ -#define AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -namespace autoware::motion::control::autonomous_emergency_braking -{ - -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_system_msgs::msg::AutowareState; -using autoware_auto_vehicle_msgs::msg::VelocityReport; -using nav_msgs::msg::Odometry; -using sensor_msgs::msg::Imu; -using sensor_msgs::msg::PointCloud2; -using PointCloud = pcl::PointCloud; -using diagnostic_updater::DiagnosticStatusWrapper; -using diagnostic_updater::Updater; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; -using visualization_msgs::msg::Marker; -using visualization_msgs::msg::MarkerArray; -using Path = std::vector; -using Vector3 = geometry_msgs::msg::Vector3; -struct ObjectData -{ - rclcpp::Time stamp; - geometry_msgs::msg::Point position; - double velocity{0.0}; - double rss{0.0}; - double distance_to_object{0.0}; -}; - -class CollisionDataKeeper -{ -public: - explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } - - void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time) - { - collision_keep_time_ = collision_keep_time; - previous_obstacle_keep_time_ = previous_obstacle_keep_time; - } - - std::pair getTimeout() - { - return {collision_keep_time_, previous_obstacle_keep_time_}; - } - - bool checkObjectDataExpired(std::optional & data, const double timeout) - { - if (!data.has_value()) return true; - const auto now = clock_->now(); - const auto & prev_obj = data.value(); - const auto & data_time_stamp = prev_obj.stamp; - if ((now - data_time_stamp).nanoseconds() * 1e-9 > timeout) { - data = std::nullopt; - return true; - } - return false; - } - - bool checkCollisionExpired() - { - return this->checkObjectDataExpired(closest_object_, collision_keep_time_); - } - - bool checkPreviousObjectDataExpired() - { - return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); - } - - ObjectData get() const - { - return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); - } - - ObjectData getPreviousObjectData() const - { - return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); - } - - void setCollisionData(const ObjectData & data) - { - closest_object_ = std::make_optional(data); - } - - void setPreviousObjectData(const ObjectData & data) - { - prev_closest_object_ = std::make_optional(data); - } - - void resetVelocityHistory() { obstacle_velocity_history_.clear(); } - - void updateVelocityHistory( - const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp) - { - // remove old msg from deque - const auto now = clock_->now(); - obstacle_velocity_history_.erase( - std::remove_if( - obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(), - [&](const auto & velocity_time_pair) { - const auto & vel_time = velocity_time_pair.second; - return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_); - }), - obstacle_velocity_history_.end()); - obstacle_velocity_history_.emplace_back( - std::make_pair(current_object_velocity, current_object_velocity_time_stamp)); - } - - std::optional getMedianObstacleVelocity() const - { - if (obstacle_velocity_history_.empty()) return std::nullopt; - std::vector raw_velocities; - for (const auto & vel_time_pair : obstacle_velocity_history_) { - raw_velocities.emplace_back(vel_time_pair.first); - } - - const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1 - : (raw_velocities.size()) / 2.0; - const size_t med2 = (raw_velocities.size()) / 2.0; - std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end()); - const double vel1 = raw_velocities.at(med1); - std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end()); - const double vel2 = raw_velocities.at(med2); - return (vel1 + vel2) / 2.0; - } - - std::optional calcObjectSpeedFromHistory( - const ObjectData & closest_object, const Path & path, const double current_ego_speed) - { - if (this->checkPreviousObjectDataExpired()) { - this->setPreviousObjectData(closest_object); - this->resetVelocityHistory(); - return std::nullopt; - } - - const auto estimated_velocity_opt = std::invoke([&]() -> std::optional { - const auto & prev_object = this->getPreviousObjectData(); - const double p_dt = - (closest_object.stamp.nanoseconds() - prev_object.stamp.nanoseconds()) * 1e-9; - if (p_dt < std::numeric_limits::epsilon()) return std::nullopt; - const auto & nearest_collision_point = closest_object.position; - const auto & prev_collision_point = prev_object.position; - - const double p_dx = nearest_collision_point.x - prev_collision_point.x; - const double p_dy = nearest_collision_point.y - prev_collision_point.y; - const double p_dist = std::hypot(p_dx, p_dy); - 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_path_pose = path.at(nearest_idx); - const auto & traj_yaw = tf2::getYaw(nearest_path_pose.orientation); - const auto estimated_velocity = p_vel * std::cos(p_yaw - traj_yaw) + current_ego_speed; - - // Current RSS distance calculation does not account for negative velocities - return (estimated_velocity > 0.0) ? estimated_velocity : 0.0; - }); - - if (!estimated_velocity_opt.has_value()) { - return std::nullopt; - } - - const auto & estimated_velocity = estimated_velocity_opt.value(); - this->setPreviousObjectData(closest_object); - this->updateVelocityHistory(estimated_velocity, closest_object.stamp); - return this->getMedianObstacleVelocity(); - } - -private: - std::optional prev_closest_object_{std::nullopt}; - std::optional closest_object_{std::nullopt}; - double collision_keep_time_{0.0}; - double previous_obstacle_keep_time_{0.0}; - - std::deque> obstacle_velocity_history_; - rclcpp::Clock::SharedPtr clock_; -}; - -class AEB : public rclcpp::Node -{ -public: - explicit AEB(const rclcpp::NodeOptions & node_options); - - // subscriber - rclcpp::Subscription::SharedPtr sub_point_cloud_; - rclcpp::Subscription::SharedPtr sub_velocity_; - rclcpp::Subscription::SharedPtr sub_imu_; - rclcpp::Subscription::SharedPtr sub_predicted_traj_; - rclcpp::Subscription::SharedPtr sub_autoware_state_; - - // publisher - rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; - rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug - - // timer - rclcpp::TimerBase::SharedPtr timer_; - - // callback - void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); - void onVelocity(const VelocityReport::ConstSharedPtr input_msg); - void onImu(const Imu::ConstSharedPtr input_msg); - void onTimer(); - void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg); - void onAutowareState(const AutowareState::ConstSharedPtr input_msg); - rcl_interfaces::msg::SetParametersResult onParameter( - const std::vector & parameters); - - bool isDataReady(); - - // main function - void onCheckCollision(DiagnosticStatusWrapper & stat); - bool checkCollision(MarkerArray & debug_markers); - bool hasCollision(const double current_v, const ObjectData & closest_object); - - Path generateEgoPath(const double curr_v, const double curr_w); - std::optional generateEgoPath(const Trajectory & predicted_traj); - std::vector generatePathFootprint(const Path & path, const double extra_width_margin); - - void createObjectDataUsingPointCloudClusters( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects, - const pcl::PointCloud::Ptr obstacle_points_ptr); - - void cropPointCloudWithEgoFootprintPath( - const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); - - void createObjectDataUsingPointCloudClusters( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects); - void cropPointCloudWithEgoFootprintPath(const std::vector & ego_polys); - - void addMarker( - const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, - const std::vector & objects, const std::optional & closest_object, - const double color_r, const double color_g, const double color_b, const double color_a, - const std::string & ns, MarkerArray & debug_markers); - - void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); - - std::optional calcObjectSpeedFromHistory( - const ObjectData & closest_object, const Path & path, const double current_ego_speed); - - PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; - VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; - Vector3::SharedPtr angular_velocity_ptr_{nullptr}; - Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; - AutowareState::ConstSharedPtr autoware_state_{nullptr}; - - tf2_ros::Buffer tf_buffer_{get_clock()}; - tf2_ros::TransformListener tf_listener_{tf_buffer_}; - - // vehicle info - VehicleInfo vehicle_info_; - - // diag - Updater updater_{this}; - - // member variables - bool publish_debug_pointcloud_; - bool use_predicted_trajectory_; - bool use_imu_path_; - double path_footprint_extra_margin_; - double detection_range_min_height_; - double detection_range_max_height_margin_; - double voxel_grid_x_; - double voxel_grid_y_; - double voxel_grid_z_; - double min_generated_path_length_; - double expand_width_; - double longitudinal_offset_; - double t_response_; - double a_ego_min_; - double a_obj_min_; - double cluster_tolerance_; - int minimum_cluster_size_; - int maximum_cluster_size_; - double imu_prediction_time_horizon_; - double imu_prediction_time_interval_; - double mpc_prediction_time_horizon_; - double mpc_prediction_time_interval_; - CollisionDataKeeper collision_data_keeper_; - // Parameter callback - OnSetParametersCallbackHandle::SharedPtr set_param_res_; -}; -} // namespace autoware::motion::control::autonomous_emergency_braking - -#endif // AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ diff --git a/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml b/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml deleted file mode 100644 index cf6640ec6be52..0000000000000 --- a/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml deleted file mode 100644 index 1ac255c21921b..0000000000000 --- a/control/autonomous_emergency_braking/package.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - autonomous_emergency_braking - 0.1.0 - Autonomous Emergency Braking package as a ROS 2 node - Takamasa Horibe - Tomoya Kimura - Mamoru Sobue - - Apache License 2.0 - - ament_cmake - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs - diagnostic_updater - geometry_msgs - motion_utils - nav_msgs - pcl_conversions - pcl_ros - pointcloud_preprocessor - rclcpp - rclcpp_components - sensor_msgs - std_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - vehicle_info_util - visualization_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp deleted file mode 100644 index d8886672a8ecd..0000000000000 --- a/control/autonomous_emergency_braking/src/node.cpp +++ /dev/null @@ -1,763 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autonomous_emergency_braking/node.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - -namespace autoware::motion::control::autonomous_emergency_braking -{ -using diagnostic_msgs::msg::DiagnosticStatus; -namespace bg = boost::geometry; - -void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) -{ - Point2d point; - point.x() = geom_point.x; - point.y() = geom_point.y; - - bg::append(polygon.outer(), point); -} - -Polygon2d createPolygon( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) -{ - Polygon2d polygon; - - const double longitudinal_offset = vehicle_info.max_longitudinal_offset_m; - const double width = vehicle_info.vehicle_width_m / 2.0 + expand_width; - const double rear_overhang = vehicle_info.rear_overhang_m; - - appendPointToPolygon( - polygon, - tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); - appendPointToPolygon( - polygon, - tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); - - appendPointToPolygon( - polygon, - tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); - appendPointToPolygon( - polygon, - tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); - appendPointToPolygon( - polygon, tier4_autoware_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 = tier4_autoware_utils::isClockwise(polygon) - ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); - - Polygon2d hull_polygon; - bg::convex_hull(polygon, hull_polygon); - - return hull_polygon; -} - -AEB::AEB(const rclcpp::NodeOptions & node_options) -: Node("AEB", node_options), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), - collision_data_keeper_(this->get_clock()) -{ - // Subscribers - { - sub_point_cloud_ = this->create_subscription( - "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&AEB::onPointCloud, this, std::placeholders::_1)); - - sub_velocity_ = this->create_subscription( - "~/input/velocity", rclcpp::QoS{1}, std::bind(&AEB::onVelocity, this, std::placeholders::_1)); - - sub_imu_ = this->create_subscription( - "~/input/imu", rclcpp::QoS{1}, std::bind(&AEB::onImu, this, std::placeholders::_1)); - - sub_predicted_traj_ = this->create_subscription( - "~/input/predicted_trajectory", rclcpp::QoS{1}, - std::bind(&AEB::onPredictedTrajectory, this, std::placeholders::_1)); - - sub_autoware_state_ = this->create_subscription( - "/autoware/state", rclcpp::QoS{1}, - std::bind(&AEB::onAutowareState, this, std::placeholders::_1)); - } - - // Publisher - { - pub_obstacle_pointcloud_ = - this->create_publisher("~/debug/obstacle_pointcloud", 1); - debug_ego_path_publisher_ = this->create_publisher("~/debug/markers", 1); - } - // Diagnostics - { - updater_.setHardwareID("autonomous_emergency_braking"); - updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision); - } - // parameter - publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); - use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); - use_imu_path_ = declare_parameter("use_imu_path"); - 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_ = - declare_parameter("detection_range_max_height_margin"); - voxel_grid_x_ = declare_parameter("voxel_grid_x"); - voxel_grid_y_ = declare_parameter("voxel_grid_y"); - voxel_grid_z_ = declare_parameter("voxel_grid_z"); - min_generated_path_length_ = declare_parameter("min_generated_path_length"); - expand_width_ = declare_parameter("expand_width"); - longitudinal_offset_ = declare_parameter("longitudinal_offset"); - t_response_ = declare_parameter("t_response"); - a_ego_min_ = declare_parameter("a_ego_min"); - a_obj_min_ = declare_parameter("a_obj_min"); - - cluster_tolerance_ = declare_parameter("cluster_tolerance"); - minimum_cluster_size_ = declare_parameter("minimum_cluster_size"); - maximum_cluster_size_ = declare_parameter("maximum_cluster_size"); - - imu_prediction_time_horizon_ = declare_parameter("imu_prediction_time_horizon"); - imu_prediction_time_interval_ = declare_parameter("imu_prediction_time_interval"); - mpc_prediction_time_horizon_ = declare_parameter("mpc_prediction_time_horizon"); - mpc_prediction_time_interval_ = declare_parameter("mpc_prediction_time_interval"); - - { // Object history data keeper setup - const auto previous_obstacle_keep_time = - declare_parameter("previous_obstacle_keep_time"); - const auto collision_keeping_sec = declare_parameter("collision_keeping_sec"); - collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); - } - - // Parameter Callback - set_param_res_ = - add_on_set_parameters_callback(std::bind(&AEB::onParameter, this, std::placeholders::_1)); - - // start time - const double aeb_hz = declare_parameter("aeb_hz"); - const auto period_ns = rclcpp::Rate(aeb_hz).period(); - timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this)); -} - -rcl_interfaces::msg::SetParametersResult AEB::onParameter( - const std::vector & parameters) -{ - using tier4_autoware_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, "path_footprint_extra_margin", path_footprint_extra_margin_); - updateParam(parameters, "detection_range_min_height", detection_range_min_height_); - updateParam( - parameters, "detection_range_max_height_margin", detection_range_max_height_margin_); - updateParam(parameters, "voxel_grid_x", voxel_grid_x_); - updateParam(parameters, "voxel_grid_y", voxel_grid_y_); - updateParam(parameters, "voxel_grid_z", voxel_grid_z_); - updateParam(parameters, "min_generated_path_length", min_generated_path_length_); - updateParam(parameters, "expand_width", expand_width_); - updateParam(parameters, "longitudinal_offset", longitudinal_offset_); - updateParam(parameters, "t_response", t_response_); - updateParam(parameters, "a_ego_min", a_ego_min_); - updateParam(parameters, "a_obj_min", a_obj_min_); - - updateParam(parameters, "cluster_tolerance", cluster_tolerance_); - updateParam(parameters, "minimum_cluster_size", minimum_cluster_size_); - updateParam(parameters, "maximum_cluster_size", maximum_cluster_size_); - - updateParam(parameters, "imu_prediction_time_horizon", imu_prediction_time_horizon_); - updateParam(parameters, "imu_prediction_time_interval", imu_prediction_time_interval_); - updateParam(parameters, "mpc_prediction_time_horizon", mpc_prediction_time_horizon_); - updateParam(parameters, "mpc_prediction_time_interval", mpc_prediction_time_interval_); - - { // Object history data keeper setup - auto [previous_obstacle_keep_time, collision_keeping_sec] = collision_data_keeper_.getTimeout(); - updateParam(parameters, "previous_obstacle_keep_time", previous_obstacle_keep_time); - updateParam(parameters, "collision_keeping_sec", collision_keeping_sec); - collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); - } - - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; - return result; -} - -void AEB::onTimer() -{ - updater_.force_update(); -} - -void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg) -{ - current_velocity_ptr_ = input_msg; -} - -void AEB::onImu(const Imu::ConstSharedPtr input_msg) -{ - // transform imu - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", input_msg->header.frame_id, input_msg->header.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" << input_msg->header.frame_id); - return; - } - - angular_velocity_ptr_ = std::make_shared(); - tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); -} - -void AEB::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) -{ - predicted_traj_ptr_ = input_msg; -} - -void AEB::onAutowareState(const AutowareState::ConstSharedPtr input_msg) -{ - autoware_state_ = input_msg; -} - -void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) -{ - PointCloud::Ptr pointcloud_ptr(new PointCloud); - pcl::fromROSMsg(*input_msg, *pointcloud_ptr); - - if (input_msg->header.frame_id != "base_link") { - RCLCPP_ERROR_STREAM( - get_logger(), - "[AEB]: Input point cloud frame is not base_link and it is " << input_msg->header.frame_id); - // transform pointcloud - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", input_msg->header.frame_id, input_msg->header.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" << input_msg->header.frame_id); - return; - } - - // transform by using eigen matrix - PointCloud2 transformed_points{}; - const Eigen::Matrix4f affine_matrix = - tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - pcl_ros::transformPointCloud(affine_matrix, *input_msg, transformed_points); - pcl::fromROSMsg(transformed_points, *pointcloud_ptr); - } - - // apply z-axis filter for removing False Positive points - PointCloud::Ptr height_filtered_pointcloud_ptr(new PointCloud); - pcl::PassThrough height_filter; - height_filter.setInputCloud(pointcloud_ptr); - height_filter.setFilterFieldName("z"); - height_filter.setFilterLimits( - detection_range_min_height_, - vehicle_info_.vehicle_height_m + detection_range_max_height_margin_); - height_filter.filter(*height_filtered_pointcloud_ptr); - - pcl::VoxelGrid filter; - PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud); - filter.setInputCloud(height_filtered_pointcloud_ptr); - filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_); - filter.filter(*no_height_filtered_pointcloud_ptr); - - obstacle_ros_pointcloud_ptr_ = std::make_shared(); - - pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); - obstacle_ros_pointcloud_ptr_->header = input_msg->header; -} - -bool AEB::isDataReady() -{ - const auto missing = [this](const auto & name) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name); - return false; - }; - - if (!current_velocity_ptr_) { - return missing("ego velocity"); - } - - if (!obstacle_ros_pointcloud_ptr_) { - return missing("object pointcloud"); - } - - if (use_imu_path_ && !angular_velocity_ptr_) { - return missing("imu"); - } - - if (use_predicted_trajectory_ && !predicted_traj_ptr_) { - return missing("control predicted trajectory"); - } - - if (!autoware_state_) { - return missing("autoware_state"); - } - - return true; -} - -void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) -{ - MarkerArray debug_markers; - checkCollision(debug_markers); - - if (!collision_data_keeper_.checkCollisionExpired()) { - const std::string error_msg = "[AEB]: Emergency Brake"; - const auto diag_level = DiagnosticStatus::ERROR; - stat.summary(diag_level, error_msg); - const auto & data = collision_data_keeper_.get(); - stat.addf("RSS", "%.2f", data.rss); - stat.addf("Distance", "%.2f", data.distance_to_object); - stat.addf("Object Speed", "%.2f", data.velocity); - addCollisionMarker(data, debug_markers); - } else { - const std::string error_msg = "[AEB]: No Collision"; - const auto diag_level = DiagnosticStatus::OK; - stat.summary(diag_level, error_msg); - } - - // publish debug markers - debug_ego_path_publisher_->publish(debug_markers); -} - -bool AEB::checkCollision(MarkerArray & debug_markers) -{ - using colorTuple = std::tuple; - - // step1. check data - if (!isDataReady()) { - return false; - } - - // if not driving, disable aeb - if (autoware_state_->state != AutowareState::DRIVING) { - return false; - } - - // step2. create velocity data check if the vehicle stops or not - const double current_v = current_velocity_ptr_->longitudinal_velocity; - if (current_v < 0.1) { - return false; - } - - auto check_collision = [&]( - 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); - - // 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) { - return o1.distance_to_object < o2.distance_to_object; - }); - - if (closest_object_point_itr == objects_from_point_clusters.end()) { - return std::nullopt; - } - const auto closest_object_speed = collision_data_keeper_.calcObjectSpeedFromHistory( - *closest_object_point_itr, path, current_v); - if (!closest_object_speed.has_value()) { - return std::nullopt; - } - closest_object_point_itr->velocity = closest_object_speed.value(); - return std::make_optional(*closest_object_point_itr); - }); - - // Add 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); - } - // check collision using rss distance - return (closest_object_point.has_value()) - ? hasCollision(current_v, closest_object_point.value()) - : false; - }; - - // step3. make function to check collision with ego path created with sensor data - const auto has_collision_ego = [&](pcl::PointCloud::Ptr filtered_objects) -> bool { - if (!use_imu_path_) return false; - const double current_w = angular_velocity_ptr_->z; - constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; - const std::string ns = "ego"; - const auto ego_path = generateEgoPath(current_v, current_w); - - return check_collision(ego_path, debug_color, ns, filtered_objects); - }; - - // step4. make function to check collision with predicted trajectory from control module - const auto has_collision_predicted = - [&](pcl::PointCloud::Ptr filtered_objects) -> bool { - if (!use_predicted_trajectory_ || !predicted_traj_ptr_) return false; - const auto predicted_traj_ptr = predicted_traj_ptr_; - const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr); - - if (!predicted_path_opt) return false; - constexpr colorTuple debug_color = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}; - const std::string ns = "predicted"; - const auto & predicted_path = predicted_path_opt.value(); - - return check_collision(predicted_path, debug_color, ns, filtered_objects); - }; - - // Data of filtered point cloud - pcl::PointCloud::Ptr filtered_objects(new PointCloud); - // evaluate if there is a collision for both paths - const bool has_collision = - has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects); - - // 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_); - } - return has_collision; -} - -bool AEB::hasCollision(const double current_v, const ObjectData & closest_object) -{ - const double & obj_v = closest_object.velocity; - const double & t = t_response_; - const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - - obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; - if (closest_object.distance_to_object < rss_dist) { - // collision happens - ObjectData collision_data = closest_object; - collision_data.rss = rss_dist; - collision_data.distance_to_object = closest_object.distance_to_object; - collision_data_keeper_.setCollisionData(collision_data); - return true; - } - return false; -} - -Path AEB::generateEgoPath(const double curr_v, const double curr_w) -{ - Path path; - double curr_x = 0.0; - 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); - path.push_back(ini_pose); - - if (curr_v < 0.1) { - // if current velocity is too small, assume it stops at the same point - return path; - } - - constexpr double epsilon = 1e-6; - const double & dt = imu_prediction_time_interval_; - const double & horizon = imu_prediction_time_horizon_; - for (double t = 0.0; t < horizon + epsilon; t += dt) { - 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) { - continue; - } - path.push_back(current_pose); - } - - // If path is shorter than minimum path length - while (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) { - continue; - } - path.push_back(current_pose); - } - return path; -} - -std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) -{ - if (predicted_traj.points.empty()) { - return std::nullopt; - } - - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", predicted_traj.header.frame_id, predicted_traj.header.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 std::nullopt; - } - - // create path - Path path; - path.resize(predicted_traj.points.size()); - for (size_t i = 0; i < predicted_traj.points.size(); ++i) { - geometry_msgs::msg::Pose map_pose; - tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); - path.at(i) = map_pose; - - if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { - break; - } - } - return path; -} - -std::vector AEB::generatePathFootprint( - const Path & path, const double extra_width_margin) -{ - std::vector polygons; - for (size_t i = 0; i < path.size() - 1; ++i) { - polygons.push_back( - createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); - } - return polygons; -} - -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) -{ - // check if the predicted path has valid number of points - if (ego_path.size() < 2 || ego_polys.empty() || obstacle_points_ptr->empty()) { - return; - } - - // eliminate noisy points by only considering points belonging to clusters of at least a certain - // size - const std::vector cluster_indices = std::invoke([&]() { - std::vector cluster_idx; - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - tree->setInputCloud(obstacle_points_ptr); - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(cluster_tolerance_); - ec.setMinClusterSize(minimum_cluster_size_); - ec.setMaxClusterSize(maximum_cluster_size_); - ec.setSearchMethod(tree); - ec.setInputCloud(obstacle_points_ptr); - ec.extract(cluster_idx); - return cluster_idx; - }); - - PointCloud::Ptr points_belonging_to_cluster_hulls(new PointCloud); - for (const auto & indices : cluster_indices) { - PointCloud::Ptr cluster(new PointCloud); - for (const auto & index : indices.indices) { - cluster->push_back((*obstacle_points_ptr)[index]); - } - // Make a 2d convex hull for the objects - pcl::ConvexHull hull; - hull.setDimension(2); - hull.setInputCloud(cluster); - std::vector polygons; - PointCloud::Ptr surface_hull(new pcl::PointCloud); - hull.reconstruct(*surface_hull, polygons); - for (const auto & p : *surface_hull) { - points_belonging_to_cluster_hulls->push_back(p); - } - } - - // select points inside the ego footprint path - const auto current_p = tier4_autoware_utils::createPoint( - ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.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 double dist_ego_to_object = - motion_utils::calcSignedArcLength(ego_path, current_p, obj_position) - - vehicle_info_.max_longitudinal_offset_m; - // objects behind ego are ignored - if (dist_ego_to_object < 0.0) continue; - - ObjectData obj; - obj.stamp = stamp; - obj.position = obj_position; - obj.velocity = 0.0; - obj.distance_to_object = dist_ego_to_object; - - const Point2d obj_point(p.x, p.y); - for (const auto & ego_poly : ego_polys) { - if (bg::within(obj_point, ego_poly)) { - objects.push_back(obj); - break; - } - } - } -} - -void AEB::cropPointCloudWithEgoFootprintPath( - const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects) -{ - PointCloud::Ptr full_points_ptr(new PointCloud); - pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr); - // Create a Point cloud with the points of the ego footprint - PointCloud::Ptr path_polygon_points(new PointCloud); - std::for_each(ego_polys.begin(), ego_polys.end(), [&](const auto & poly) { - std::for_each(poly.outer().begin(), poly.outer().end(), [&](const auto & p) { - pcl::PointXYZ point(p.x(), p.y(), 0.0); - path_polygon_points->push_back(point); - }); - }); - // Make a surface hull with the ego footprint to filter out points - pcl::ConvexHull hull; - hull.setDimension(2); - hull.setInputCloud(path_polygon_points); - std::vector polygons; - pcl::PointCloud::Ptr surface_hull(new pcl::PointCloud); - hull.reconstruct(*surface_hull, polygons); - // Filter out points outside of the path's convex hull - pcl::CropHull path_polygon_hull_filter; - path_polygon_hull_filter.setDim(2); - path_polygon_hull_filter.setInputCloud(full_points_ptr); - path_polygon_hull_filter.setHullIndices(polygons); - path_polygon_hull_filter.setHullCloud(surface_hull); - path_polygon_hull_filter.filter(*filtered_objects); - filtered_objects->header.stamp = full_points_ptr->header.stamp; -} - -void AEB::addMarker( - const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, - const std::vector & objects, const std::optional & closest_object, - 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( - "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)); - 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( - "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)); - 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); - 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( - "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)); - for (const auto & e : objects) { - object_data_marker.points.push_back(e.position); - } - debug_markers.markers.push_back(object_data_marker); - - // 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( - "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); - closest_object_velocity_marker_array.pose.position = obj.position; - const auto ego_velocity = current_velocity_ptr_->longitudinal_velocity; - closest_object_velocity_marker_array.text = - "Object velocity: " + std::to_string(obj.velocity) + " [m/s]\n"; - closest_object_velocity_marker_array.text += - "Object relative velocity to ego: " + std::to_string(obj.velocity - ego_velocity) + " [m/s]"; - debug_markers.markers.push_back(closest_object_velocity_marker_array); - } -} - -void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) -{ - auto point_marker = tier4_autoware_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)); - point_marker.pose.position = data.position; - debug_markers.markers.push_back(point_marker); -} - -} // namespace autoware::motion::control::autonomous_emergency_braking - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::autonomous_emergency_braking::AEB) diff --git a/control/autoware_autonomous_emergency_braking/CMakeLists.txt b/control/autoware_autonomous_emergency_braking/CMakeLists.txt new file mode 100644 index 0000000000000..7f38e4387b452 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_autonomous_emergency_braking) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED) + +include_directories( + include + SYSTEM + ${PCL_INCLUDE_DIRS} +) + +set(AEB_NODE ${PROJECT_NAME}_node) +ament_auto_add_library(${AEB_NODE} SHARED + src/node.cpp +) + +rclcpp_components_register_node(${AEB_NODE} + PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB" + EXECUTABLE ${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md new file mode 100644 index 0000000000000..2f3f5f08d4156 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -0,0 +1,200 @@ +# Autonomous Emergency Braking (AEB) + +## Purpose / Role + +`autonomous_emergency_braking` is a module that prevents collisions with obstacles on the predicted path created by a control module or sensor values estimated from the control module. + +### Assumptions + +This module has following assumptions. + +- It is used when driving at low speeds (about 15 km/h). + +- The predicted path of the ego vehicle can be made from either the path created from sensors or the path created from a control module, or both. + +- The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses point cloud as obstacles. + +### IMU path generation: steering angle vs IMU's angular velocity + +Currently, the IMU-based path is generated using the angular velocity obtained by the IMU itself. It has been suggested that the steering angle could be used instead onf the angular velocity. + +The pros and cons of both approaches are: + +IMU angular velocity: + +- (+) Usually, it has high accuracy +- (-)Vehicle vibration might introduce noise. + +Steering angle: + +- (+) Not so noisy +- (-) May have a steering offset or a wrong gear ratio, and the steering angle of Autoware and the real steering may not be the same. + +For the moment, there are no plans to implement the steering angle on the path creation process of the AEB module. + +## Inner-workings / Algorithms + +AEB has the following steps before it outputs the emergency stop signal. + +1. Activate AEB if necessary. + +2. Generate a predicted path of the ego vehicle. + +3. Get target obstacles from the input point cloud. + +4. Collision check with target obstacles. + +5. Send emergency stop signals to `/diagnostics`. + +We give more details of each section below. + +### 1. Activate AEB if necessary + +We do not activate AEB module if it satisfies the following conditions. + +- Ego vehicle is not in autonomous driving state + +- When the ego vehicle is not moving (Current Velocity is very low) + +### 2. Generate a predicted path of the ego vehicle + +AEB generates a predicted path based on current velocity and current angular velocity obtained from attached sensors. Note that if `use_imu_path` is `false`, it skips this step. This predicted path is generated as: + +$$ +x_{k+1} = x_k + v cos(\theta_k) dt \\ +y_{k+1} = y_k + v sin(\theta_k) dt \\ +\theta_{k+1} = \theta_k + \omega dt +$$ + +where $v$ and $\omega$ are current longitudinal velocity and angular velocity respectively. $dt$ is time interval that users can define in advance. + +### 3. Get target obstacles from the input point cloud + +After generating the ego predicted path, we select target obstacles from the input point cloud. This obstacle filtering has three major steps, which are rough filtering, noise filtering with clustering and rigorous filtering. + +#### Rough filtering + +In rough filtering step, we select target obstacle with simple filter. Create a search area up to a certain distance (default is half of the ego vehicle width plus the `path_footprint_extra_margin` parameter) away from the predicted path of the ego vehicle and ignore the point cloud that are not within it. The image of the rough filtering is illustrated below. + +![rough_filtering](./image/obstacle_filtering_1.drawio.svg) + +#### Noise filtering with clustering and convex hulls + +To prevent the AEB from considering noisy points, euclidean clustering is performed on the filtered point cloud. The points in the point cloud that are not close enough to other points to form a cluster are discarded. The parameters `cluster_tolerance`, `minimum_cluster_size` and `maximum_cluster_size` can be used to tune the clustering and the size of objects to be ignored, for more information about the clustering method used by the AEB module, please check the official documentation on euclidean clustering of the PCL library: . + +Furthermore, a 2D convex hull is created around each detected cluster, the vertices of each hull represent the most extreme/outside points of the cluster. These vertices are then checked in the next step. + +#### Rigorous filtering + +After Noise filtering, the module performs a geometric collision check to determine whether the filtered obstacles/hull vertices actually have possibility to collide with the ego vehicle. In this check, the ego vehicle is represented as a rectangle, and the point cloud obstacles are represented as points. Only the vertices with a possibility of collision are kept. Finally, the vertex that is closest to the ego vehicle is chosen as the candidate for collision checking: Since rss distance is used to judge if a collision will happen or not, if the closest vertex to the ego is deemed to be safe, the rest of the vertices (and the points in the clusters) will also be safe. + +![rigorous_filtering](./image/obstacle_filtering_2.drawio.svg) + +#### Obstacle velocity estimation + +Once the position of the closest obstacle/point is determined, the AEB modules uses the history of previously detected objects to estimate the closest object relative speed using the following equations: + +$$ +d_{t} = t_{1} - t_{0} +$$ + +$$ +d_{x} = norm(o_{x} - prev_{x}) +$$ + +$$ +v_{norm} = d_{x} / d_{t} +$$ + +Where $t_{1}$ and $t_{0}$ are the timestamps of the point clouds used to detect the current closest object and the closest object of the previous point cloud frame, and $o_{x}$ and $prev_{x}$ are the positions of those objects, respectively. + +![relative_speed](./image/object_relative_speed.drawio.svg) + +Finally, the velocity vector is compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$: + +$$ +v_{obj} = v_{norm} * Cos(yaw_{diff}) + v_{ego} +$$ + +where $yaw_{diff}$ is the difference in yaw between the ego path and the displacement vector $$v_{pos} = o_{pos} - prev_{pos} $$ and $v_{ego}$ is the ego's current speed, which accounts for the movement of points caused by the ego moving and not the object. All these equations are performed disregarding the z axis (in 2D). + +Note that, the object velocity is calculated against the ego's current movement direction. If the object moves in the opposite direction to the ego's movement, the object velocity is set to 0 m/s. That is because the RSS distance calculation assumes the ego and the object move in the same direction and it cannot deal with negative velocities. + +### 4. Collision check with target obstacles using RSS distance + +In the fourth step, it checks the collision with the closest obstacle point using RSS distance. RSS distance is formulated as: + +$$ +d = v_{ego}*t_{response} + v_{ego}^2/(2*a_{min}) - v_{obj}^2/(2*a_{obj_{min}}) + offset +$$ + +where $v_{ego}$ and $v_{obj}$ is current ego and obstacle velocity, $a_{min}$ and $a_{obj_{min}}$ is ego and object minimum acceleration (maximum deceleration), $t_{response}$ is response time of the ego vehicle to start deceleration. Therefore the distance from the ego vehicle to the obstacle is smaller than this RSS distance $d$, the ego vehicle send emergency stop signals. This is illustrated in the following picture. + +![rss_check](./image/rss_check.drawio.svg) + +### 5. Send emergency stop signals to `/diagnostics` + +If AEB detects collision with point cloud obstacles in the previous step, it sends emergency signal to `/diagnostics` in this step. Note that in order to enable emergency stop, it has to send ERROR level emergency. Moreover, AEB user should modify the setting file to keep the emergency level, otherwise Autoware does not hold the emergency state. + +## Use cases + +### Front vehicle suddenly brakes + +The AEB can activate when a vehicle in front suddenly brakes, and a collision is detected by the AEB module. Provided the distance between the ego vehicle and the front vehicle is large enough and the ego’s emergency acceleration value is high enough, it is possible to avoid or soften collisions with vehicles in front that suddenly brake. NOTE: the acceleration used by the AEB to calculate rss_distance is NOT necessarily the acceleration used by the ego while doing an emergency brake. The acceleration used by the real vehicle can be tuned by changing the [mrm_emergency stop jerk and acceleration values](https://github.com/tier4/autoware_launch/blob/d1b2688f2788acab95bb9995d72efd7182e9006a/autoware_launch/config/system/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml#L4). + +![front vehicle collision prevention](./image/front_vehicle_collision.drawio.svg) + +### Stop for objects that appear suddenly + +When an object appears suddenly, the AEB can act as a fail-safe to stop the ego vehicle when other modules fail to detect the object on time. If sudden object cut ins are expected, it might be useful for the AEB module to detect collisions of objects BEFORE they enter the real ego vehicle path by increasing the `expand_width` parameter. + +![occluded object collision prevention](./image/occluded_space.drawio.svg) + +### Preventing Collisions with rear objects + +The AEB module can also prevent collisions when the ego vehicle is moving backwards. + +![backward driving](./image/backward-driving.drawio.svg) + +### Preventing collisions in case of wrong Odometry (IMU path only) + +When vehicle odometry information is faulty, it is possible that the MPC fails to predict a correct path for the ego vehicle. If the MPC predicted path is wrong, collision avoidance will not work as intended on the planning modules. However, the AEB’s IMU path does not depend on the MPC and could be able to predict a collision when the other modules cannot. As an example you can see a figure of a hypothetical case in which the MPC path is wrong and only the AEB’s IMU path detects a collision. + +![backward driving](./image/wrong-mpc.drawio.svg) + +## Parameters + +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | +| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | +| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | +| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | +| detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | +| detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | +| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | +| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | +| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | +| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | +| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | +| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | +| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | +| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | +| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | +| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | +| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | +| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | +| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | + +## Limitations + +- AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud. + +- Longitudinal acceleration information obtained from sensors is not used due to the high amount of noise. + +- The accuracy of the predicted path created from sensor data depends on the accuracy of sensors attached to the ego vehicle. + +- Currently, the module calculates thee closest object velocity if it goes in the same direction as the ego vehicle, otherwise the velocity is set to 0 m/s since RSS distance calculation does not use negative velocities. + +![aeb_range](./image/range.drawio.svg) diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml similarity index 95% rename from control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml rename to control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 68a42383feb1f..4233f8b6bebcb 100644 --- a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -3,6 +3,7 @@ # Ego path calculation use_predicted_trajectory: true use_imu_path: false + use_object_velocity_calculation: true min_generated_path_length: 0.5 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 diff --git a/control/autoware_autonomous_emergency_braking/image/backward-driving.drawio.svg b/control/autoware_autonomous_emergency_braking/image/backward-driving.drawio.svg new file mode 100644 index 0000000000000..4897078d28f00 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/backward-driving.drawio.svg @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 0000000000000..ee5ec62fbc1d4 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg @@ -0,0 +1,300 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
Original Point Cloud
+
+
+ + + + + + + + + + + + +
+
+
+ Cropping the Point Cloud with extended path +
+
+
+ + + + + + + + + + + + + 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 new file mode 100644 index 0000000000000..1d12fe233ad8e --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg @@ -0,0 +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/autonomous_emergency_braking/image/range.drawio.svg b/control/autoware_autonomous_emergency_braking/image/range.drawio.svg similarity index 100% rename from control/autonomous_emergency_braking/image/range.drawio.svg rename to control/autoware_autonomous_emergency_braking/image/range.drawio.svg diff --git a/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg new file mode 100644 index 0000000000000..2d1716519631d --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg @@ -0,0 +1,134 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + Closest Point Distance +
+
+
+ + + + + + + + + + + + + + + + + +
+
+
RSS distance
+
+
+ + + + + + + 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 new file mode 100644 index 0000000000000..7e06af73b0f11 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -0,0 +1,342 @@ +// 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 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 +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +namespace autoware::motion::control::autonomous_emergency_braking +{ + +using autoware_planning_msgs::msg::Trajectory; +using autoware_system_msgs::msg::AutowareState; +using autoware_vehicle_msgs::msg::VelocityReport; +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 visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +using Path = std::vector; +using Vector3 = geometry_msgs::msg::Vector3; +struct ObjectData +{ + rclcpp::Time stamp; + geometry_msgs::msg::Point position; + double velocity{0.0}; + double rss{0.0}; + double distance_to_object{0.0}; +}; + +class CollisionDataKeeper +{ +public: + explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } + + void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time) + { + collision_keep_time_ = collision_keep_time; + previous_obstacle_keep_time_ = previous_obstacle_keep_time; + } + + std::pair getTimeout() + { + return {collision_keep_time_, previous_obstacle_keep_time_}; + } + + bool checkObjectDataExpired(std::optional & data, const double timeout) + { + if (!data.has_value()) return true; + const auto now = clock_->now(); + const auto & prev_obj = data.value(); + const auto & data_time_stamp = prev_obj.stamp; + if ((now - data_time_stamp).nanoseconds() * 1e-9 > timeout) { + data = std::nullopt; + return true; + } + return false; + } + + bool checkCollisionExpired() + { + return this->checkObjectDataExpired(closest_object_, collision_keep_time_); + } + + bool checkPreviousObjectDataExpired() + { + return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); + } + + ObjectData get() const + { + return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); + } + + ObjectData getPreviousObjectData() const + { + return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); + } + + void setCollisionData(const ObjectData & data) + { + closest_object_ = std::make_optional(data); + } + + void setPreviousObjectData(const ObjectData & data) + { + prev_closest_object_ = std::make_optional(data); + } + + void resetVelocityHistory() { obstacle_velocity_history_.clear(); } + + void updateVelocityHistory( + const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp) + { + // remove old msg from deque + const auto now = clock_->now(); + obstacle_velocity_history_.erase( + std::remove_if( + obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(), + [&](const auto & velocity_time_pair) { + const auto & vel_time = velocity_time_pair.second; + return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_); + }), + obstacle_velocity_history_.end()); + obstacle_velocity_history_.emplace_back( + std::make_pair(current_object_velocity, current_object_velocity_time_stamp)); + } + + std::optional getMedianObstacleVelocity() const + { + if (obstacle_velocity_history_.empty()) return std::nullopt; + std::vector raw_velocities; + for (const auto & vel_time_pair : obstacle_velocity_history_) { + raw_velocities.emplace_back(vel_time_pair.first); + } + + const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1 + : (raw_velocities.size()) / 2.0; + const size_t med2 = (raw_velocities.size()) / 2.0; + std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end()); + const double vel1 = raw_velocities.at(med1); + std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end()); + const double vel2 = raw_velocities.at(med2); + return (vel1 + vel2) / 2.0; + } + + std::optional calcObjectSpeedFromHistory( + const ObjectData & closest_object, const Path & path, const double current_ego_speed) + { + if (this->checkPreviousObjectDataExpired()) { + this->setPreviousObjectData(closest_object); + this->resetVelocityHistory(); + return std::nullopt; + } + + const auto estimated_velocity_opt = std::invoke([&]() -> std::optional { + const auto & prev_object = this->getPreviousObjectData(); + const double p_dt = + (closest_object.stamp.nanoseconds() - prev_object.stamp.nanoseconds()) * 1e-9; + if (p_dt < std::numeric_limits::epsilon()) return std::nullopt; + const auto & nearest_collision_point = closest_object.position; + const auto & prev_collision_point = prev_object.position; + + const double p_dx = nearest_collision_point.x - prev_collision_point.x; + const double p_dy = nearest_collision_point.y - prev_collision_point.y; + const double p_dist = std::hypot(p_dx, p_dy); + const double p_yaw = std::atan2(p_dy, p_dx); + const double p_vel = p_dist / p_dt; + + 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) + ? tf2::getYaw(nearest_path_pose.orientation) + : tf2::getYaw(nearest_path_pose.orientation) + M_PI; + const auto estimated_velocity = + p_vel * std::cos(p_yaw - traj_yaw) + std::abs(current_ego_speed); + + // Current RSS distance calculation does not account for negative velocities + return (estimated_velocity > 0.0) ? estimated_velocity : 0.0; + }); + + if (!estimated_velocity_opt.has_value()) { + return std::nullopt; + } + + const auto & estimated_velocity = estimated_velocity_opt.value(); + this->setPreviousObjectData(closest_object); + this->updateVelocityHistory(estimated_velocity, closest_object.stamp); + return this->getMedianObstacleVelocity(); + } + +private: + std::optional prev_closest_object_{std::nullopt}; + std::optional closest_object_{std::nullopt}; + double collision_keep_time_{0.0}; + double previous_obstacle_keep_time_{0.0}; + + std::deque> obstacle_velocity_history_; + rclcpp::Clock::SharedPtr clock_; +}; + +class AEB : public rclcpp::Node +{ +public: + explicit AEB(const rclcpp::NodeOptions & node_options); + + // subscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_point_cloud_{ + this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; + autoware::universe_utils::InterProcessPollingSubscriber sub_velocity_{ + this, "~/input/velocity"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_traj_{ + this, "~/input/predicted_trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ + this, "/autoware/state"}; + // publisher + rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; + rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug + + // timer + rclcpp::TimerBase::SharedPtr timer_; + + // callback + void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); + void onImu(const Imu::ConstSharedPtr input_msg); + void onTimer(); + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + + bool fetchLatestData(); + + // main function + void onCheckCollision(DiagnosticStatusWrapper & stat); + bool checkCollision(MarkerArray & debug_markers); + bool hasCollision(const double current_v, const ObjectData & closest_object); + + Path generateEgoPath(const double curr_v, const double curr_w); + std::optional generateEgoPath(const Trajectory & predicted_traj); + std::vector generatePathFootprint(const Path & path, const double extra_width_margin); + + void createObjectDataUsingPointCloudClusters( + const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + std::vector & objects, + const pcl::PointCloud::Ptr obstacle_points_ptr); + + void cropPointCloudWithEgoFootprintPath( + const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); + + void createObjectDataUsingPointCloudClusters( + const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + std::vector & objects); + void cropPointCloudWithEgoFootprintPath(const std::vector & ego_polys); + + void addMarker( + const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, + const std::vector & objects, const std::optional & closest_object, + const double color_r, const double color_g, const double color_b, const double color_a, + const std::string & ns, MarkerArray & debug_markers); + + void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); + + std::optional calcObjectSpeedFromHistory( + const ObjectData & closest_object, const Path & path, const double current_ego_speed); + + PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; + VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; + Vector3::SharedPtr angular_velocity_ptr_{nullptr}; + Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; + AutowareState::ConstSharedPtr autoware_state_{nullptr}; + + tf2_ros::Buffer tf_buffer_{get_clock()}; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + + // vehicle info + VehicleInfo vehicle_info_; + + // diag + Updater updater_{this}; + + // member variables + bool publish_debug_pointcloud_; + bool use_predicted_trajectory_; + bool use_imu_path_; + bool use_object_velocity_calculation_; + double path_footprint_extra_margin_; + double detection_range_min_height_; + double detection_range_max_height_margin_; + double voxel_grid_x_; + double voxel_grid_y_; + double voxel_grid_z_; + double min_generated_path_length_; + double expand_width_; + double longitudinal_offset_; + double t_response_; + double a_ego_min_; + double a_obj_min_; + double cluster_tolerance_; + int minimum_cluster_size_; + int maximum_cluster_size_; + double imu_prediction_time_horizon_; + double imu_prediction_time_interval_; + double mpc_prediction_time_horizon_; + double mpc_prediction_time_interval_; + CollisionDataKeeper collision_data_keeper_; + // Parameter callback + OnSetParametersCallbackHandle::SharedPtr set_param_res_; +}; +} // namespace autoware::motion::control::autonomous_emergency_braking + +#endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_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 new file mode 100644 index 0000000000000..75b1a9dcf822d --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml new file mode 100644 index 0000000000000..c822adb510805 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -0,0 +1,46 @@ + + + + autoware_autonomous_emergency_braking + 0.1.0 + Autonomous Emergency Braking package as a ROS 2 node + Takamasa Horibe + Tomoya Kimura + Mamoru Sobue + Daniel Sanchez + + Apache License 2.0 + + ament_cmake + 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 + nav_msgs + pcl_conversions + pcl_ros + pointcloud_preprocessor + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp new file mode 100644 index 0000000000000..90cf3f900673e --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -0,0 +1,764 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "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 +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +namespace autoware::motion::control::autonomous_emergency_braking +{ +using diagnostic_msgs::msg::DiagnosticStatus; +namespace bg = boost::geometry; + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + bg::append(polygon.outer(), point); +} + +Polygon2d createPolygon( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) +{ + Polygon2d polygon; + + const double longitudinal_offset = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + expand_width; + const double rear_overhang = vehicle_info.rear_overhang_m; + + appendPointToPolygon( + polygon, + autoware::universe_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); + appendPointToPolygon( + polygon, + autoware::universe_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); + appendPointToPolygon( + polygon, + autoware::universe_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + autoware::universe_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); + + appendPointToPolygon( + polygon, + autoware::universe_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); + appendPointToPolygon( + polygon, + autoware::universe_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); + appendPointToPolygon( + polygon, + autoware::universe_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + autoware::universe_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); + + polygon = autoware::universe_utils::isClockwise(polygon) + ? polygon + : autoware::universe_utils::inverseClockwise(polygon); + + Polygon2d hull_polygon; + bg::convex_hull(polygon, hull_polygon); + + return hull_polygon; +} + +AEB::AEB(const rclcpp::NodeOptions & node_options) +: Node("AEB", node_options), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), + collision_data_keeper_(this->get_clock()) +{ + // Publisher + { + pub_obstacle_pointcloud_ = + this->create_publisher("~/debug/obstacle_pointcloud", 1); + debug_ego_path_publisher_ = this->create_publisher("~/debug/markers", 1); + } + // Diagnostics + { + updater_.setHardwareID("autonomous_emergency_braking"); + updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision); + } + // parameter + 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_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_ = + declare_parameter("detection_range_max_height_margin"); + voxel_grid_x_ = declare_parameter("voxel_grid_x"); + voxel_grid_y_ = declare_parameter("voxel_grid_y"); + voxel_grid_z_ = declare_parameter("voxel_grid_z"); + min_generated_path_length_ = declare_parameter("min_generated_path_length"); + expand_width_ = declare_parameter("expand_width"); + longitudinal_offset_ = declare_parameter("longitudinal_offset"); + t_response_ = declare_parameter("t_response"); + a_ego_min_ = declare_parameter("a_ego_min"); + a_obj_min_ = declare_parameter("a_obj_min"); + + cluster_tolerance_ = declare_parameter("cluster_tolerance"); + minimum_cluster_size_ = declare_parameter("minimum_cluster_size"); + maximum_cluster_size_ = declare_parameter("maximum_cluster_size"); + + imu_prediction_time_horizon_ = declare_parameter("imu_prediction_time_horizon"); + imu_prediction_time_interval_ = declare_parameter("imu_prediction_time_interval"); + mpc_prediction_time_horizon_ = declare_parameter("mpc_prediction_time_horizon"); + mpc_prediction_time_interval_ = declare_parameter("mpc_prediction_time_interval"); + + { // Object history data keeper setup + const auto previous_obstacle_keep_time = + declare_parameter("previous_obstacle_keep_time"); + const auto collision_keeping_sec = declare_parameter("collision_keeping_sec"); + collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); + } + + // Parameter Callback + set_param_res_ = + add_on_set_parameters_callback(std::bind(&AEB::onParameter, this, std::placeholders::_1)); + + // start time + const double aeb_hz = declare_parameter("aeb_hz"); + const auto period_ns = rclcpp::Rate(aeb_hz).period(); + timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this)); +} + +rcl_interfaces::msg::SetParametersResult AEB::onParameter( + const std::vector & parameters) +{ + 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_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( + parameters, "detection_range_max_height_margin", detection_range_max_height_margin_); + updateParam(parameters, "voxel_grid_x", voxel_grid_x_); + updateParam(parameters, "voxel_grid_y", voxel_grid_y_); + updateParam(parameters, "voxel_grid_z", voxel_grid_z_); + updateParam(parameters, "min_generated_path_length", min_generated_path_length_); + updateParam(parameters, "expand_width", expand_width_); + updateParam(parameters, "longitudinal_offset", longitudinal_offset_); + updateParam(parameters, "t_response", t_response_); + updateParam(parameters, "a_ego_min", a_ego_min_); + updateParam(parameters, "a_obj_min", a_obj_min_); + + updateParam(parameters, "cluster_tolerance", cluster_tolerance_); + updateParam(parameters, "minimum_cluster_size", minimum_cluster_size_); + updateParam(parameters, "maximum_cluster_size", maximum_cluster_size_); + + updateParam(parameters, "imu_prediction_time_horizon", imu_prediction_time_horizon_); + updateParam(parameters, "imu_prediction_time_interval", imu_prediction_time_interval_); + updateParam(parameters, "mpc_prediction_time_horizon", mpc_prediction_time_horizon_); + updateParam(parameters, "mpc_prediction_time_interval", mpc_prediction_time_interval_); + + { // Object history data keeper setup + auto [previous_obstacle_keep_time, collision_keeping_sec] = collision_data_keeper_.getTimeout(); + updateParam(parameters, "previous_obstacle_keep_time", previous_obstacle_keep_time); + updateParam(parameters, "collision_keeping_sec", collision_keeping_sec); + collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + +void AEB::onTimer() +{ + updater_.force_update(); +} + +void AEB::onImu(const Imu::ConstSharedPtr input_msg) +{ + // transform imu + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", input_msg->header.frame_id, input_msg->header.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" << input_msg->header.frame_id); + return; + } + + angular_velocity_ptr_ = std::make_shared(); + tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); +} + +void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) +{ + PointCloud::Ptr pointcloud_ptr(new PointCloud); + pcl::fromROSMsg(*input_msg, *pointcloud_ptr); + + if (input_msg->header.frame_id != "base_link") { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB]: Input point cloud frame is not base_link and it is " << input_msg->header.frame_id); + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", input_msg->header.frame_id, input_msg->header.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" << input_msg->header.frame_id); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *input_msg, transformed_points); + pcl::fromROSMsg(transformed_points, *pointcloud_ptr); + } + + // apply z-axis filter for removing False Positive points + PointCloud::Ptr height_filtered_pointcloud_ptr(new PointCloud); + pcl::PassThrough height_filter; + height_filter.setInputCloud(pointcloud_ptr); + height_filter.setFilterFieldName("z"); + height_filter.setFilterLimits( + detection_range_min_height_, + vehicle_info_.vehicle_height_m + detection_range_max_height_margin_); + height_filter.filter(*height_filtered_pointcloud_ptr); + + pcl::VoxelGrid filter; + PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud); + filter.setInputCloud(height_filtered_pointcloud_ptr); + filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_); + filter.filter(*no_height_filtered_pointcloud_ptr); + + obstacle_ros_pointcloud_ptr_ = std::make_shared(); + + pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); + obstacle_ros_pointcloud_ptr_->header = input_msg->header; +} + +bool AEB::fetchLatestData() +{ + const auto missing = [this](const auto & name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name); + return false; + }; + + current_velocity_ptr_ = sub_velocity_.takeData(); + if (!current_velocity_ptr_) { + return missing("ego velocity"); + } + + 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"); + } + + const auto imu_ptr = sub_imu_.takeData(); + if (use_imu_path_) { + if (!imu_ptr) { + return missing("imu message"); + } + // imu_ptr is valid + onImu(imu_ptr); + } + if (use_imu_path_ && !angular_velocity_ptr_) { + return missing("imu"); + } + + predicted_traj_ptr_ = sub_predicted_traj_.takeData(); + if (use_predicted_trajectory_ && !predicted_traj_ptr_) { + return missing("control predicted trajectory"); + } + + autoware_state_ = sub_autoware_state_.takeData(); + if (!autoware_state_) { + return missing("autoware_state"); + } + + return true; +} + +void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) +{ + MarkerArray debug_markers; + checkCollision(debug_markers); + + if (!collision_data_keeper_.checkCollisionExpired()) { + const std::string error_msg = "[AEB]: Emergency Brake"; + const auto diag_level = DiagnosticStatus::ERROR; + stat.summary(diag_level, error_msg); + const auto & data = collision_data_keeper_.get(); + stat.addf("RSS", "%.2f", data.rss); + stat.addf("Distance", "%.2f", data.distance_to_object); + stat.addf("Object Speed", "%.2f", data.velocity); + addCollisionMarker(data, debug_markers); + } else { + const std::string error_msg = "[AEB]: No Collision"; + const auto diag_level = DiagnosticStatus::OK; + stat.summary(diag_level, error_msg); + } + + // publish debug markers + debug_ego_path_publisher_->publish(debug_markers); +} + +bool AEB::checkCollision(MarkerArray & debug_markers) +{ + using colorTuple = std::tuple; + + // step1. check data + if (!fetchLatestData()) { + return false; + } + + // if not driving, disable aeb + if (autoware_state_->state != AutowareState::DRIVING) { + return false; + } + + // step2. create velocity data check if the vehicle stops or not + constexpr double min_moving_velocity_th{0.1}; + const double current_v = current_velocity_ptr_->longitudinal_velocity; + if (std::abs(current_v) < min_moving_velocity_th) { + return false; + } + + auto check_collision = [&]( + 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); + + // 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) { + return o1.distance_to_object < o2.distance_to_object; + }); + + if (closest_object_point_itr == objects_from_point_clusters.end()) { + return std::nullopt; + } + 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; + } + closest_object_point_itr->velocity = closest_object_speed.value(); + return std::make_optional(*closest_object_point_itr); + }); + + // Add 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); + } + // check collision using rss distance + return (closest_object_point.has_value()) + ? hasCollision(current_v, closest_object_point.value()) + : false; + }; + + // step3. make function to check collision with ego path created with sensor data + const auto has_collision_ego = [&](pcl::PointCloud::Ptr filtered_objects) -> bool { + if (!use_imu_path_) return false; + const double current_w = angular_velocity_ptr_->z; + constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; + const std::string ns = "ego"; + const auto ego_path = generateEgoPath(current_v, current_w); + + return check_collision(ego_path, debug_color, ns, filtered_objects); + }; + + // step4. make function to check collision with predicted trajectory from control module + const auto has_collision_predicted = + [&](pcl::PointCloud::Ptr filtered_objects) -> bool { + if (!use_predicted_trajectory_ || !predicted_traj_ptr_) return false; + const auto predicted_traj_ptr = predicted_traj_ptr_; + const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr); + + if (!predicted_path_opt) return false; + constexpr colorTuple debug_color = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}; + const std::string ns = "predicted"; + const auto & predicted_path = predicted_path_opt.value(); + + return check_collision(predicted_path, debug_color, ns, filtered_objects); + }; + + // Data of filtered point cloud + pcl::PointCloud::Ptr filtered_objects(new PointCloud); + // evaluate if there is a collision for both paths + const bool has_collision = + has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects); + + // 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_); + } + return has_collision; +} + +bool AEB::hasCollision(const double current_v, const ObjectData & closest_object) +{ + const double & obj_v = closest_object.velocity; + const double & t = t_response_; + const double rss_dist = std::abs(current_v) * t + + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - + obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; + if (closest_object.distance_to_object < rss_dist) { + // collision happens + ObjectData collision_data = closest_object; + collision_data.rss = rss_dist; + collision_data.distance_to_object = closest_object.distance_to_object; + collision_data_keeper_.setCollisionData(collision_data); + return true; + } + return false; +} + +Path AEB::generateEgoPath(const double curr_v, const double curr_w) +{ + Path path; + double curr_x = 0.0; + double curr_y = 0.0; + double curr_yaw = 0.0; + geometry_msgs::msg::Pose ini_pose; + 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) { + // if current velocity is too small, assume it stops at the same point + return path; + } + + constexpr double epsilon = 1e-6; + const double & dt = imu_prediction_time_interval_; + const double & horizon = imu_prediction_time_horizon_; + for (double t = 0.0; t < horizon + epsilon; t += dt) { + 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 = 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 (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 = 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); + } + return path; +} + +std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) +{ + if (predicted_traj.points.empty()) { + return std::nullopt; + } + + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", predicted_traj.header.frame_id, predicted_traj.header.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 std::nullopt; + } + + // create path + Path path; + path.resize(predicted_traj.points.size()); + for (size_t i = 0; i < predicted_traj.points.size(); ++i) { + geometry_msgs::msg::Pose map_pose; + tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); + path.at(i) = map_pose; + + if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { + break; + } + } + return path; +} + +std::vector AEB::generatePathFootprint( + const Path & path, const double extra_width_margin) +{ + std::vector polygons; + for (size_t i = 0; i < path.size() - 1; ++i) { + polygons.push_back( + createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); + } + return polygons; +} + +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) +{ + // check if the predicted path has valid number of points + if (ego_path.size() < 2 || ego_polys.empty() || obstacle_points_ptr->empty()) { + return; + } + + // eliminate noisy points by only considering points belonging to clusters of at least a certain + // size + const std::vector cluster_indices = std::invoke([&]() { + std::vector cluster_idx; + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(obstacle_points_ptr); + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(cluster_tolerance_); + ec.setMinClusterSize(minimum_cluster_size_); + ec.setMaxClusterSize(maximum_cluster_size_); + ec.setSearchMethod(tree); + ec.setInputCloud(obstacle_points_ptr); + ec.extract(cluster_idx); + return cluster_idx; + }); + + PointCloud::Ptr points_belonging_to_cluster_hulls(new PointCloud); + for (const auto & indices : cluster_indices) { + PointCloud::Ptr cluster(new PointCloud); + for (const auto & index : indices.indices) { + cluster->push_back((*obstacle_points_ptr)[index]); + } + // Make a 2d convex hull for the objects + pcl::ConvexHull hull; + hull.setDimension(2); + hull.setInputCloud(cluster); + std::vector polygons; + PointCloud::Ptr surface_hull(new pcl::PointCloud); + hull.reconstruct(*surface_hull, polygons); + for (const auto & p : *surface_hull) { + points_belonging_to_cluster_hulls->push_back(p); + } + } + + // select points inside the ego footprint path + 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); + }(); + + for (const auto & p : *points_belonging_to_cluster_hulls) { + const auto obj_position = autoware::universe_utils::createPoint(p.x, p.y, p.z); + 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 = 0.0; + obj.distance_to_object = std::abs(dist_ego_to_object); + + const Point2d obj_point(p.x, p.y); + for (const auto & ego_poly : ego_polys) { + if (bg::within(obj_point, ego_poly)) { + objects.push_back(obj); + break; + } + } + } +} + +void AEB::cropPointCloudWithEgoFootprintPath( + const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects) +{ + PointCloud::Ptr full_points_ptr(new PointCloud); + pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr); + // Create a Point cloud with the points of the ego footprint + PointCloud::Ptr path_polygon_points(new PointCloud); + std::for_each(ego_polys.begin(), ego_polys.end(), [&](const auto & poly) { + std::for_each(poly.outer().begin(), poly.outer().end(), [&](const auto & p) { + pcl::PointXYZ point(p.x(), p.y(), 0.0); + path_polygon_points->push_back(point); + }); + }); + // Make a surface hull with the ego footprint to filter out points + pcl::ConvexHull hull; + hull.setDimension(2); + hull.setInputCloud(path_polygon_points); + std::vector polygons; + pcl::PointCloud::Ptr surface_hull(new pcl::PointCloud); + hull.reconstruct(*surface_hull, polygons); + // Filter out points outside of the path's convex hull + pcl::CropHull path_polygon_hull_filter; + path_polygon_hull_filter.setDim(2); + path_polygon_hull_filter.setInputCloud(full_points_ptr); + path_polygon_hull_filter.setHullIndices(polygons); + path_polygon_hull_filter.setHullCloud(surface_hull); + path_polygon_hull_filter.filter(*filtered_objects); + filtered_objects->header.stamp = full_points_ptr->header.stamp; +} + +void AEB::addMarker( + const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, + const std::vector & objects, const std::optional & closest_object, + 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 = autoware::universe_utils::createDefaultMarker( + "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP, + 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 = autoware::universe_utils::createDefaultMarker( + "base_link", current_time, ns + "_polygon", 0, Marker::LINE_LIST, + 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 = + 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 = autoware::universe_utils::createDefaultMarker( + "base_link", current_time, ns + "_objects", 0, Marker::SPHERE_LIST, + 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); + } + debug_markers.markers.push_back(object_data_marker); + + // Visualize planner type text + if (closest_object.has_value()) { + const auto & obj = closest_object.value(); + 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, + 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 = + "Object velocity: " + std::to_string(obj.velocity) + " [m/s]\n"; + closest_object_velocity_marker_array.text += + "Object relative velocity to ego: " + std::to_string(obj.velocity - std::abs(ego_velocity)) + + " [m/s]"; + debug_markers.markers.push_back(closest_object_velocity_marker_array); + } +} + +void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) +{ + auto point_marker = autoware::universe_utils::createDefaultMarker( + "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, + 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); +} + +} // namespace autoware::motion::control::autonomous_emergency_braking + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::autonomous_emergency_braking::AEB) diff --git a/control/autoware_control_validator/CMakeLists.txt b/control/autoware_control_validator/CMakeLists.txt new file mode 100644 index 0000000000000..2b25b5b984c12 --- /dev/null +++ b/control/autoware_control_validator/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(autoware_control_validator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_control_validator_helpers SHARED + src/utils.cpp + src/debug_marker.cpp +) + +# control validator +ament_auto_add_library(autoware_control_validator_component SHARED + include/autoware/control_validator/control_validator.hpp + src/control_validator.cpp +) +target_link_libraries(autoware_control_validator_component autoware_control_validator_helpers) +rclcpp_components_register_node(autoware_control_validator_component + PLUGIN "autoware::control_validator::ControlValidator" + EXECUTABLE autoware_control_validator_node +) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/ControlValidatorStatus.msg" + DEPENDENCIES builtin_interfaces +) + +# to use a message defined in the same package +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(autoware_control_validator_component + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(autoware_control_validator_component "${cpp_typesupport_target}") +endif() + +# if(BUILD_TESTING) +# endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/control/autoware_control_validator/README.md b/control/autoware_control_validator/README.md new file mode 100644 index 0000000000000..9c4a9be0732a5 --- /dev/null +++ b/control/autoware_control_validator/README.md @@ -0,0 +1,58 @@ +# Control Validator + +The `control_validator` is a module that checks the validity of the output of the control component. The status of the validation can be viewed in the `/diagnostics` topic. + +![control_validator](./image/control_validator.drawio.svg) + +## Supported features + +The following features are supported for the validation and can have thresholds set by parameters: + +- **Deviation check between reference trajectory and predicted trajectory** : invalid when the largest deviation between the predicted trajectory and reference trajectory is greater than the given threshold. + +![trajectory_deviation](./image/trajectory_deviation.drawio.svg) + +Other features are to be implemented. + +## Inputs/Outputs + +### Inputs + +The `control_validator` takes in the following inputs: + +| Name | Type | Description | +| ------------------------------ | --------------------------------- | ------------------------------------------------------------------------------ | +| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | +| `~/input/reference_trajectory` | autoware_planning_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed | +| `~/input/predicted_trajectory` | autoware_planning_msgs/Trajectory | predicted trajectory which is outputted from control module | + +### Outputs + +It outputs the following: + +| Name | Type | Description | +| ---------------------------- | ---------------------------------------- | ------------------------------------------------------------------------- | +| `~/output/validation_status` | control_validator/ControlValidatorStatus | validator status to inform the reason why the trajectory is valid/invalid | +| `/diagnostics` | diagnostic_msgs/DiagnosticStatus | diagnostics to report errors | + +## Parameters + +The following parameters can be set for the `control_validator`: + +### System parameters + +| Name | Type | Description | Default value | +| :--------------------------- | :--- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `publish_diag` | bool | if true, diagnostics msg is published. | true | +| `diag_error_count_threshold` | int | the Diag will be set to ERROR when the number of consecutive invalid trajectory exceeds this threshold. (For example, threshold = 1 means, even if the trajectory is invalid, the Diag will not be ERROR if the next trajectory is valid.) | true | +| `display_on_terminal` | bool | show error msg on terminal | true | + +### Algorithm parameters + +#### Thresholds + +The input trajectory is detected as invalid if the index exceeds the following thresholds. + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0 | diff --git a/control/control_validator/config/control_validator.param.yaml b/control/autoware_control_validator/config/control_validator.param.yaml similarity index 100% rename from control/control_validator/config/control_validator.param.yaml rename to control/autoware_control_validator/config/control_validator.param.yaml diff --git a/control/control_validator/image/control_validator.drawio.svg b/control/autoware_control_validator/image/control_validator.drawio.svg similarity index 100% rename from control/control_validator/image/control_validator.drawio.svg rename to control/autoware_control_validator/image/control_validator.drawio.svg diff --git a/control/control_validator/image/trajectory_deviation.drawio.svg b/control/autoware_control_validator/image/trajectory_deviation.drawio.svg similarity index 100% rename from control/control_validator/image/trajectory_deviation.drawio.svg rename to control/autoware_control_validator/image/trajectory_deviation.drawio.svg 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 new file mode 100644 index 0000000000000..ecb46aee123e3 --- /dev/null +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -0,0 +1,101 @@ +// 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 AUTOWARE__CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ +#define AUTOWARE__CONTROL_VALIDATOR__CONTROL_VALIDATOR_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 +#include + +#include +#include +#include + +#include +#include + +namespace autoware::control_validator +{ +using autoware_control_validator::msg::ControlValidatorStatus; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +using nav_msgs::msg::Odometry; + +struct ValidationParams +{ + double max_distance_deviation_threshold; +}; + +class ControlValidator : public rclcpp::Node +{ +public: + explicit ControlValidator(const rclcpp::NodeOptions & options); + + void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); + + bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory); + +private: + void setupDiag(); + + void setupParameters(); + + bool isDataReady(); + + void validate(const Trajectory & trajectory); + + void publishPredictedTrajectory(); + void publishDebugInfo(); + void displayStatus(); + + void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); + + rclcpp::Subscription::SharedPtr sub_predicted_traj_; + autoware::universe_utils::InterProcessPollingSubscriber sub_kinematics_{ + this, "~/input/kinematics"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_reference_traj_{ + this, "~/input/reference_trajectory"}; + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_markers_; + + // system parameters + int diag_error_count_threshold_ = 0; + bool display_on_terminal_ = true; + + Updater diag_updater_{this}; + + ControlValidatorStatus validation_status_; + ValidationParams validation_params_; // for thresholds + + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + + bool isAllValid(const ControlValidatorStatus & status); + + Trajectory::ConstSharedPtr current_reference_trajectory_; + Trajectory::ConstSharedPtr current_predicted_trajectory_; + + Odometry::ConstSharedPtr current_kinematics_; + + std::shared_ptr debug_pose_publisher_; +}; +} // namespace autoware::control_validator + +#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 new file mode 100644 index 0000000000000..c03ecb46b8117 --- /dev/null +++ b/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp @@ -0,0 +1,60 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ +#define AUTOWARE__CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +class ControlValidatorDebugMarkerPublisher +{ +public: + explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node); + + void pushPoseMarker( + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0); + void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); + void pushVirtualWall(const geometry_msgs::msg::Pose & pose); + void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); + void publish(); + + void clearMarkers(); + +private: + rclcpp::Node * node_; + visualization_msgs::msg::MarkerArray marker_array_; + visualization_msgs::msg::MarkerArray marker_array_virtual_wall_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + std::map marker_id_; + + int getMarkerId(const std::string & ns) + { + if (marker_id_.count(ns) == 0) { + marker_id_[ns] = 0.0; + } + return marker_id_[ns]++; + } +}; + +#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 new file mode 100644 index 0000000000000..375ad557d02df --- /dev/null +++ b/control/autoware_control_validator/include/autoware/control_validator/utils.hpp @@ -0,0 +1,55 @@ +// 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 AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_ +#define AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_ + +#include +#include + +#include + +#include +#include +#include + +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 TrajectoryPoints = std::vector; + +void shiftPose(Pose & pose, double longitudinal); + +void insertPointInPredictedTrajectory( + TrajectoryPoints & modified_trajectory, const geometry_msgs::msg::Pose & reference_pose, + const TrajectoryPoints & predicted_trajectory); + +TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory); + +bool removeFrontTrajectoryPoint( + const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, + const TrajectoryPoints & predicted_trajectory_points); + +Trajectory alignTrajectoryWithReferenceTrajectory( + const Trajectory & trajectory, const Trajectory & predicted_trajectory); + +double calcMaxLateralDistance( + const Trajectory & trajectory, const Trajectory & predicted_trajectory); +} // namespace autoware::control_validator + +#endif // AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_ diff --git a/control/control_validator/launch/control_validator.launch.xml b/control/autoware_control_validator/launch/control_validator.launch.xml similarity index 79% rename from control/control_validator/launch/control_validator.launch.xml rename to control/autoware_control_validator/launch/control_validator.launch.xml index 9727e06e60523..651398a17fc78 100644 --- a/control/control_validator/launch/control_validator.launch.xml +++ b/control/autoware_control_validator/launch/control_validator.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/control/control_validator/msg/ControlValidatorStatus.msg b/control/autoware_control_validator/msg/ControlValidatorStatus.msg similarity index 100% rename from control/control_validator/msg/ControlValidatorStatus.msg rename to control/autoware_control_validator/msg/ControlValidatorStatus.msg diff --git a/control/autoware_control_validator/package.xml b/control/autoware_control_validator/package.xml new file mode 100644 index 0000000000000..71fefafbaffb1 --- /dev/null +++ b/control/autoware_control_validator/package.xml @@ -0,0 +1,44 @@ + + + + autoware_control_validator + 0.1.0 + ros node for autoware_control_validator + Kyoichi Sugahara + Takamasa Horibe + Makoto Kurihara + Mamoru Sobue + Takayuki Murooka + + Apache License 2.0 + + Kyoichi Sugahara + Takamasa Horibe + Makoto Kurihara + + ament_cmake_auto + autoware_cmake + rosidl_default_generators + + autoware_motion_utils + autoware_planning_msgs + autoware_universe_utils + autoware_vehicle_info_utils + diagnostic_updater + geometry_msgs + nav_msgs + rclcpp + rclcpp_components + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/control/control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp similarity index 86% rename from control/control_validator/src/control_validator.cpp rename to control/autoware_control_validator/src/control_validator.cpp index 5282e31fef898..4c8ca3c831824 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "control_validator/control_validator.hpp" +#include "autoware/control_validator/control_validator.hpp" -#include "control_validator/utils.hpp" +#include "autoware/control_validator/utils.hpp" #include #include #include -namespace control_validator +namespace autoware::control_validator { using diagnostic_msgs::msg::DiagnosticStatus; @@ -28,13 +28,6 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) : Node("control_validator", options) { using std::placeholders::_1; - - sub_kinematics_ = create_subscription( - "~/input/kinematics", 1, - [this](const Odometry::ConstSharedPtr msg) { current_kinematics_ = msg; }); - sub_reference_traj_ = create_subscription( - "~/input/reference_trajectory", 1, - std::bind(&ControlValidator::onReferenceTrajectory, this, _1)); sub_predicted_traj_ = create_subscription( "~/input/predicted_trajectory", 1, std::bind(&ControlValidator::onPredictedTrajectory, this, _1)); @@ -61,7 +54,7 @@ void ControlValidator::setupParameters() } try { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); } catch (...) { RCLCPP_ERROR(get_logger(), "failed to get vehicle info. use default value."); vehicle_info_.front_overhang_m = 0.5; @@ -116,21 +109,11 @@ bool ControlValidator::isDataReady() return true; } -void ControlValidator::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) -{ - if (msg->points.size() < 2) { - RCLCPP_ERROR(get_logger(), "planning trajectory size is invalid (%lu)", msg->points.size()); - return; - } - - current_reference_trajectory_ = msg; - - return; -} - void ControlValidator::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg) { current_predicted_trajectory_ = msg; + current_reference_trajectory_ = sub_reference_traj_.takeData(); + current_kinematics_ = sub_kinematics_.takeData(); if (!isDataReady()) return; @@ -210,7 +193,7 @@ void ControlValidator::displayStatus() "predicted trajectory is too far from planning trajectory!!"); } -} // namespace control_validator +} // namespace autoware::control_validator #include -RCLCPP_COMPONENTS_REGISTER_NODE(control_validator::ControlValidator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::control_validator::ControlValidator) diff --git a/control/autoware_control_validator/src/debug_marker.cpp b/control/autoware_control_validator/src/debug_marker.cpp new file mode 100644 index 0000000000000..d5d8644515ab1 --- /dev/null +++ b/control/autoware_control_validator/src/debug_marker.cpp @@ -0,0 +1,101 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/control_validator/debug_marker.hpp" + +#include +#include + +#include +#include +#include + +using visualization_msgs::msg::Marker; + +ControlValidatorDebugMarkerPublisher::ControlValidatorDebugMarkerPublisher(rclcpp::Node * node) +: node_(node) +{ + debug_viz_pub_ = + node_->create_publisher("~/debug/marker", 1); + + virtual_wall_pub_ = + node_->create_publisher("~/virtual_wall", 1); +} + +void ControlValidatorDebugMarkerPublisher::clearMarkers() +{ + marker_array_.markers.clear(); + marker_array_virtual_wall_.markers.clear(); +} + +void ControlValidatorDebugMarkerPublisher::pushPoseMarker( + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) +{ + pushPoseMarker(p.pose, ns, id); +} + +void ControlValidatorDebugMarkerPublisher::pushPoseMarker( + const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) +{ + using autoware::universe_utils::createMarkerColor; + + // append arrow marker + std_msgs::msg::ColorRGBA color; + if (id == 0) // Red + { + color = createMarkerColor(1.0, 0.0, 0.0, 0.999); + } + if (id == 1) // Green + { + color = createMarkerColor(0.0, 1.0, 0.0, 0.999); + } + if (id == 2) // Blue + { + color = createMarkerColor(0.0, 0.0, 1.0, 0.999); + } + Marker marker = autoware::universe_utils::createDefaultMarker( + "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, + autoware::universe_utils::createMarkerScale(0.2, 0.1, 0.3), color); + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.pose = pose; + + marker_array_.markers.push_back(marker); +} + +void ControlValidatorDebugMarkerPublisher::pushWarningMsg( + const geometry_msgs::msg::Pose & pose, const std::string & msg) +{ + visualization_msgs::msg::Marker marker = autoware::universe_utils::createDefaultMarker( + "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, + 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; + marker_array_virtual_wall_.markers.push_back(marker); +} + +void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs::msg::Pose & pose) +{ + const auto now = node_->get_clock()->now(); + const auto stop_wall_marker = + autoware::motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); + autoware::universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); +} + +void ControlValidatorDebugMarkerPublisher::publish() +{ + debug_viz_pub_->publish(marker_array_); + virtual_wall_pub_->publish(marker_array_virtual_wall_); +} diff --git a/control/autoware_control_validator/src/utils.cpp b/control/autoware_control_validator/src/utils.cpp new file mode 100644 index 0000000000000..821d64c3af6e6 --- /dev/null +++ b/control/autoware_control_validator/src/utils.cpp @@ -0,0 +1,165 @@ +// 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 "autoware/control_validator/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::control_validator +{ + +void shiftPose(Pose & pose, double longitudinal) +{ + const auto yaw = tf2::getYaw(pose.orientation); + pose.position.x += std::cos(yaw) * longitudinal; + pose.position.y += std::sin(yaw) * longitudinal; +} + +void insertPointInPredictedTrajectory( + TrajectoryPoints & modified_trajectory, const Pose & reference_pose, + const TrajectoryPoints & predicted_trajectory) +{ + const auto point_to_interpolate = autoware::motion_utils::calcInterpolatedPoint( + convertToTrajectory(predicted_trajectory), reference_pose); + modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate); +} + +TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points) +{ + TrajectoryPoints reversed_trajectory_points; + reversed_trajectory_points.reserve(trajectory_points.size()); + std::reverse_copy( + trajectory_points.begin(), trajectory_points.end(), + std::back_inserter(reversed_trajectory_points)); + return reversed_trajectory_points; +} + +bool removeFrontTrajectoryPoint( + const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, + const TrajectoryPoints & predicted_trajectory_points) +{ + bool predicted_trajectory_point_removed = false; + for (const auto & point : predicted_trajectory_points) { + if ( + 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; + } else { + break; + } + } + + return predicted_trajectory_point_removed; +} + +Trajectory alignTrajectoryWithReferenceTrajectory( + const Trajectory & trajectory, const Trajectory & predicted_trajectory) +{ + 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 + // predicted_trajectory: p1------------------pN + // trajectory: t1------------------tN + // OR + // predicted_trajectory: p1------------------pN + // trajectory: t1------------------tN + const bool & is_p_n_before_t1 = + autoware::motion_utils::calcLongitudinalOffsetToSegment( + trajectory.points, 0, predicted_trajectory.points.back().pose.position) < 0.0; + 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 > + 0.0; + const bool is_no_overlapping = (is_p_n_before_t1 || is_p1_behind_t_n); + + if (is_no_overlapping) { + return Trajectory(); + } + + auto modified_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); + auto predicted_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); + auto trajectory_points = convertToTrajectoryPointArray(trajectory); + + // If first point of predicted_trajectory is in front of start of trajectory, erase points which + // are in front of trajectory start point and insert pNew along the predicted_trajectory + // predicted_trajectory: p1-----p2-----p3----//------pN + // trajectory: t1--------//------tN + // ↓ + // predicted_trajectory: pNew--p3----//------pN + // trajectory: t1--------//------tN + auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( + trajectory_points, modified_trajectory_points, predicted_trajectory_points); + + if (predicted_trajectory_point_removed) { + insertPointInPredictedTrajectory( + modified_trajectory_points, trajectory_points.front().pose, predicted_trajectory_points); + } + + // If last point of predicted_trajectory is behind of end of trajectory, erase points which are + // behind trajectory last point and insert pNew along the predicted_trajectory + // predicted_trajectory: p1-----//------pN-2-----pN-1-----pN + // trajectory: t1-----//-----tN-1--tN + // ↓ + // predicted_trajectory: p1-----//------pN-2-pNew + // trajectory: t1-----//-----tN-1--tN + + auto reversed_predicted_trajectory_points = reverseTrajectoryPoints(predicted_trajectory_points); + auto reversed_trajectory_points = reverseTrajectoryPoints(trajectory_points); + auto reversed_modified_trajectory_points = reverseTrajectoryPoints(modified_trajectory_points); + + auto reversed_predicted_trajectory_point_removed = removeFrontTrajectoryPoint( + reversed_trajectory_points, reversed_modified_trajectory_points, + reversed_predicted_trajectory_points); + + if (reversed_predicted_trajectory_point_removed) { + insertPointInPredictedTrajectory( + reversed_modified_trajectory_points, reversed_trajectory_points.front().pose, + reversed_predicted_trajectory_points); + } + + return convertToTrajectory(reverseTrajectoryPoints(reversed_modified_trajectory_points)); +} + +double calcMaxLateralDistance( + const Trajectory & reference_trajectory, const Trajectory & predicted_trajectory) +{ + const auto alined_predicted_trajectory = + alignTrajectoryWithReferenceTrajectory(reference_trajectory, predicted_trajectory); + double max_dist = 0; + for (const auto & point : alined_predicted_trajectory.points) { + const auto p0 = autoware::universe_utils::getPoint(point); + // find nearest segment + const size_t 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; + } + } + return max_dist; +} + +} // namespace autoware::control_validator diff --git a/control/autoware_external_cmd_selector/CMakeLists.txt b/control/autoware_external_cmd_selector/CMakeLists.txt new file mode 100644 index 0000000000000..4dccabcd85d8e --- /dev/null +++ b/control/autoware_external_cmd_selector/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_external_cmd_selector) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/autoware_external_cmd_selector/external_cmd_selector_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "autoware::external_cmd_selector::ExternalCmdSelector" + EXECUTABLE autoware_external_cmd_selector +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_external_cmd_selector/README.md b/control/autoware_external_cmd_selector/README.md new file mode 100644 index 0000000000000..0495004b68b84 --- /dev/null +++ b/control/autoware_external_cmd_selector/README.md @@ -0,0 +1,34 @@ +# autoware_external_cmd_selector + +## Purpose + +`autoware_external_cmd_selector` is the package to publish `external_control_cmd`, `gear_cmd`, `hazard_lights_cmd`, `heartbeat` and `turn_indicators_cmd`, according to the current mode, which is `remote` or `local`. + +The current mode is set via service, `remote` is remotely operated, `local` is to use the values calculated by Autoware. + +## Input / Output + +### Input topics + +| Name | Type | Description | +| ---------------------------------------------- | ---- | ------------------------------------------------------- | +| `/api/external/set/command/local/control` | TBD | Local. Calculated control value. | +| `/api/external/set/command/local/heartbeat` | TBD | Local. Heartbeat. | +| `/api/external/set/command/local/shift` | TBD | Local. Gear shift like drive, rear and etc. | +| `/api/external/set/command/local/turn_signal` | TBD | Local. Turn signal like left turn, right turn and etc. | +| `/api/external/set/command/remote/control` | TBD | Remote. Calculated control value. | +| `/api/external/set/command/remote/heartbeat` | TBD | Remote. Heartbeat. | +| `/api/external/set/command/remote/shift` | TBD | Remote. Gear shift like drive, rear and etc. | +| `/api/external/set/command/remote/turn_signal` | TBD | Remote. Turn signal like left turn, right turn and etc. | + +### Output topics + +| Name | Type | Description | +| ------------------------------------------------------ | ------------------------------------------------- | ----------------------------------------------- | +| `/control/external_cmd_selector/current_selector_mode` | TBD | Current selected mode, remote or local. | +| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Check if node is active or not. | +| `/external/selected/external_control_cmd` | TBD | Pass through control command with current mode. | +| `/external/selected/gear_cmd` | autoware_vehicle_msgs::msg::GearCommand | Pass through gear command with current mode. | +| `/external/selected/hazard_lights_cmd` | autoware_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | +| `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | +| `/external/selected/turn_indicators_cmd` | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | diff --git a/control/external_cmd_selector/config/external_cmd_selector.param.yaml b/control/autoware_external_cmd_selector/config/external_cmd_selector.param.yaml similarity index 100% rename from control/external_cmd_selector/config/external_cmd_selector.param.yaml rename to control/autoware_external_cmd_selector/config/external_cmd_selector.param.yaml diff --git a/control/external_cmd_selector/include/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 85% rename from control/external_cmd_selector/include/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 f2803e897ba3e..464ca0be8c163 100644 --- a/control/external_cmd_selector/include/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,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ -#define 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 #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -30,7 +30,8 @@ #include #include - +namespace autoware::external_cmd_selector +{ class ExternalCmdSelector : public rclcpp::Node { public: @@ -39,9 +40,9 @@ class ExternalCmdSelector : public rclcpp::Node private: using CommandSourceSelect = tier4_control_msgs::srv::ExternalCommandSelect; using CommandSourceMode = tier4_control_msgs::msg::ExternalCommandSelectorMode; - using InternalGearShift = autoware_auto_vehicle_msgs::msg::GearCommand; - using InternalTurnSignal = autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; - using InternalHazardSignal = autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using InternalGearShift = autoware_vehicle_msgs::msg::GearCommand; + using InternalTurnSignal = autoware_vehicle_msgs::msg::TurnIndicatorsCommand; + using InternalHazardSignal = autoware_vehicle_msgs::msg::HazardLightsCommand; using InternalHeartbeat = tier4_external_api_msgs::msg::Heartbeat; using ExternalControlCommand = tier4_external_api_msgs::msg::ControlCommandStamped; using ExternalGearShift = tier4_external_api_msgs::msg::GearShiftStamped; @@ -101,5 +102,5 @@ class ExternalCmdSelector : public rclcpp::Node // Diagnostics Updater diagnostic_updater::Updater updater_{this}; }; - -#endif // EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ +} // namespace autoware::external_cmd_selector +#endif // AUTOWARE__EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ diff --git a/control/external_cmd_selector/launch/external_cmd_selector.launch.py b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.py similarity index 97% rename from control/external_cmd_selector/launch/external_cmd_selector.launch.py rename to control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.py index 358cc135996f7..95673502e2b51 100644 --- a/control/external_cmd_selector/launch/external_cmd_selector.launch.py +++ b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.py @@ -32,8 +32,8 @@ def launch_setup(context, *args, **kwargs): external_cmd_selector_param = yaml.safe_load(f)["/**"]["ros__parameters"] component = ComposableNode( - package="external_cmd_selector", - plugin="ExternalCmdSelector", + package="autoware_external_cmd_selector", + plugin="autoware::external_cmd_selector::ExternalCmdSelector", name="external_cmd_selector", remappings=[ _create_mapping_tuple("service/select_external_command"), diff --git a/control/external_cmd_selector/launch/external_cmd_selector.launch.xml b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.xml similarity index 95% rename from control/external_cmd_selector/launch/external_cmd_selector.launch.xml rename to control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.xml index cb373802e5183..7e8b32ac68d8d 100644 --- a/control/external_cmd_selector/launch/external_cmd_selector.launch.xml +++ b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.xml @@ -26,7 +26,7 @@ - + diff --git a/control/autoware_external_cmd_selector/package.xml b/control/autoware_external_cmd_selector/package.xml new file mode 100644 index 0000000000000..19881f06c96e5 --- /dev/null +++ b/control/autoware_external_cmd_selector/package.xml @@ -0,0 +1,35 @@ + + + + autoware_external_cmd_selector + 0.1.0 + The autoware_external_cmd_selector package + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + Fumiya Watanabe + Takamasa Horibe + Satoshi Ota + Takayuki Murooka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_vehicle_msgs + diagnostic_updater + geometry_msgs + rclcpp + rclcpp_components + std_msgs + tier4_auto_msgs_converter + tier4_control_msgs + tier4_external_api_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp b/control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp similarity index 96% rename from control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp rename to control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp index 0039ec9cd1547..4156f54025d35 100644 --- a/control/external_cmd_selector/src/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 "external_cmd_selector/external_cmd_selector_node.hpp" +#include "autoware/external_cmd_selector/external_cmd_selector_node.hpp" #include @@ -21,6 +21,8 @@ #include #include +namespace autoware::external_cmd_selector +{ ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_options) : Node("external_cmd_selector", node_options) { @@ -202,6 +204,7 @@ ExternalCmdSelector::InternalHeartbeat ExternalCmdSelector::convert( { return command; } +} // namespace autoware::external_cmd_selector #include -RCLCPP_COMPONENTS_REGISTER_NODE(ExternalCmdSelector) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::external_cmd_selector::ExternalCmdSelector) diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt new file mode 100644 index 0000000000000..9c7d8b3ad475a --- /dev/null +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_joy_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_joy_controller_node SHARED + DIRECTORY src +) + +rclcpp_components_register_node(autoware_joy_controller_node + PLUGIN "autoware::joy_controller::AutowareJoyControllerNode" + EXECUTABLE autoware_joy_controller +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_joy_controller/README.md b/control/autoware_joy_controller/README.md new file mode 100644 index 0000000000000..a6919356f3dbc --- /dev/null +++ b/control/autoware_joy_controller/README.md @@ -0,0 +1,125 @@ +# autoware_joy_controller + +## Role + +`autoware_joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle. + +## Usage + +### ROS 2 launch + +```bash +# With default config (ds4) +ros2 launch autoware_joy_controller joy_controller.launch.xml + +# Default config but select from the existing parameter files +ros2 launch autoware_joy_controller joy_controller_param_selection.launch.xml joy_type:=ds4 # or g29, p65, xbox + +# Override the param file +ros2 launch autoware_joy_controller joy_controller.launch.xml config_file:=/path/to/your/param.yaml +``` + +## Input / Output + +### Input topics + +| Name | Type | Description | +| ------------------ | ----------------------- | --------------------------------- | +| `~/input/joy` | sensor_msgs::msg::Joy | joy controller command | +| `~/input/odometry` | nav_msgs::msg::Odometry | ego vehicle odometry to get twist | + +### Output topics + +| Name | Type | Description | +| ----------------------------------- | --------------------------------------------------- | ---------------------------------------- | +| `~/output/control_command` | autoware_control_msgs::msg::Control | lateral and longitudinal control command | +| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command | +| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command | +| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command | +| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) | +| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat | +| `~/output/vehicle_engage` | autoware_vehicle_msgs::msg::Engage | vehicle engage | + +## Parameters + +| Parameter | Type | Description | +| ------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------ | +| `joy_type` | string | joy controller type (default: DS4) | +| `update_rate` | double | update rate to publish control commands | +| `accel_ratio` | double | ratio to calculate acceleration (commanded acceleration is ratio \* operation) | +| `brake_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | +| `steer_ratio` | double | ratio to calculate deceleration (commanded steer is ratio \* operation) | +| `steering_angle_velocity` | double | steering angle velocity for operation | +| `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | +| `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | +| `raw_control` | bool | skip input odometry if true | +| `velocity_gain` | double | ratio to calculate velocity by acceleration | +| `max_forward_velocity` | double | absolute max velocity to go forward | +| `max_backward_velocity` | double | absolute max velocity to go backward | +| `backward_accel_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | + +## P65 Joystick Key Map + +| Action | Button | +| -------------------- | --------------------- | +| Acceleration | R2 | +| Brake | L2 | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | L1 | +| Turn Signal Right | R1 | +| Clear Turn Signal | A | +| Gate Mode | B | +| Emergency Stop | Select | +| Clear Emergency Stop | Start | +| Autoware Engage | X | +| Autoware Disengage | Y | +| Vehicle Engage | PS | +| Vehicle Disengage | Right Trigger | + +## DS4 Joystick Key Map + +| Action | Button | +| -------------------- | -------------------------- | +| Acceleration | R2, ×, or Right Stick Up | +| Brake | L2, □, or Right Stick Down | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | L1 | +| Turn Signal Right | R1 | +| Clear Turn Signal | SHARE | +| Gate Mode | OPTIONS | +| Emergency Stop | PS | +| Clear Emergency Stop | PS | +| Autoware Engage | ○ | +| Autoware Disengage | ○ | +| Vehicle Engage | △ | +| Vehicle Disengage | △ | + +## XBOX Joystick Key Map + +| Action | Button | +| -------------------- | --------------------- | +| Acceleration | RT | +| Brake | LT | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | LB | +| Turn Signal Right | RB | +| Clear Turn Signal | A | +| Gate Mode | B | +| Emergency Stop | View | +| Clear Emergency Stop | Menu | +| Autoware Engage | X | +| Autoware Disengage | Y | +| Vehicle Engage | Left Stick Button | +| Vehicle Disengage | Right Stick Button | diff --git a/control/joy_controller/config/joy_controller_ds4.param.yaml b/control/autoware_joy_controller/config/joy_controller_ds4.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_ds4.param.yaml rename to control/autoware_joy_controller/config/joy_controller_ds4.param.yaml diff --git a/control/joy_controller/config/joy_controller_g29.param.yaml b/control/autoware_joy_controller/config/joy_controller_g29.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_g29.param.yaml rename to control/autoware_joy_controller/config/joy_controller_g29.param.yaml diff --git a/control/joy_controller/config/joy_controller_p65.param.yaml b/control/autoware_joy_controller/config/joy_controller_p65.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_p65.param.yaml rename to control/autoware_joy_controller/config/joy_controller_p65.param.yaml diff --git a/control/joy_controller/config/joy_controller_xbox.param.yaml b/control/autoware_joy_controller/config/joy_controller_xbox.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_xbox.param.yaml rename to control/autoware_joy_controller/config/joy_controller_xbox.param.yaml diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp similarity index 78% rename from control/joy_controller/include/joy_controller/joy_controller.hpp rename to control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp index 22064f9cefaad..fd2d89f446dbb 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONTROLLER_HPP_ -#define JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#ifndef AUTOWARE__JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#define AUTOWARE__JOY_CONTROLLER__JOY_CONTROLLER_HPP_ -#include "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" +#include #include -#include -#include +#include +#include #include #include #include @@ -36,7 +37,7 @@ #include #include -namespace joy_controller +namespace autoware::joy_controller { using GearShiftType = tier4_external_api_msgs::msg::GearShift::_data_type; using TurnSignalType = tier4_external_api_msgs::msg::TurnSignal::_data_type; @@ -66,30 +67,30 @@ class AutowareJoyControllerNode : public rclcpp::Node double backward_accel_ratio_; // CallbackGroups - rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_; rclcpp::CallbackGroup::SharedPtr callback_group_services_; // Subscriber - rclcpp::Subscription::SharedPtr sub_joy_; - rclcpp::Subscription::SharedPtr sub_odom_; + autoware::universe_utils::InterProcessPollingSubscriber sub_joy_{ + this, "input/joy"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ + this, "input/odometry"}; rclcpp::Time last_joy_received_time_; std::shared_ptr joy_; geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; - void onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg); - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onJoy(); + void onOdometry(); // Publisher - rclcpp::Publisher::SharedPtr - pub_control_command_; + rclcpp::Publisher::SharedPtr pub_control_command_; rclcpp::Publisher::SharedPtr pub_external_control_command_; rclcpp::Publisher::SharedPtr pub_shift_; rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_heartbeat_; rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_vehicle_engage_; + rclcpp::Publisher::SharedPtr pub_vehicle_engage_; void publishControlCommand(); void publishExternalControlCommand(); @@ -106,7 +107,7 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::Client::SharedPtr client_autoware_engage_; // Previous State - autoware_auto_control_msgs::msg::AckermannControlCommand prev_control_command_; + autoware_control_msgs::msg::Control prev_control_command_; tier4_external_api_msgs::msg::ControlCommand prev_external_control_command_; GearShiftType prev_shift_ = tier4_external_api_msgs::msg::GearShift::NONE; TurnSignalType prev_turn_signal_ = tier4_external_api_msgs::msg::TurnSignal::NONE; @@ -119,6 +120,6 @@ class AutowareJoyControllerNode : public rclcpp::Node bool isDataReady(); void onTimer(); }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONTROLLER_HPP_ diff --git a/control/joy_controller/include/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 89% rename from control/joy_controller/include/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 faa920d493478..26878e1bd73cb 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/ds4_joy_converter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ -#define 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 "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" #include -namespace joy_controller +namespace autoware::joy_controller { class DS4JoyConverter : public JoyConverterBase { @@ -90,6 +90,6 @@ class DS4JoyConverter : public JoyConverterBase bool reverse() const { return Share(); } }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/include/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 88% rename from control/joy_controller/include/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 7ba062fe10d19..ea253e86305b7 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/g29_joy_converter.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ -#define 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 "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" -namespace joy_controller +namespace autoware::joy_controller { class G29JoyConverter : public JoyConverterBase { @@ -88,6 +88,6 @@ class G29JoyConverter : public JoyConverterBase bool reverse() const { return Share(); } }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/include/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 82% rename from control/joy_controller/include/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 94587b88e22f5..fa6081721511c 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/joy_converter_base.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/joy_converter_base.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ -#define 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 #include -namespace joy_controller +namespace autoware::joy_controller { class JoyConverterBase { @@ -50,6 +50,6 @@ class JoyConverterBase virtual bool vehicle_engage() const = 0; virtual bool vehicle_disengage() const = 0; }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ diff --git a/control/joy_controller/include/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 88% rename from control/joy_controller/include/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 e4cb822846fef..aebe38433e219 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/p65_joy_converter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ -#define 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 "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" #include -namespace joy_controller +namespace autoware::joy_controller { class P65JoyConverter : public JoyConverterBase { @@ -76,6 +76,6 @@ class P65JoyConverter : public JoyConverterBase const sensor_msgs::msg::Joy j_; }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/include/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 88% rename from control/joy_controller/include/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 a009ee1e12975..9d0c53b3e42cf 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/xbox_joy_converter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ -#define 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 "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware/joy_controller/joy_converter/joy_converter_base.hpp" #include -namespace joy_controller +namespace autoware::joy_controller { class XBOXJoyConverter : public JoyConverterBase { @@ -76,6 +76,6 @@ class XBOXJoyConverter : public JoyConverterBase const sensor_msgs::msg::Joy j_; }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#endif // AUTOWARE__JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/autoware_joy_controller/launch/joy_controller.launch.xml similarity index 89% rename from control/joy_controller/launch/joy_controller.launch.xml rename to control/autoware_joy_controller/launch/joy_controller.launch.xml index 5236077680d0d..02220f0026cea 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/joy_controller.launch.xml @@ -12,9 +12,9 @@ - + - + diff --git a/control/autoware_joy_controller/launch/joy_controller_param_selection.launch.xml b/control/autoware_joy_controller/launch/joy_controller_param_selection.launch.xml new file mode 100644 index 0000000000000..3d0aae9a45c4a --- /dev/null +++ b/control/autoware_joy_controller/launch/joy_controller_param_selection.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml new file mode 100644 index 0000000000000..3a9acf07a35bf --- /dev/null +++ b/control/autoware_joy_controller/package.xml @@ -0,0 +1,39 @@ + + + + autoware_joy_controller + 0.1.0 + The autoware_joy_controller package + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + Fumiya Watanabe + Takamasa Horibe + Satoshi Ota + Takayuki Murooka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_control_msgs + autoware_universe_utils + autoware_vehicle_msgs + geometry_msgs + joy + nav_msgs + rclcpp + rclcpp_components + sensor_msgs + std_srvs + tier4_api_utils + tier4_control_msgs + tier4_external_api_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/autoware_joy_controller/schema/joy_controller.schema.json similarity index 97% rename from control/joy_controller/schema/joy_controller.schema.json rename to control/autoware_joy_controller/schema/joy_controller.schema.json index d4c6351324935..4c95057aec46d 100644 --- a/control/joy_controller/schema/joy_controller.schema.json +++ b/control/autoware_joy_controller/schema/joy_controller.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Joy Controller node", "type": "object", "definitions": { - "joy_controller": { + "autoware_joy_controller": { "type": "object", "properties": { "joy_type": { @@ -112,7 +112,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/joy_controller" + "$ref": "#/definitions/autoware_joy_controller" } }, "required": ["ros__parameters"], diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/autoware_joy_controller/src/joy_controller_node.cpp similarity index 87% rename from control/joy_controller/src/joy_controller/joy_controller_node.cpp rename to control/autoware_joy_controller/src/joy_controller_node.cpp index 5eec438032410..cab15c93d753f 100644 --- a/control/joy_controller/src/joy_controller/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 "joy_controller/joy_controller.hpp" -#include "joy_controller/joy_converter/ds4_joy_converter.hpp" -#include "joy_controller/joy_converter/g29_joy_converter.hpp" -#include "joy_controller/joy_converter/p65_joy_converter.hpp" -#include "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 @@ -27,9 +27,9 @@ namespace { -using joy_controller::GateModeType; -using joy_controller::GearShiftType; -using joy_controller::TurnSignalType; +using autoware::joy_controller::GateModeType; +using autoware::joy_controller::GearShiftType; +using autoware::joy_controller::TurnSignalType; using GearShift = tier4_external_api_msgs::msg::GearShift; using TurnSignal = tier4_external_api_msgs::msg::TurnSignal; using GateMode = tier4_control_msgs::msg::GateMode; @@ -146,10 +146,15 @@ double calcMapping(const double input, const double sensitivity) } // namespace -namespace joy_controller +namespace autoware::joy_controller { -void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg) +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); @@ -190,8 +195,17 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt } } -void AutowareJoyControllerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +void AutowareJoyControllerNode::onOdometry() { + if (raw_control_) { + return; + } + + const auto msg = sub_odom_.takeData(); + if (!msg) { + return; + } + auto twist = std::make_shared(); twist->header = msg->header; twist->twist = msg->twist.twist; @@ -243,6 +257,9 @@ bool AutowareJoyControllerNode::isDataReady() void AutowareJoyControllerNode::onTimer() { + onOdometry(); + onJoy(); + if (!isDataReady()) { return; } @@ -254,7 +271,7 @@ void AutowareJoyControllerNode::onTimer() void AutowareJoyControllerNode::publishControlCommand() { - autoware_auto_control_msgs::msg::AckermannControlCommand cmd; + autoware_control_msgs::msg::Control cmd; cmd.stamp = this->now(); { cmd.lateral.steering_tire_angle = steer_ratio_ * joy_->steer(); @@ -262,24 +279,24 @@ void AutowareJoyControllerNode::publishControlCommand() if (joy_->accel()) { cmd.longitudinal.acceleration = accel_ratio_ * joy_->accel(); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = twist_->twist.linear.x + velocity_gain_ * cmd.longitudinal.acceleration; - cmd.longitudinal.speed = - std::min(cmd.longitudinal.speed, static_cast(max_forward_velocity_)); + cmd.longitudinal.velocity = + std::min(cmd.longitudinal.velocity, static_cast(max_forward_velocity_)); } if (joy_->brake()) { - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = -brake_ratio_ * joy_->brake(); } // Backward if (joy_->accel() && joy_->brake()) { cmd.longitudinal.acceleration = backward_accel_ratio_ * joy_->accel(); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = twist_->twist.linear.x - velocity_gain_ * cmd.longitudinal.acceleration; - cmd.longitudinal.speed = - std::max(cmd.longitudinal.speed, static_cast(-max_backward_velocity_)); + cmd.longitudinal.velocity = + std::max(cmd.longitudinal.velocity, static_cast(-max_backward_velocity_)); } } @@ -426,7 +443,7 @@ void AutowareJoyControllerNode::publishAutowareEngage() void AutowareJoyControllerNode::publishVehicleEngage() { - autoware_auto_vehicle_msgs::msg::Engage engage; + autoware_vehicle_msgs::msg::Engage engage; if (joy_->vehicle_engage()) { engage.engage = true; @@ -450,7 +467,7 @@ void AutowareJoyControllerNode::initTimer(double period_s) } AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options) -: Node("joy_controller", node_options) +: Node("autoware_joy_controller", node_options) { // Parameter joy_type_ = declare_parameter("joy_type"); @@ -470,30 +487,17 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & RCLCPP_INFO(get_logger(), "Joy type: %s", joy_type_.c_str()); // Callback Groups - callback_group_subscribers_ = - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); callback_group_services_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscriber_option = rclcpp::SubscriptionOptions(); - subscriber_option.callback_group = callback_group_subscribers_; // Subscriber - sub_joy_ = this->create_subscription( - "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), - subscriber_option); - if (!raw_control_) { - sub_odom_ = this->create_subscription( - "input/odometry", 1, - std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), - subscriber_option); - } else { + if (raw_control_) { twist_ = std::make_shared(); } // Publisher pub_control_command_ = - this->create_publisher( - "output/control_command", 1); + this->create_publisher("output/control_command", 1); pub_external_control_command_ = this->create_publisher( "output/external_control_command", 1); @@ -505,7 +509,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & pub_heartbeat_ = this->create_publisher("output/heartbeat", 1); pub_vehicle_engage_ = - this->create_publisher("output/vehicle_engage", 1); + this->create_publisher("output/vehicle_engage", 1); // Service Client client_emergency_stop_ = this->create_client( @@ -525,7 +529,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & // Timer initTimer(1.0 / update_rate_); } -} // namespace joy_controller +} // namespace autoware::joy_controller #include -RCLCPP_COMPONENTS_REGISTER_NODE(joy_controller::AutowareJoyControllerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::joy_controller::AutowareJoyControllerNode) diff --git a/control/autoware_lane_departure_checker/CMakeLists.txt b/control/autoware_lane_departure_checker/CMakeLists.txt new file mode 100644 index 0000000000000..d545e1cbb5ad1 --- /dev/null +++ b/control/autoware_lane_departure_checker/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_lane_departure_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +ament_auto_add_library(autoware_lane_departure_checker SHARED + src/lane_departure_checker_node/lane_departure_checker.cpp + src/lane_departure_checker_node/lane_departure_checker_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::lane_departure_checker::LaneDepartureCheckerNode" + EXECUTABLE lane_departure_checker_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_lane_departure_checker/README.md b/control/autoware_lane_departure_checker/README.md new file mode 100644 index 0000000000000..6eb62dcb2c23a --- /dev/null +++ b/control/autoware_lane_departure_checker/README.md @@ -0,0 +1,102 @@ +# Lane Departure Checker + +The **Lane Departure Checker** checks if vehicle follows a trajectory. If it does not follow the trajectory, it reports its status via `diagnostic_updater`. + +## Features + +This package includes the following features: + +- **Lane Departure**: Check if ego vehicle is going to be out of lane boundaries based on output from control module (predicted trajectory). +- **Trajectory Deviation**: Check if ego vehicle's pose does not deviate from the trajectory. Checking lateral, longitudinal and yaw deviation. +- **Road Border Departure**: Check if ego vehicle's footprint, generated from the control's output, extends beyond the road border. + +## Inner-workings / Algorithms + +### How to extend footprint by covariance + +1. Calculate the standard deviation of error ellipse(covariance) in vehicle coordinate. + + 1.Transform covariance into vehicle coordinate. + + $$ + \begin{align} + \left( \begin{array}{cc} x_{vehicle}\\ y_{vehicle}\\ \end{array} \right) = R_{map2vehicle} \left( \begin{array}{cc} x_{map}\\ y_{map}\\ \end{array} \right) + \end{align} + $$ + + Calculate covariance in vehicle coordinate. + + $$ + \begin{align} + Cov_{vehicle} &= E \left[ + \left( \begin{array}{cc} x_{vehicle}\\ y_{vehicle}\\ \end{array} \right) (x_{vehicle}, y_{vehicle}) \right] \\ + &= E \left[ R\left( \begin{array}{cc} x_{map}\\ y_{map}\\ \end{array} \right) + (x_{map}, y_{map})R^t + \right] \\ + &= R E\left[ \left( \begin{array}{cc} x_{map}\\ y_{map}\\ \end{array} \right) + (x_{map}, y_{map}) + \right] R^t \\ + &= R Cov_{map} R^t + \end{align} + $$ + + 2.The longitudinal length we want to expand is correspond to marginal distribution of $x_{vehicle}$, which is represented in $Cov_{vehicle}(0,0)$. In the same way, the lateral length is represented in $Cov_{vehicle}(1,1)$. Wikipedia reference [here](https://en.wikipedia.org/wiki/Multivariate_normal_distribution#Marginal_distributions). + +2. Expand footprint based on the standard deviation multiplied with `footprint_margin_scale`. + +## Interface + +### Input + +- /localization/kinematic_state [`nav_msgs::msg::Odometry`] +- /map/vector_map [`autoware_map_msgs::msg::LaneletMapBin`] +- /planning/mission_planning/route [`autoware_planning_msgs::msg::LaneletRoute`] +- /planning/scenario_planning/trajectory [`autoware_planning_msgs::msg::Trajectory`] +- /control/trajectory_follower/predicted_trajectory [`autoware_planning_msgs::msg::Trajectory`] + +### Output + +- [`diagnostic_updater`] lane_departure : Update diagnostic level when ego vehicle is out of lane. +- [`diagnostic_updater`] trajectory_deviation : Update diagnostic level when ego vehicle deviates from trajectory. + +## Parameters + +### Node Parameters + +#### General Parameters + +| Name | Type | Description | Default value | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| will_out_of_lane_checker | bool | Enable checker whether ego vehicle footprint will depart from lane | True | +| out_of_lane_checker | bool | Enable checker whether ego vehicle footprint is out of lane | True | +| boundary_departure_checker | bool | Enable checker whether ego vehicle footprint wil depart from boundary specified by boundary_types_to_detect | False | +| update_rate | double | Frequency for publishing [Hz] | 10.0 | +| visualize_lanelet | bool | Flag for visualizing lanelet | False | + +#### Parameters For Lane Departure + +| Name | Type | Description | Default value | +| :------------------------ | :--- | :------------------------------------------------ | :------------ | +| include_right_lanes | bool | Flag for including right lanelet in borders | False | +| include_left_lanes | bool | Flag for including left lanelet in borders | False | +| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | +| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | + +#### Parameters For Road Border Departure + +| Name | Type | Description | Default value | +| :----------------------- | :------------------------- | :---------------------------------------------------------- | :------------ | +| boundary_types_to_detect | std::vector\ | line_string types to detect with boundary_departure_checker | [road_border] | + +### Core Parameters + +| Name | Type | Description | Default value | +| :------------------------- | :----- | :--------------------------------------------------------------------------------- | :------------ | +| footprint_margin_scale | double | Coefficient for expanding footprint margin. Multiplied by 1 standard deviation. | 1.0 | +| footprint_extra_margin | double | Coefficient for expanding footprint margin. When checking for lane departure | 0.0 | +| resample_interval | double | Minimum Euclidean distance between points when resample trajectory.[m] | 0.3 | +| max_deceleration | double | Maximum deceleration when calculating braking distance. | 2.8 | +| delay_time | double | Delay time which took to actuate brake when calculating braking distance. [second] | 1.3 | +| max_lateral_deviation | double | Maximum lateral deviation in vehicle coordinate. [m] | 2.0 | +| max_longitudinal_deviation | double | Maximum longitudinal deviation in vehicle coordinate. [m] | 2.0 | +| max_yaw_deviation_deg | double | Maximum ego yaw deviation from trajectory. [deg] | 60.0 | diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml similarity index 95% rename from control/lane_departure_checker/config/lane_departure_checker.param.yaml rename to control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml index f0a8df21c425b..2724c28e2536a 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml @@ -17,6 +17,7 @@ # Core footprint_margin_scale: 1.0 + footprint_extra_margin: 0.0 resample_interval: 0.3 max_deceleration: 2.8 delay_time: 1.3 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp similarity index 77% rename from control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp rename to control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index c658cf4497973..b43a7d0d0369a 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ -#define 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 -#include -#include #include +#include +#include #include #include #include #include +#include #include #include @@ -45,21 +45,22 @@ #include #include -namespace lane_departure_checker +namespace autoware::lane_departure_checker { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::PoseDeviation; +using autoware::universe_utils::Segment2d; using autoware_planning_msgs::msg::LaneletRoute; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::PoseDeviation; -using tier4_autoware_utils::Segment2d; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; typedef boost::geometry::index::rtree> SegmentRtree; struct Param { double footprint_margin_scale; + double footprint_extra_margin; double resample_interval; double max_deceleration; double delay_time; @@ -102,17 +103,17 @@ class LaneDepartureChecker public: Output update(const Input & input); - void setParam(const Param & param, const vehicle_info_util::VehicleInfo vehicle_info) + void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info) { param_ = param; - vehicle_info_ptr_ = std::make_shared(vehicle_info); + vehicle_info_ptr_ = std::make_shared(vehicle_info); } void setParam(const Param & param) { param_ = param; } - void setVehicleInfo(const vehicle_info_util::VehicleInfo vehicle_info) + void setVehicleInfo(const autoware::vehicle_info_utils::VehicleInfo vehicle_info) { - vehicle_info_ptr_ = std::make_shared(vehicle_info); + vehicle_info_ptr_ = std::make_shared(vehicle_info); } bool checkPathWillLeaveLane( @@ -121,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( @@ -136,7 +137,7 @@ class LaneDepartureChecker private: Param param_; - std::shared_ptr vehicle_info_ptr_; + std::shared_ptr vehicle_info_ptr_; static PoseDeviation calcTrajectoryDeviation( const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, @@ -169,6 +170,6 @@ class LaneDepartureChecker const std::vector & vehicle_footprints, const SegmentRtree & uncrossable_segments); }; -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker -#endif // 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 new file mode 100644 index 0000000000000..e6421b2af84bb --- /dev/null +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -0,0 +1,160 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef 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 "autoware/universe_utils/ros/polling_subscriber.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::lane_departure_checker +{ +using autoware_map_msgs::msg::LaneletMapBin; + +struct NodeParam +{ + bool will_out_of_lane_checker; + bool out_of_lane_checker; + bool boundary_departure_checker; + + double update_rate; + bool visualize_lanelet; + bool include_right_lanes; + bool include_left_lanes; + bool include_opposite_lanes; + bool include_conflicting_lanes; + std::vector boundary_types_to_detect; +}; + +class LaneDepartureCheckerNode : public rclcpp::Node +{ +public: + explicit LaneDepartureCheckerNode(const rclcpp::NodeOptions & options); + +private: + // Subscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ + this, "~/input/odometry"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_lanelet_map_bin_{ + this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber sub_route_{ + this, "~/input/route"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ + this, "~/input/reference_trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ + this, "~/input/predicted_trajectory"}; + + // Data Buffer + nav_msgs::msg::Odometry::ConstSharedPtr current_odom_; + lanelet::LaneletMapPtr lanelet_map_; + lanelet::ConstLanelets shoulder_lanelets_; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_; + lanelet::routing::RoutingGraphPtr routing_graph_; + LaneletRoute::ConstSharedPtr route_; + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr cov_; + LaneletRoute::ConstSharedPtr last_route_; + lanelet::ConstLanelets route_lanelets_; + Trajectory::ConstSharedPtr reference_trajectory_; + Trajectory::ConstSharedPtr predicted_trajectory_; + + // Callback + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg); + void onRoute(const LaneletRoute::ConstSharedPtr msg); + void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); + void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); + + // Publisher + autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; + autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this}; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + bool isDataReady(); + bool isDataTimeout(); + bool isDataValid(); + void onTimer(); + + // Parameter + NodeParam node_param_; + Param param_; + double vehicle_length_m_; + + // Parameter callback + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + + // Core + Input input_{}; + Output output_{}; + std::unique_ptr lane_departure_checker_; + + // Diagnostic Updater + diagnostic_updater::Updater updater_{this}; + + void checkLaneDeparture(diagnostic_updater::DiagnosticStatusWrapper & stat); + void checkTrajectoryDeviation(diagnostic_updater::DiagnosticStatusWrapper & stat); + + // Visualization + visualization_msgs::msg::MarkerArray createMarkerArray() const; + + // Lanelet Neighbor Search + lanelet::ConstLanelets getAllSharedLineStringLanelets( + const lanelet::ConstLanelet & current_lane, const bool is_right, const bool is_left, + const bool is_opposite, const bool is_conflicting, const bool & invert_opposite); + + lanelet::ConstLanelets getAllRightSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite, + const bool & invert_opposite = false); + + lanelet::ConstLanelets getAllLeftSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite, + const bool & invert_opposite = false); + + boost::optional getLeftLanelet(const lanelet::ConstLanelet & lanelet); + + lanelet::Lanelets getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet); + boost::optional getRightLanelet( + const lanelet::ConstLanelet & lanelet) const; + + lanelet::Lanelets getRightOppositeLanelets(const lanelet::ConstLanelet & lanelet); +}; +} // namespace autoware::lane_departure_checker + +#endif // AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ diff --git a/control/lane_departure_checker/include/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 86% rename from control/lane_departure_checker/include/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 edc62cd6659fd..4435f282054ad 100644 --- a/control/lane_departure_checker/include/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 LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ -#define 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 #include @@ -63,4 +63,4 @@ inline FootprintMargin calcFootprintMargin( return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale}; } -#endif // LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#endif // AUTOWARE__LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml b/control/autoware_lane_departure_checker/launch/lane_departure_checker.launch.xml similarity index 75% rename from control/lane_departure_checker/launch/lane_departure_checker.launch.xml rename to control/autoware_lane_departure_checker/launch/lane_departure_checker.launch.xml index 62799c1187651..b9f820f66690b 100644 --- a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml +++ b/control/autoware_lane_departure_checker/launch/lane_departure_checker.launch.xml @@ -4,12 +4,12 @@ - + - + - + diff --git a/control/autoware_lane_departure_checker/package.xml b/control/autoware_lane_departure_checker/package.xml new file mode 100644 index 0000000000000..2a08db7288568 --- /dev/null +++ b/control/autoware_lane_departure_checker/package.xml @@ -0,0 +1,41 @@ + + + + autoware_lane_departure_checker + 0.1.0 + The autoware_lane_departure_checker package + Kyoichi Sugahara + Makoto Kurihara + Zulfaqar Azmi + + Apache License 2.0 + + ament_cmake_auto + 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 + nav_msgs + rclcpp + rclcpp_components + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_debug_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/lane_departure_checker/schema/lane_departure_checker.json b/control/autoware_lane_departure_checker/schema/lane_departure_checker.json similarity index 92% rename from control/lane_departure_checker/schema/lane_departure_checker.json rename to control/autoware_lane_departure_checker/schema/lane_departure_checker.json index c7f39fbf7ef8a..26e2f92f78ab4 100644 --- a/control/lane_departure_checker/schema/lane_departure_checker.json +++ b/control/autoware_lane_departure_checker/schema/lane_departure_checker.json @@ -11,6 +11,11 @@ "default": 1.0, "description": "Coefficient for expanding footprint margin. Multiplied by 1 standard deviation." }, + "footprint_extra_margin": { + "type": "number", + "default": 10.0, + "description": "Coefficient for expanding footprint margin." + }, "resample_interval": { "type": "number", "default": 0.3, @@ -53,6 +58,7 @@ }, "required": [ "footprint_margin_scale", + "footprint_extra_margin", "resample_interval", "max_deceleration", "max_lateral_deviation", diff --git a/control/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 similarity index 86% rename from control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp rename to control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index c48383a17ab4b..132eba8dde0c9 100644 --- a/control/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 "lane_departure_checker/lane_departure_checker.hpp" +#include "autoware/lane_departure_checker/lane_departure_checker.hpp" -#include "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,17 +30,17 @@ #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 { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Point; @@ -96,13 +96,13 @@ lanelet::ConstLanelets getCandidateLanelets( } // namespace -namespace lane_departure_checker +namespace autoware::lane_departure_checker { 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; @@ -261,13 +261,14 @@ std::vector LaneDepartureChecker::createVehicleFootprints( const PathWithLaneId & path) const { // Create vehicle footprint in base_link coordinate - const auto local_vehicle_footprint = vehicle_info_ptr_->createFootprint(); + const auto local_vehicle_footprint = + vehicle_info_ptr_->createFootprint(param_.footprint_extra_margin); // 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; @@ -321,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); @@ -338,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(); @@ -459,4 +462,4 @@ bool LaneDepartureChecker::willCrossBoundary( return false; } -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker diff --git a/control/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 similarity index 90% rename from control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp rename to control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 06d11133920f4..427cf0898470e 100644 --- a/control/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 "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 { @@ -120,7 +120,7 @@ void update_param( } // namespace -namespace lane_departure_checker +namespace autoware::lane_departure_checker { LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & options) : Node("lane_departure_checker_node", options) @@ -145,11 +145,12 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o declare_parameter>("boundary_types_to_detect"); // Vehicle Info - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_length_m_ = vehicle_info.vehicle_length_m; // Core Parameter param_.footprint_margin_scale = declare_parameter("footprint_margin_scale"); + param_.footprint_extra_margin = declare_parameter("footprint_extra_margin"); param_.resample_interval = declare_parameter("resample_interval"); param_.max_deceleration = declare_parameter("max_deceleration"); param_.delay_time = declare_parameter("delay_time"); @@ -168,22 +169,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o lane_departure_checker_ = std::make_unique(); lane_departure_checker_->setParam(param_, vehicle_info); - // Subscriber - sub_odom_ = this->create_subscription( - "~/input/odometry", 1, std::bind(&LaneDepartureCheckerNode::onOdometry, this, _1)); - sub_lanelet_map_bin_ = this->create_subscription( - "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(), - std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1)); - sub_route_ = this->create_subscription( - "~/input/route", rclcpp::QoS{1}.transient_local(), - std::bind(&LaneDepartureCheckerNode::onRoute, this, _1)); - sub_reference_trajectory_ = this->create_subscription( - "~/input/reference_trajectory", 1, - std::bind(&LaneDepartureCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = this->create_subscription( - "~/input/predicted_trajectory", 1, - std::bind(&LaneDepartureCheckerNode::onPredictedTrajectory, this, _1)); - // Publisher // Nothing @@ -200,36 +185,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o this, get_clock(), period_ns, std::bind(&LaneDepartureCheckerNode::onTimer, this)); } -void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - current_odom_ = msg; -} - -void LaneDepartureCheckerNode::onLaneletMapBin(const HADMapBin::ConstSharedPtr msg) -{ - lanelet_map_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_); - - // get all shoulder lanes - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_); - shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); -} - -void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) -{ - route_ = msg; -} - -void LaneDepartureCheckerNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) -{ - reference_trajectory_ = msg; -} - -void LaneDepartureCheckerNode::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg) -{ - predicted_trajectory_ = msg; -} - bool LaneDepartureCheckerNode::isDataReady() { if (!current_odom_) { @@ -296,9 +251,25 @@ 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(); + route_ = sub_route_.takeData(); + reference_trajectory_ = sub_reference_trajectory_.takeData(); + predicted_trajectory_ = sub_predicted_trajectory_.takeData(); + + const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeNewData(); + if (lanelet_map_bin_msg) { + lanelet_map_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *lanelet_map_bin_msg, lanelet_map_, &traffic_rules_, &routing_graph_); + + // get all shoulder lanes + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_); + shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); + } + if (!isDataReady()) { return; } @@ -403,6 +374,7 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( // Core update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale); + update_param(parameters, "footprint_extra_margin", param_.footprint_extra_margin); update_param(parameters, "resample_interval", param_.resample_interval); update_param(parameters, "max_deceleration", param_.max_deceleration); update_param(parameters, "delay_time", param_.delay_time); @@ -481,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; @@ -757,7 +729,7 @@ lanelet::Lanelets LaneDepartureCheckerNode::getRightOppositeLanelets( return opposite_lanelets; } -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE(lane_departure_checker::LaneDepartureCheckerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::lane_departure_checker::LaneDepartureCheckerNode) diff --git a/control/autoware_mpc_lateral_controller/CMakeLists.txt b/control/autoware_mpc_lateral_controller/CMakeLists.txt new file mode 100644 index 0000000000000..dff85d70419a1 --- /dev/null +++ b/control/autoware_mpc_lateral_controller/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_mpc_lateral_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(steering_offset_lib SHARED + src/steering_offset/steering_offset.cpp +) + +set(MPC_LAT_CON_LIB ${PROJECT_NAME}_lib) +ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED + src/mpc_lateral_controller.cpp + src/lowpass_filter.cpp + src/steering_predictor.cpp + src/mpc.cpp + src/mpc_trajectory.cpp + src/mpc_utils.cpp + src/qp_solver/qp_solver_osqp.cpp + src/qp_solver/qp_solver_unconstraint_fast.cpp + src/vehicle_model/vehicle_model_bicycle_dynamics.cpp + src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp + src/vehicle_model/vehicle_model_bicycle_kinematics.cpp + src/vehicle_model/vehicle_model_interface.cpp +) +target_link_libraries(${MPC_LAT_CON_LIB} steering_offset_lib) + +if(BUILD_TESTING) + set(TEST_LAT_SOURCES + test/test_mpc.cpp + test/test_mpc_utils.cpp + test/test_lowpass_filter.cpp + ) + set(TEST_LATERAL_CONTROLLER_EXE test_lateral_controller) + ament_add_ros_isolated_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES}) + target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${MPC_LAT_CON_LIB}) +endif() + +ament_auto_package(INSTALL_TO_SHARE + param +) diff --git a/control/autoware_mpc_lateral_controller/README.md b/control/autoware_mpc_lateral_controller/README.md new file mode 100644 index 0000000000000..6c5064a4aafe8 --- /dev/null +++ b/control/autoware_mpc_lateral_controller/README.md @@ -0,0 +1,266 @@ +# MPC Lateral Controller + +This is the design document for the lateral controller node +in the `autoware_trajectory_follower_node` package. + +## Purpose / Use cases + + + + +This node is used to general lateral control commands (steering angle and steering rate) +when following a path. + +## Design + + + + +The node uses an implementation of linear model predictive control (MPC) for accurate path tracking. +The MPC uses a model of the vehicle to simulate the trajectory resulting from the control command. +The optimization of the control command is formulated as a Quadratic Program (QP). + +Different vehicle models are implemented: + +- kinematics : bicycle kinematics model with steering 1st-order delay. +- kinematics_no_delay : bicycle kinematics model without steering delay. +- dynamics : bicycle dynamics model considering slip angle. + The kinematics model is being used by default. Please see the reference [1] for more details. + +For the optimization, a Quadratic Programming (QP) solver is used and two options are currently implemented: + + + +- unconstraint_fast : use least square method to solve unconstraint QP with eigen. +- [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) + algorithm (for more details see the related papers at + the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section): + +### Filtering + +Filtering is required for good noise reduction. +A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is employed for processing the yaw and lateral errors, which are used as inputs for the MPC, as well as for refining the output steering angle. +Other filtering methods can be considered as long as the noise reduction performances are good +enough. +The moving average filter for example is not suited and can yield worse results than without any +filtering. + +## Assumptions / Known limits + + + +The tracking is not accurate if the first point of the reference trajectory is at or in front of the current ego pose. + +## Inputs / Outputs / API + + + + +### Inputs + +Set the following from the [controller_node](../autoware_trajectory_follower_node/README.md) + +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry +- `autoware_vehicle_msgs/SteeringReport`: current steering + +### Outputs + +Return LateralOutput which contains the following to the controller node + +- `autoware_control_msgs/Lateral` +- LateralSyncData + - steer angle convergence + +### MPC class + +The `MPC` class (defined in `mpc.hpp`) provides the interface with the MPC algorithm. +Once a vehicle model, a QP solver, and the reference trajectory to follow have been set +(using `setVehicleModel()`, `setQPSolver()`, `setReferenceTrajectory()`), a lateral control command +can be calculated by providing the current steer, velocity, and pose to function `calculateMPC()`. + +### Parameter description + +The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the +AutonomouStuff Lexus RX 450h for under 40 km/h driving. + +#### System + +| Name | Type | Description | Default value | +| :------------------------ | :------ | :-------------------------------------------------------------------------- | :------------ | +| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | +| use_steer_prediction | boolean | flag for using steer prediction (do not use steer measurement) | false | +| admissible_position_error | double | stop vehicle when following position error is larger than this value [m] | 5.0 | +| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad] | 1.57 | + +#### Path Smoothing + +| Name | Type | Description | Default value | +| :-------------------------------- | :------ | :--------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_path_smoothing | boolean | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | false | +| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 25 | +| curvature_smoothing_num_traj | int | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 | +| curvature_smoothing_num_ref_steer | int | index distance of points used in curvature calculation for reference steering command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 | + +#### Trajectory Extending + +| Name | Type | Description | Default value | +| :------------------------------------ | :------ | :-------------------------------------------- | :------------ | +| extend_trajectory_for_end_yaw_control | boolean | trajectory extending flag for end yaw control | true | + +#### MPC Optimization + +| Name | Type | Description | Default value | +| :-------------------------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------- | :------------ | +| qp_solver_type | string | QP solver option. described below in detail. | "osqp" | +| mpc_prediction_horizon | int | total prediction step for MPC | 50 | +| mpc_prediction_dt | double | prediction period for one step [s] | 0.1 | +| mpc_weight_lat_error | double | weight for lateral error | 1.0 | +| mpc_weight_heading_error | double | weight for heading error | 0.0 | +| mpc_weight_heading_error_squared_vel | double | weight for heading error \* velocity | 0.3 | +| mpc_weight_steering_input | double | weight for steering error (steer command - reference steer) | 1.0 | +| mpc_weight_steering_input_squared_vel | double | weight for steering error (steer command - reference steer) \* velocity | 0.25 | +| mpc_weight_lat_jerk | double | weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.1 | +| mpc_weight_steer_rate | double | weight for steering rate [rad/s] | 0.0 | +| mpc_weight_steer_acc | double | weight for derivatives of the steering rate [rad/ss] | 0.000001 | +| mpc_low_curvature_weight_lat_error | double | [used in a low curvature trajectory] weight for lateral error | 0.1 | +| mpc_low_curvature_weight_heading_error | double | [used in a low curvature trajectory] weight for heading error | 0.0 | +| mpc_low_curvature_weight_heading_error_squared_vel | double | [used in a low curvature trajectory] weight for heading error \* velocity | 0.3 | +| mpc_low_curvature_weight_steering_input | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) | 1.0 | +| mpc_low_curvature_weight_steering_input_squared_vel | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) \* velocity | 0.25 | +| mpc_low_curvature_weight_lat_jerk | double | [used in a low curvature trajectory] weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | +| mpc_low_curvature_weight_steer_rate | double | [used in a low curvature trajectory] weight for steering rate [rad/s] | 0.0 | +| mpc_low_curvature_weight_steer_acc | double | [used in a low curvature trajectory] weight for derivatives of the steering rate [rad/ss] | 0.000001 | +| mpc_low_curvature_thresh_curvature | double | threshold of curvature to use "low_curvature" parameter | 0.0 | +| mpc_weight_terminal_lat_error | double | terminal lateral error weight in matrix Q to improve mpc stability | 1.0 | +| mpc_weight_terminal_heading_error | double | terminal heading error weight in matrix Q to improve mpc stability | 0.1 | +| mpc_zero_ff_steer_deg | double | threshold that feed-forward angle becomes zero | 0.5 | +| mpc_acceleration_limit | double | limit on the vehicle's acceleration | 2.0 | +| mpc_velocity_time_constant | double | time constant used for velocity smoothing | 0.3 | +| mpc_min_prediction_length | double | minimum prediction length | 5.0 | + +#### Vehicle Model + +| Name | Type | Description | Default value | +| :----------------------------------- | :------- | :--------------------------------------------------------------------------------- | :------------------- | +| vehicle_model_type | string | vehicle model type for mpc prediction | "kinematics" | +| input_delay | double | steering input delay time for delay compensation | 0.24 | +| vehicle_model_steer_tau | double | steering dynamics time constant (1d approximation) [s] | 0.3 | +| steer_rate_lim_dps_list_by_curvature | [double] | steering angle rate limit list depending on curvature [deg/s] | [40.0, 50.0, 60.0] | +| curvature_list_for_steer_rate_lim | [double] | curvature list for steering angle rate limit interpolation in ascending order [/m] | [0.001, 0.002, 0.01] | +| steer_rate_lim_dps_list_by_velocity | [double] | steering angle rate limit list depending on velocity [deg/s] | [60.0, 50.0, 40.0] | +| velocity_list_for_steer_rate_lim | [double] | velocity list for steering angle rate limit interpolation in ascending order [m/s] | [10.0, 15.0, 20.0] | +| acceleration_limit | double | acceleration limit for trajectory velocity modification [m/ss] | 2.0 | +| velocity_time_constant | double | velocity dynamics time constant for trajectory velocity modification [s] | 0.3 | + +#### Lowpass Filter for Noise Reduction + +| Name | Type | Description | Default value | +| :------------------------ | :----- | :------------------------------------------------------------------ | :------------ | +| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 | +| error_deriv_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for error derivative [Hz] | 5.0 | + +#### Stop State + +| Name | Type | Description | Default value | +| :------------------------------------------- | :------ | :---------------------------------------------------------------------------------------------- | :------------ | +| stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.001 | +| stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.001 | +| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | +| keep_steer_control_until_converged | boolean | keep steer control until steer is converged | true | +| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | +| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | +| mpc_converged_threshold_rps | double | threshold value to be sure output of the optimization is converged, it is used in stopped state | 0.01 | + +(\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state. + +#### Steer Offset + +Defined in the `steering_offset` namespace. This logic is designed as simple as possible, with minimum design parameters. + +| Name | Type | Description | Default value | +| :---------------------------------- | :------ | :----------------------------------------------------------------------------------------------- | :------------ | +| enable_auto_steering_offset_removal | boolean | Estimate the steering offset and apply compensation | true | +| update_vel_threshold | double | If the velocity is smaller than this value, the data is not used for the offset estimation | 5.56 | +| update_steer_threshold | double | If the steering angle is larger than this value, the data is not used for the offset estimation. | 0.035 | +| average_num | int | The average of this number of data is used as a steering offset. | 1000 | +| steering_offset_limit | double | The angle limit to be applied to the offset compensation. | 0.02 | + +##### For dynamics model (WIP) + +| Name | Type | Description | Default value | +| :------------ | :----- | :------------------------------------------ | :------------ | +| cg_to_front_m | double | distance from baselink to the front axle[m] | 1.228 | +| cg_to_rear_m | double | distance from baselink to the rear axle [m] | 1.5618 | +| mass_fl | double | mass applied to front left tire [kg] | 600 | +| mass_fr | double | mass applied to front right tire [kg] | 600 | +| mass_rl | double | mass applied to rear left tire [kg] | 600 | +| mass_rr | double | mass applied to rear right tire [kg] | 600 | +| cf | double | front cornering power [N/rad] | 155494.663 | +| cr | double | rear cornering power [N/rad] | 155494.663 | + +### How to tune MPC parameters + +#### Set kinematics information + +First, it's important to set the appropriate parameters for vehicle kinematics. This includes parameters like `wheelbase`, which represents the distance between the front and rear wheels, and `max_steering_angle`, which indicates the maximum tire steering angle. These parameters should be set in the `vehicle_info.param.yaml`. + +#### Set dynamics information + +Next, you need to set the proper parameters for the dynamics model. These include the time constant `steering_tau` and time delay `steering_delay` for steering dynamics, and the maximum acceleration `mpc_acceleration_limit` and the time constant `mpc_velocity_time_constant` for velocity dynamics. + +#### Confirmation of the input information + +It's also important to make sure the input information is accurate. Information such as the velocity of the center of the rear wheel [m/s] and the steering angle of the tire [rad] is required. Please note that there have been frequent reports of performance degradation due to errors in input information. For instance, there are cases where the velocity of the vehicle is offset due to an unexpected difference in tire radius, or the tire angle cannot be accurately measured due to a deviation in the steering gear ratio or midpoint. It is suggested to compare information from multiple sensors (e.g., integrated vehicle speed and GNSS position, steering angle and IMU angular velocity), and ensure the input information for MPC is appropriate. + +#### MPC weight tuning + +Then, tune the weights of the MPC. One simple approach of tuning is to keep the weight for the lateral deviation (`weight_lat_error`) constant, and vary the input weight (`weight_steering_input`) while observing the trade-off between steering oscillation and control accuracy. + +Here, `weight_lat_error` acts to suppress the lateral error in path following, while `weight_steering_input` works to adjust the steering angle to a standard value determined by the path's curvature. When `weight_lat_error` is large, the steering moves significantly to improve accuracy, which can cause oscillations. On the other hand, when `weight_steering_input` is large, the steering doesn't respond much to tracking errors, providing stable driving but potentially reducing tracking accuracy. + +The steps are as follows: + +1. Set `weight_lat_error` = 0.1, `weight_steering_input` = 1.0 and other weights to 0. +2. If the vehicle oscillates when driving, set `weight_steering_input` larger. +3. If the tracking accuracy is low, set `weight_steering_input` smaller. + +If you want to adjust the effect only in the high-speed range, you can use `weight_steering_input_squared_vel`. This parameter corresponds to the steering weight in the high-speed range. + +#### Descriptions for weights + +- `weight_lat_error`: Reduce lateral tracking error. This acts like P gain in PID. +- `weight_heading_error`: Make a drive straight. This acts like D gain in PID. +- `weight_heading_error_squared_vel_coeff` : Make a drive straight in high speed range. +- `weight_steering_input`: Reduce oscillation of tracking. +- `weight_steering_input_squared_vel_coeff`: Reduce oscillation of tracking in high speed range. +- `weight_lat_jerk`: Reduce lateral jerk. +- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability. +- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability. + +#### Other tips for tuning + +Here are some tips for adjusting other parameters: + +- In theory, increasing terminal weights, `weight_terminal_lat_error` and `weight_terminal_heading_error`, can enhance the tracking stability. This method sometimes proves effective. +- A larger `prediction_horizon` and a smaller `prediction_sampling_time` are efficient for tracking performance. However, these come at the cost of higher computational costs. +- If you want to modify the weight according to the trajectory curvature (for instance, when you're driving on a sharp curve and want a larger weight), use `mpc_low_curvature_thresh_curvature` and adjust `mpc_low_curvature_weight_**` weights. +- If you want to adjust the steering rate limit based on the vehicle speed and trajectory curvature, you can modify the values of `steer_rate_lim_dps_list_by_curvature`, `curvature_list_for_steer_rate_lim`, `steer_rate_lim_dps_list_by_velocity`, `velocity_list_for_steer_rate_lim`. By doing this, you can enforce the steering rate limit during high-speed driving or relax it while curving. +- In case your target curvature appears jagged, adjusting `curvature_smoothing` becomes critically important for accurate curvature calculations. A larger value yields a smooth curvature calculation which reduces noise but can cause delay in feedforward computation and potentially degrade performance. +- Adjusting the `steering_lpf_cutoff_hz` value can also be effective to forcefully reduce computational noise. This refers to the cutoff frequency in the second order Butterworth filter installed in the final layer. The smaller the cutoff frequency, the stronger the noise reduction, but it also induce operation delay. +- If the vehicle consistently deviates laterally from the trajectory, it's most often due to the offset of the steering sensor or self-position estimation. It's preferable to eliminate these biases before inputting into MPC, but it's also possible to remove this bias within MPC. To utilize this, set `enable_auto_steering_offset_removal` to true and activate the steering offset remover. The steering offset estimation logic works when driving at high speeds with the steering close to the center, applying offset removal. +- If the onset of steering in curves is late, it's often due to incorrect delay time and time constant in the steering model. Please recheck the values of `input_delay` and `vehicle_model_steer_tau`. Additionally, as a part of its debug information, MPC outputs the current steering angle assumed by the MPC model, so please check if that steering angle matches the actual one. + +## References / External links + + + +- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", + Robotics Institute, Carnegie Mellon University, February 2009. + +## Related issues + + diff --git a/control/mpc_lateral_controller/image/vehicle_error_kinematics.png b/control/autoware_mpc_lateral_controller/image/vehicle_error_kinematics.png similarity index 100% rename from control/mpc_lateral_controller/image/vehicle_error_kinematics.png rename to control/autoware_mpc_lateral_controller/image/vehicle_error_kinematics.png diff --git a/control/mpc_lateral_controller/image/vehicle_kinematics.png b/control/autoware_mpc_lateral_controller/image/vehicle_kinematics.png similarity index 100% rename from control/mpc_lateral_controller/image/vehicle_kinematics.png rename to control/autoware_mpc_lateral_controller/image/vehicle_kinematics.png 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 new file mode 100644 index 0000000000000..8a32d06c8066e --- /dev/null +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/lowpass_filter.hpp @@ -0,0 +1,105 @@ +// Copyright 2018-2021 The Autoware Foundation +// +// 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__MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define AUTOWARE__MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ + +#include +#include +#include +#include + +namespace autoware::motion::control::mpc_lateral_controller +{ +/** + * @brief 2nd-order Butterworth Filter + * reference : S. Butterworth, "On the Theory of Filter Amplifier", Experimental wireless, 1930. + */ +class Butterworth2dFilter +{ +private: + double m_y1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_y2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_u1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_u2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_a1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_a2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_b0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_b1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_b2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + +public: + /** + * @brief constructor with initialization + * @param [in] dt sampling time + * @param [in] f_cutoff_hz cutoff frequency [Hz] + */ + explicit Butterworth2dFilter(double dt = 0.01, double f_cutoff_hz = 5.0); + + /** + * @brief destructor + */ + ~Butterworth2dFilter(); + + /** + * @brief constructor + * @param [in] dt sampling time + * @param [in] f_cutoff_hz cutoff frequency [Hz] + */ + void initialize(const double & dt, const double & f_cutoff_hz); + + /** + * @brief filtering (call this function at each sampling time with input) + * @param [in] u scalar input for filter + * @return filtered scalar value + */ + double filter(const double & u); + + /** + * @brief filtering for time-series data + * @param [in] t time-series data for input vector + * @param [out] u object vector + */ + void filt_vector(const std::vector & t, std::vector & u) const; + + /** + * @brief filtering for time-series data from both forward-backward direction for zero phase delay + * @param [in] t time-series data for input vector + * @param [out] u object vector + */ + void filtfilt_vector( + const std::vector & t, + std::vector & u) const; // filtering forward and backward direction + + /** + * @brief get filter coefficients + * @param [out] coeffs coefficients of filter [a0, a1, a2, b0, b1, b2]. + */ + void getCoefficients(std::vector & coeffs) const; +}; + +/** + * @brief Move Average Filter + */ +namespace MoveAverageFilter +{ +/** + * @brief filtering vector + * @param [in] num index distance for moving average filter + * @param [out] u object vector + */ +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_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp similarity index 94% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 2e83c5ab847c4..058eb45bfaaff 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_HPP_ - -#include "mpc_lateral_controller/lowpass_filter.hpp" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" -#include "mpc_lateral_controller/steering_predictor.hpp" -#include "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_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" @@ -38,9 +38,9 @@ namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32MultiArrayStamped; @@ -378,7 +378,7 @@ class MPC */ Float32MultiArrayStamped generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, - const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, + const MPCMatrix & mpc_matrix, const Lateral & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const; /** @@ -441,9 +441,8 @@ class MPC * @return True if the MPC calculation is successful, false otherwise. */ bool calculateMPC( - const SteeringReport & current_steer, const Odometry & current_kinematics, - AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, - Float32MultiArrayStamped & diagnostic); + const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd, + Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic); /** * @brief Set the reference trajectory to be followed. @@ -526,4 +525,4 @@ class MPC }; // class MPC } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp similarity index 86% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp index 92b01247105c6..1f16bbc39bbbf 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -12,20 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ - -#include "mpc_lateral_controller/mpc.hpp" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" -#include "mpc_lateral_controller/steering_offset/steering_offset.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 "trajectory_follower_base/lateral_controller_base.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -42,9 +41,9 @@ namespace autoware::motion::control::mpc_lateral_controller { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; @@ -97,10 +96,10 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase bool m_is_mpc_history_filled{false}; // store the last mpc outputs for 1 sec - std::vector> m_mpc_steering_history{}; + std::vector> m_mpc_steering_history{}; // set the mpc steering output to history - void setSteeringToHistory(const AckermannLateralCommand & steering); + void setSteeringToHistory(const Lateral & steering); // check if the mpc steering output is converged bool isMpcConverged(); @@ -118,7 +117,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase bool m_is_ctrl_cmd_prev_initialized = false; // Previous control command for path following. - AckermannLateralCommand m_ctrl_cmd_prev; + Lateral m_ctrl_cmd_prev; // Flag indicating whether the first trajectory has been received. bool m_has_received_first_trajectory = false; @@ -200,7 +199,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @param ctrl_cmd Control command to be created. * @return Created control command. */ - AckermannLateralCommand createCtrlCmdMsg(const AckermannLateralCommand & ctrl_cmd); + Lateral createCtrlCmdMsg(const Lateral & ctrl_cmd); /** * @brief Publish the predicted future trajectory. @@ -218,13 +217,13 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @brief Get the stop control command. * @return Stop control command. */ - AckermannLateralCommand getStopControlCommand() const; + Lateral getStopControlCommand() const; /** * @brief Get the control command applied before initialization. * @return Initial control command. */ - AckermannLateralCommand getInitialControlCommand() const; + Lateral getInitialControlCommand() const; /** * @brief Check if the ego car is in a stopped state. @@ -250,7 +249,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @param cmd Steering control command to be checked. * @return True if the steering control is converged and stable, false otherwise. */ - bool isSteerConverged(const AckermannLateralCommand & cmd) const; + bool isSteerConverged(const Lateral & cmd) const; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; @@ -282,4 +281,4 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp similarity index 86% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp index 7e1c7ebdf1348..b761aa9f19d23 100644 --- a/control/mpc_lateral_controller/include/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 MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ -#define 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" @@ -109,15 +109,15 @@ class MPCTrajectory return points; } - std::vector toTrajectoryPoints() const + std::vector toTrajectoryPoints() const { - std::vector points; + std::vector points; for (size_t i = 0; i < x.size(); ++i) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; + autoware_planning_msgs::msg::TrajectoryPoint point; 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 // MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_utils.hpp similarity index 95% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_utils.hpp index 9062ff1ea34e3..87f6c0064a2b8 100644 --- a/control/mpc_lateral_controller/include/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 MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ -#define 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,10 +26,10 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware/mpc_lateral_controller/mpc_trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -43,8 +43,8 @@ namespace autoware::motion::control::mpc_lateral_controller namespace MPCUtils { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; /** @@ -230,4 +230,4 @@ void update_param( } // namespace MPCUtils } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ diff --git a/control/mpc_lateral_controller/include/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/mpc_lateral_controller/include/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 ca30bd30e4dd1..9756d547c97ff 100644 --- a/control/mpc_lateral_controller/include/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 MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ -#define 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 // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/include/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 88% rename from control/mpc_lateral_controller/include/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 2611f94da324f..1e1295cb06b1e 100644 --- a/control/mpc_lateral_controller/include/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 MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#define 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 "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 // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ diff --git a/control/mpc_lateral_controller/include/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/mpc_lateral_controller/include/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 ef337eaaa8528..79d7d25aeff91 100644 --- a/control/mpc_lateral_controller/include/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 MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ -#define 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 "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 // 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/mpc_lateral_controller/include/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/mpc_lateral_controller/include/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 494961ef679d2..f5e357ddc717b 100644 --- a/control/mpc_lateral_controller/include/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 MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ -#define 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 // MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/steering_predictor.hpp similarity index 86% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp rename to control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/steering_predictor.hpp index 7ca2fa1a61528..474c98bfa13b2 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/steering_predictor.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ -#define 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" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include #include namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; +using autoware_control_msgs::msg::Lateral; class SteeringPredictor { @@ -61,7 +61,7 @@ class SteeringPredictor double m_input_delay; // Buffer of sent control commands. - std::vector m_ctrl_cmd_vec; + std::vector m_ctrl_cmd_vec; /** * @brief Get the sum of all steering commands over the given time range. @@ -81,4 +81,4 @@ class SteeringPredictor } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#endif // AUTOWARE__MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ diff --git a/control/mpc_lateral_controller/include/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 92% rename from control/mpc_lateral_controller/include/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 488a3c7ab8be2..654b7665f942a 100644 --- a/control/mpc_lateral_controller/include/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 MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#define 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 "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 // 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 new file mode 100644 index 0000000000000..d638142daa844 --- /dev/null +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -0,0 +1,109 @@ +// Copyright 2018-2021 The Autoware Foundation +// +// 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. + +/* + * Representation + * e : lateral error + * th : heading angle error + * steer : steering angle + * steer_d: desired steering angle (input) + * v : velocity + * W : wheelbase length + * tau : time constant for steering dynamics + * + * State & Input + * x = [e, th, steer]^T + * u = steer_d + * + * Nonlinear model + * dx1/dt = v * sin(x2) + * dx2/dt = v * tan(x3) / W + * dx3/dt = -(x3 - u) / tau + * + * Linearized model around reference point (v = v_r, th = th_r, steer = steer_r) + * [0, vr, 0] [ 0] [ 0] + * dx/dt = [0, 0, vr/W/cos(steer_r)^2] * x + [ 0] * u + [-vr*steer_r/W/cos(steer_r)^2] + * [0, 0, 1/tau] [1/tau] [ 0] + * + */ + +#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 +#include + +#include + +namespace autoware::motion::control::mpc_lateral_controller +{ + +/** + * Vehicle model class of bicycle kinematics + * @brief calculate model-related values + */ +class KinematicsBicycleModel : public VehicleModelInterface +{ +public: + /** + * @brief constructor with parameter initialization + * @param [in] wheelbase wheelbase length [m] + * @param [in] steer_lim steering angle limit [rad] + * @param [in] steer_tau steering time constant for 1d-model [s] + */ + KinematicsBicycleModel(const double wheelbase, const double steer_lim, const double steer_tau); + + /** + * @brief destructor + */ + ~KinematicsBicycleModel() = default; + + /** + * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk + * @param [out] a_d coefficient matrix + * @param [out] b_d coefficient matrix + * @param [out] c_d coefficient matrix + * @param [out] w_d coefficient matrix + * @param [in] dt Discretization time [s] + */ + void calculateDiscreteMatrix( + Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, + const double dt) override; + + /** + * @brief calculate reference input + * @param [out] u_ref input + */ + void calculateReferenceInput(Eigen::MatrixXd & u_ref) override; + + std::string modelName() override { return "kinematics"; }; + + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + +private: + double m_steer_lim; //!< @brief steering angle limit [rad] + 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_ diff --git a/control/mpc_lateral_controller/include/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 87% rename from control/mpc_lateral_controller/include/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 b503ad8eb1d13..83d0f8f9c1ae1 100644 --- a/control/mpc_lateral_controller/include/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 MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +#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 "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -101,4 +101,6 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface double m_steer_lim; //!< @brief steering angle limit [rad] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +// clang-format off +#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 new file mode 100644 index 0000000000000..1b3db8fb3f80f --- /dev/null +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -0,0 +1,152 @@ +// Copyright 2018-2021 The Autoware Foundation +// +// 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__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 + +#include + +namespace autoware::motion::control::mpc_lateral_controller +{ + +/** + * Vehicle model class + * @brief calculate model-related values + */ +class VehicleModelInterface +{ +protected: + const int m_dim_x; //!< @brief dimension of state x + const int m_dim_u; //!< @brief dimension of input u + const int m_dim_y; //!< @brief dimension of output y + double m_velocity; //!< @brief vehicle velocity [m/s] + double m_curvature; //!< @brief curvature on the linearized point on path + double m_wheelbase; //!< @brief wheelbase of the vehicle [m] + +public: + /** + * @brief constructor + * @param [in] dim_x dimension of state x + * @param [in] dim_u dimension of input u + * @param [in] dim_y dimension of output y + * @param [in] wheelbase wheelbase of the vehicle [m] + */ + VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheelbase); + + /** + * @brief destructor + */ + virtual ~VehicleModelInterface() = default; + + /** + * @brief get state x dimension + * @return state dimension + */ + int getDimX() const; + + /** + * @brief get input u dimension + * @return input dimension + */ + int getDimU() const; + + /** + * @brief get output y dimension + * @return output dimension + */ + int getDimY() const; + + /** + * @brief get wheelbase of the vehicle + * @return wheelbase value [m] + */ + double getWheelbase() const; + + /** + * @brief set velocity + * @param [in] velocity vehicle velocity + */ + void setVelocity(const double velocity); + + /** + * @brief set curvature + * @param [in] curvature curvature on the linearized point on path + */ + void setCurvature(const double curvature); + + /** + * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk + * @param [out] a_d coefficient matrix + * @param [out] b_d coefficient matrix + * @param [out] c_d coefficient matrix + * @param [out] w_d coefficient matrix + * @param [in] dt Discretization time [s] + */ + virtual void calculateDiscreteMatrix( + Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, + const double dt) = 0; + + /** + * @brief calculate reference input + * @param [out] u_ref input + */ + virtual void calculateReferenceInput(Eigen::MatrixXd & u_ref) = 0; + + /** + * @brief returns model name e.g. kinematics, dynamics + */ + virtual std::string modelName() = 0; + + /** + * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in world + * coordinate + * @param a_d The MPC A matrix used for optimization. + * @param b_d The MPC B matrix used for optimization. + * @param c_d The MPC C matrix used for optimization. + * @param w_d The MPC W matrix used for optimization. + * @param x0 initial state vector. + * @param Uex The optimized input vector. + * @param reference_trajectory The resampled reference trajectory. + * @param dt delta time used in the optimization + * @return The predicted trajectory. + */ + virtual MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const = 0; + + /** + * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in Frenet + * Coordinate + * @param a_d The MPC A matrix used for optimization. + * @param b_d The MPC B matrix used for optimization. + * @param c_d The MPC C matrix used for optimization. + * @param w_d The MPC W matrix used for optimization. + * @param x0 initial state vector. + * @param Uex The optimized input vector. + * @param reference_trajectory The resampled reference trajectory. + * @param dt delta time used in the optimization + * @return The predicted trajectory. + */ + virtual MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + 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_ diff --git a/control/mpc_lateral_controller/model_predictive_control_algorithm.md b/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md similarity index 100% rename from control/mpc_lateral_controller/model_predictive_control_algorithm.md rename to control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml new file mode 100644 index 0000000000000..e25352797a0d0 --- /dev/null +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -0,0 +1,48 @@ + + + + autoware_mpc_lateral_controller + 1.0.0 + MPC-based lateral controller + + Takamasa Horibe + Takayuki Murooka + + Apache 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + 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 + diagnostic_updater + eigen + geometry_msgs + interpolation + osqp_interface + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros + tier4_debug_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml similarity index 100% rename from control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml rename to control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml diff --git a/control/mpc_lateral_controller/src/lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp similarity index 98% rename from control/mpc_lateral_controller/src/lowpass_filter.cpp rename to control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp index 8fbf3488c5a2e..bd90b2aa896f6 100644 --- a/control/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 "mpc_lateral_controller/lowpass_filter.hpp" +#include "autoware/mpc_lateral_controller/lowpass_filter.hpp" #include diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp similarity index 95% rename from control/mpc_lateral_controller/src/mpc.cpp rename to control/autoware_mpc_lateral_controller/src/mpc.cpp index 373de2e0bd911..1f3327bf28f2e 100644 --- a/control/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 "mpc_lateral_controller/mpc.hpp" +#include "autoware/mpc_lateral_controller/mpc.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 "mpc_lateral_controller/mpc_utils.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) { @@ -36,9 +36,8 @@ MPC::MPC(rclcpp::Node & node) } bool MPC::calculateMPC( - const SteeringReport & current_steer, const Odometry & current_kinematics, - AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, - Float32MultiArrayStamped & diagnostic) + const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd, + Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic) { // since the reference trajectory does not take into account the current velocity of the ego // vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics. @@ -117,7 +116,7 @@ bool MPC::calculateMPC( Float32MultiArrayStamped MPC::generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, - const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, + const MPCMatrix & mpc_matrix, const Lateral & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const { Float32MultiArrayStamped diagnostic; @@ -167,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); @@ -184,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; @@ -390,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( @@ -500,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; @@ -677,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/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp similarity index 94% rename from control/mpc_lateral_controller/src/mpc_lateral_controller.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 882150ffc1644..7634ebb74eca7 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc_lateral_controller.hpp" - -#include "motion_utils/trajectory/trajectory.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#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 "tf2/utils.h" #include "tf2_ros/create_timer_ros.h" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -69,7 +69,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps"); // [rad/s] /* mpc parameters */ - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; constexpr double deg2rad = static_cast(M_PI) / 180.0; m_mpc->m_steer_lim = vehicle_info.max_steer_angle_rad; @@ -239,7 +239,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_current_steering.steering_tire_angle -= steering_offset_->getOffset(); } - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory predicted_traj; Float32MultiArrayStamped debug_values; @@ -309,7 +309,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( return createLateralOutput(ctrl_cmd, is_mpc_solved); } -bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) const +bool MpcLateralController::isSteerConverged(const Lateral & cmd) const { // wait for a while to propagate the trajectory shape to the output command when the trajectory // shape is changed. @@ -381,17 +381,17 @@ void MpcLateralController::setTrajectory( } } -AckermannLateralCommand MpcLateralController::getStopControlCommand() const +Lateral MpcLateralController::getStopControlCommand() const { - AckermannLateralCommand cmd; + Lateral cmd; cmd.steering_tire_angle = static_cast(m_steer_cmd_prev); cmd.steering_tire_rotation_rate = 0.0; return cmd; } -AckermannLateralCommand MpcLateralController::getInitialControlCommand() const +Lateral MpcLateralController::getInitialControlCommand() const { - AckermannLateralCommand cmd; + Lateral cmd; cmd.steering_tire_angle = m_current_steering.steering_tire_angle; cmd.steering_tire_rotation_rate = 0.0; return cmd; @@ -408,7 +408,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); @@ -429,8 +429,7 @@ bool MpcLateralController::isStoppedState() const } } -AckermannLateralCommand MpcLateralController::createCtrlCmdMsg( - const AckermannLateralCommand & ctrl_cmd) +Lateral MpcLateralController::createCtrlCmdMsg(const Lateral & ctrl_cmd) { auto out = ctrl_cmd; out.stamp = clock_->now(); @@ -456,7 +455,7 @@ void MpcLateralController::publishDebugValues(Float32MultiArrayStamped & debug_v m_pub_steer_offset->publish(offset); } -void MpcLateralController::setSteeringToHistory(const AckermannLateralCommand & steering) +void MpcLateralController::setSteeringToHistory(const Lateral & steering) { const auto time = clock_->now(); if (m_mpc_steering_history.empty()) { @@ -617,7 +616,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; diff --git a/control/mpc_lateral_controller/src/mpc_trajectory.cpp b/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp similarity index 97% rename from control/mpc_lateral_controller/src/mpc_trajectory.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp index b7fed942375e8..842689527c847 100644 --- a/control/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 "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/mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp similarity index 95% rename from control/mpc_lateral_controller/src/mpc_utils.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index 01a81d9015b47..37eb47ab0396e 100644 --- a/control/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 "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/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp similarity index 97% rename from control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp rename to control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index 7c76d67e75aa3..66a7dcacbeae1 100644 --- a/control/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 "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/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 similarity index 93% rename from control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp rename to control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp index b0357138cd646..5c69c176a1858 100644 --- a/control/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 "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/mpc_lateral_controller/src/steering_offset/steering_offset.cpp b/control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp similarity index 95% rename from control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp rename to control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp index 60d4dd7901394..63c0c097c2ca1 100644 --- a/control/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 "mpc_lateral_controller/steering_offset/steering_offset.hpp" +#include "autoware/mpc_lateral_controller/steering_offset/steering_offset.hpp" #include #include diff --git a/control/mpc_lateral_controller/src/steering_predictor.cpp b/control/autoware_mpc_lateral_controller/src/steering_predictor.cpp similarity index 97% rename from control/mpc_lateral_controller/src/steering_predictor.cpp rename to control/autoware_mpc_lateral_controller/src/steering_predictor.cpp index f2570047d5bd2..7683d2f8153ff 100644 --- a/control/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 "mpc_lateral_controller/steering_predictor.hpp" +#include "autoware/mpc_lateral_controller/steering_predictor.hpp" namespace autoware::motion::control::mpc_lateral_controller { @@ -47,7 +47,7 @@ double SteeringPredictor::calcSteerPrediction() void SteeringPredictor::storeSteerCmd(const double steer) { const auto time_delayed = m_clock->now() + rclcpp::Duration::from_seconds(m_input_delay); - AckermannLateralCommand cmd; + Lateral cmd; cmd.stamp = time_delayed; cmd.steering_tire_angle = static_cast(steer); diff --git a/control/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 similarity index 98% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index 2467b1f0c2311..b5fa3ef1ea275 100644 --- a/control/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 "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 new file mode 100644 index 0000000000000..32d97a783627c --- /dev/null +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -0,0 +1,152 @@ +// Copyright 2018-2021 The Autoware Foundation +// +// 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/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" + +#include + +namespace autoware::motion::control::mpc_lateral_controller +{ +KinematicsBicycleModel::KinematicsBicycleModel( + const double wheelbase, const double steer_lim, const double steer_tau) +: VehicleModelInterface(/* dim_x */ 3, /* dim_u */ 1, /* dim_y */ 2, wheelbase) +{ + m_steer_lim = steer_lim; + m_steer_tau = steer_tau; +} + +void KinematicsBicycleModel::calculateDiscreteMatrix( + Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, + const double dt) +{ + auto sign = [](double x) { return (x > 0.0) - (x < 0.0); }; + + /* Linearize delta around delta_r (reference delta) */ + double delta_r = atan(m_wheelbase * m_curvature); + if (std::abs(delta_r) >= m_steer_lim) { + delta_r = m_steer_lim * static_cast(sign(delta_r)); + } + double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r)); + double velocity = m_velocity; + if (std::abs(m_velocity) < 1e-04) { + velocity = 1e-04 * (m_velocity >= 0 ? 1 : -1); + } + + a_d << 0.0, velocity, 0.0, 0.0, 0.0, velocity / m_wheelbase * cos_delta_r_squared_inv, 0.0, 0.0, + -1.0 / m_steer_tau; + + b_d << 0.0, 0.0, 1.0 / m_steer_tau; + + c_d << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0; + + w_d << 0.0, + -velocity * m_curvature + + velocity / m_wheelbase * (tan(delta_r) - delta_r * cos_delta_r_squared_inv), + 0.0; + + // bilinear discretization for ZOH system + // no discretization is needed for Cd + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x); + const Eigen::MatrixXd i_dt2a_inv = (I - dt * 0.5 * a_d).inverse(); + a_d = i_dt2a_inv * (I + dt * 0.5 * a_d); + b_d = i_dt2a_inv * b_d * dt; + w_d = i_dt2a_inv * w_d * dt; +} + +void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) +{ + u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); +} + +MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate( + [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + // Calculate predicted state in world coordinate since there is modeling errors in Frenet + // Relative coordinate x = [lat_err, yaw_err, steer] + // World coordinate x = [x, y, yaw, steer] + + const auto & t = reference_trajectory; + + // create initial state in the world coordinate + Eigen::VectorXd state_w = [&]() { + Eigen::VectorXd state = Eigen::VectorXd::Zero(4); + const auto lateral_error_0 = x0(0); + const auto yaw_error_0 = x0(1); + state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x + state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y + state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw + state(3, 0) = x0(2); // steering + return state; + }(); + + // update state in the world coordinate + const auto updateState = [&]( + const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, + const double dt, const double velocity) { + const auto yaw = state_w(2); + const auto steer = state_w(3); + const auto desired_steer = input(0); + + Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4); + dstate(0) = velocity * std::cos(yaw); + dstate(1) = velocity * std::sin(yaw); + dstate(2) = velocity * std::tan(steer) / m_wheelbase; + dstate(3) = -(steer - desired_steer) / m_steer_tau; + + // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation + // in Eigen. + const Eigen::VectorXd next_state = state_w + dstate * dt; + return next_state; + }; + + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_U = getDimU(); + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); + mpc_predicted_trajectory.push_back( + state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), + t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} + +MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // Relative coordinate x = [lat_err, yaw_err, steer] + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 1); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/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 similarity index 98% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index 8f4510510aca9..824dc25220482 100644 --- a/control/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 "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 new file mode 100644 index 0000000000000..78cf8ba63b59b --- /dev/null +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -0,0 +1,47 @@ +// Copyright 2018-2021 The Autoware Foundation +// +// 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/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" + +namespace autoware::motion::control::mpc_lateral_controller +{ +VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheelbase) +: m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase) +{ +} +int VehicleModelInterface::getDimX() const +{ + return m_dim_x; +} +int VehicleModelInterface::getDimU() const +{ + return m_dim_u; +} +int VehicleModelInterface::getDimY() const +{ + return m_dim_y; +} +double VehicleModelInterface::getWheelbase() const +{ + return m_wheelbase; +} +void VehicleModelInterface::setVelocity(const double velocity) +{ + m_velocity = velocity; +} +void VehicleModelInterface::setCurvature(const double curvature) +{ + m_curvature = curvature; +} +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp similarity index 98% rename from control/mpc_lateral_controller/test/test_lowpass_filter.cpp rename to control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp index c68513586847b..a642976efc497 100644 --- a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/mpc_lateral_controller/lowpass_filter.hpp" #include "gtest/gtest.h" -#include "mpc_lateral_controller/lowpass_filter.hpp" #include diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp similarity index 93% rename from control/mpc_lateral_controller/test/test_mpc.cpp rename to control/autoware_mpc_lateral_controller/test/test_mpc.cpp index ba145b5dd146c..c4a67552a6083 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp @@ -12,19 +12,19 @@ // 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 "gtest/gtest.h" -#include "mpc_lateral_controller/mpc.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" @@ -41,10 +41,10 @@ namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; @@ -206,7 +206,7 @@ TEST_F(MPCTest, InitializeAndCalculate) initializeMPC(*mpc); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -238,7 +238,7 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -265,7 +265,7 @@ TEST_F(MPCTest, OsqpCalculate) ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -293,7 +293,7 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -323,7 +323,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -354,7 +354,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -379,7 +379,7 @@ TEST_F(MPCTest, DynamicCalculate) ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -403,7 +403,7 @@ TEST_F(MPCTest, MultiSolveWithBuffer) mpc->m_input_buffer = {0.0, 0.0, 0.0}; // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -443,7 +443,7 @@ TEST_F(MPCTest, FailureCases) Pose pose_far; pose_far.position.x = pose_zero.position.x - admissible_position_error - 1.0; pose_far.position.y = pose_zero.position.y - admissible_position_error - 1.0; - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_far, default_velocity); diff --git a/control/mpc_lateral_controller/test/test_mpc_utils.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp similarity index 86% rename from control/mpc_lateral_controller/test/test_mpc_utils.cpp rename to control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp index 75a7208074b90..2f6b93139cbd2 100644 --- a/control/mpc_lateral_controller/test/test_mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc_utils.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_trajectory.hpp" +#include "autoware/mpc_lateral_controller/mpc_utils.hpp" #include "gtest/gtest.h" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include @@ -25,8 +25,8 @@ namespace { namespace MPCUtils = autoware::motion::control::mpc_lateral_controller::MPCUtils; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; TrajectoryPoint makePoint(const double x, const double y, const float vx) { diff --git a/control/autoware_operation_mode_transition_manager/CMakeLists.txt b/control/autoware_operation_mode_transition_manager/CMakeLists.txt new file mode 100644 index 0000000000000..2ebd6ec153ed2 --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_operation_mode_transition_manager) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(autoware_operation_mode_transition_manager_node SHARED + src/compatibility.cpp + src/data.cpp + src/node.cpp + src/state.cpp +) + +rclcpp_components_register_node(autoware_operation_mode_transition_manager_node + PLUGIN "autoware::operation_mode_transition_manager::OperationModeTransitionManager" + EXECUTABLE autoware_operation_mode_transition_manager_exe +) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/OperationModeTransitionManagerDebug.msg" + DEPENDENCIES builtin_interfaces +) + +# to use same package defined message +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(autoware_operation_mode_transition_manager_node + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(autoware_operation_mode_transition_manager_node "${cpp_typesupport_target}") +endif() + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_operation_mode_transition_manager/README.md b/control/autoware_operation_mode_transition_manager/README.md new file mode 100644 index 0000000000000..3a9680f42702b --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/README.md @@ -0,0 +1,138 @@ +# autoware_operation_mode_transition_manager + +## Purpose / Use cases + +This module is responsible for managing the different modes of operation for the Autoware system. The possible modes are: + +- `Autonomous`: the vehicle is fully controlled by the autonomous driving system +- `Local`: the vehicle is controlled by a physically connected control system such as a joy stick +- `Remote`: the vehicle is controlled by a remote controller +- `Stop`: the vehicle is stopped and there is no active control system. + +There is also an `In Transition` state that occurs during each mode transitions. During this state, the transition to the new operator is not yet complete, and the previous operator is still responsible for controlling the system until the transition is complete. Some actions may be restricted during the `In Transition` state, such as sudden braking or steering. (This is restricted by the `vehicle_cmd_gate`). + +### Features + +- Transit mode between `Autonomous`, `Local`, `Remote` and `Stop` based on the indication command. +- Check whether the each transition is available (safe or not). +- Limit some sudden motion control in `In Transition` mode (this is done with `vehicle_cmd_gate` feature). +- Check whether the transition is completed. + +- Transition between the `Autonomous`, `Local`, `Remote`, and `Stop` modes based on the indicated command. +- Determine whether each transition is safe to execute. +- Restrict certain sudden motion controls during the `In Transition` mode (using the `vehicle_cmd_gate` feature). +- Verify that the transition is complete. + +## Design + +A rough design of the relationship between `autoware_operation_mode_transition_manager`` and the other nodes is shown below. + +![transition_rough_structure](image/transition_rough_structure.drawio.svg) + +A more detailed structure is below. + +![transition_detailed_structure](image/transition_detailed_structure.drawio.svg) + +Here we see that `autoware_operation_mode_transition_manager` has multiple state transitions as follows + +- **AUTOWARE ENABLED <---> DISABLED** + - **ENABLED**: the vehicle is controlled by Autoware. + - **DISABLED**: the vehicle is out of Autoware control, expecting the e.g. manual driving. +- **AUTOWARE ENABLED <---> AUTO/LOCAL/REMOTE/NONE** + - **AUTO**: the vehicle is controlled by Autoware, with the autonomous control command calculated by the planning/control component. + - **LOCAL**: the vehicle is controlled by Autoware, with the locally connected operator, e.g. joystick controller. + - **REMOTE**: the vehicle is controlled by Autoware, with the remotely connected operator. + - **NONE**: the vehicle is not controlled by any operator. +- **IN TRANSITION <---> COMPLETED** + - **IN TRANSITION**: the mode listed above is in the transition process, expecting the former operator to have a responsibility to confirm the transition is completed. + - **COMPLETED**: the mode transition is completed. + +## Inputs / Outputs / API + +### Inputs + +For the mode transition: + +- /system/operation_mode/change_autoware_control [`tier4_system_msgs/srv/ChangeAutowareControl`]: change operation mode to Autonomous +- /system/operation_mode/change_operation_mode [`tier4_system_msgs/srv/ChangeOperationMode`]: change operation mode + +For the transition availability/completion check: + +- /control/command/control_cmd [`autoware_control_msgs/msg/Control`]: vehicle control signal +- /localization/kinematic_state [`nav_msgs/msg/Odometry`]: ego vehicle state +- /planning/scenario_planning/trajectory [`autoware_planning_msgs/msg/Trajectory`]: planning trajectory +- /vehicle/status/control_mode [`autoware_vehicle_msgs/msg/ControlModeReport`]: vehicle control mode (autonomous/manual) +- /control/vehicle_cmd_gate/operation_mode [`autoware_adapi_v1_msgs/msg/OperationModeState`]: the operation mode in the `vehicle_cmd_gate`. (To be removed) + +For the backward compatibility (to be removed): + +- /api/autoware/get/engage [`autoware_vehicle_msgs/msg/Engage`] +- /control/current_gate_mode [`tier4_control_msgs/msg/GateMode`] +- /control/external_cmd_selector/current_selector_mode [`tier4_control_msgs/msg/ExternalCommandSelectorMode`] + +### Outputs + +- /system/operation_mode/state [`autoware_adapi_v1_msgs/msg/OperationModeState`]: to inform the current operation mode +- /control/autoware_operation_mode_transition_manager/debug_info [`autoware_operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug`]: detailed information about the operation mode transition + +- /control/gate_mode_cmd [`tier4_control_msgs/msg/GateMode`]: to change the `vehicle_cmd_gate` state to use its features (to be removed) +- /autoware/engage [`autoware_vehicle_msgs/msg/Engage`]: + +- /control/control_mode_request [`autoware_vehicle_msgs/srv/ControlModeCommand`]: to change the vehicle control mode (autonomous/manual) +- /control/external_cmd_selector/select_external_command [`tier4_control_msgs/srv/ExternalCommandSelect`]: + +## Parameters + +{{ json_to_markdown("control/autoware_operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json") }} + +| Name | Type | Description | Default value | +| :--------------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | +| `frequency_hz` | `double` | running hz | 10.0 | +| `enable_engage_on_driving` | `bool` | Set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted. | 0.1 | +| `check_engage_condition` | `bool` | If false, autonomous transition is always available | 0.1 | +| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 | +| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 | + +For `engage_acceptable_limits` related parameters: + +| Name | Type | Description | Default value | +| :---------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `allow_autonomous_in_stopped` | `bool` | If true, autonomous transition is available when the vehicle is stopped even if other checks fail. | true | +| `dist_threshold` | `double` | the distance between the trajectory and ego vehicle must be within this distance for `Autonomous` transition. | 1.5 | +| `yaw_threshold` | `double` | the yaw angle between trajectory and ego vehicle must be within this threshold for `Autonomous` transition. | 0.524 | +| `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold for `Autonomous` transition. | 10.0 | +| `speed_lower_threshold` | `double` | the velocity deviation between the control command and ego vehicle must be within this threshold for `Autonomous` transition. | -10.0 | +| `acc_threshold` | `double` | the control command acceleration must be less than this threshold for `Autonomous` transition. | 1.5 | +| `lateral_acc_threshold` | `double` | the control command lateral acceleration must be less than this threshold for `Autonomous` transition. | 1.0 | +| `lateral_acc_diff_threshold` | `double` | the lateral acceleration deviation between the control command must be less than this threshold for `Autonomous` transition. | 0.5 | + +For `stable_check` related parameters: + +| Name | Type | Description | Default value | +| :---------------------- | :------- | :-------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `duration` | `double` | the stable condition must be satisfied for this duration to complete the transition. | 0.1 | +| `dist_threshold` | `double` | the distance between the trajectory and ego vehicle must be within this distance to complete `Autonomous` transition. | 1.5 | +| `yaw_threshold` | `double` | the yaw angle between trajectory and ego vehicle must be within this threshold to complete `Autonomous` transition. | 0.262 | +| `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | +| `speed_lower_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | + +## Engage check behavior on each parameter setting + +This matrix describes the scenarios in which the vehicle can be engaged based on the combinations of parameter settings: + +| `enable_engage_on_driving` | `check_engage_condition` | `allow_autonomous_in_stopped` | Scenarios where engage is permitted | +| :------------------------: | :----------------------: | :---------------------------: | :---------------------------------------------------------------- | +| x | x | x | Only when the vehicle is stationary. | +| x | x | o | Only when the vehicle is stationary. | +| x | o | x | When the vehicle is stationary and all engage conditions are met. | +| x | o | o | Only when the vehicle is stationary. | +| o | x | x | At any time (Caution: Not recommended). | +| o | x | o | At any time (Caution: Not recommended). | +| o | o | x | When all engage conditions are met, regardless of vehicle status. | +| o | o | o | When all engage conditions are met or the vehicle is stationary. | + +## Future extensions / Unimplemented parts + +- Need to remove backward compatibility interfaces. +- This node should be merged to the `vehicle_cmd_gate` due to its strong connection. diff --git a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/control/autoware_operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml similarity index 100% rename from control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml rename to control/autoware_operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml diff --git a/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg b/control/autoware_operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg similarity index 100% rename from control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg rename to control/autoware_operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg diff --git a/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg b/control/autoware_operation_mode_transition_manager/image/transition_rough_structure.drawio.svg similarity index 100% rename from control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg rename to control/autoware_operation_mode_transition_manager/image/transition_rough_structure.drawio.svg diff --git a/control/autoware_operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml b/control/autoware_operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml new file mode 100644 index 0000000000000..700228b8afbc3 --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg b/control/autoware_operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg similarity index 100% rename from control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg rename to control/autoware_operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg diff --git a/control/autoware_operation_mode_transition_manager/package.xml b/control/autoware_operation_mode_transition_manager/package.xml new file mode 100644 index 0000000000000..de514e70a0f2e --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/package.xml @@ -0,0 +1,40 @@ + + autoware_operation_mode_transition_manager + 0.1.0 + The operation_mode_transition_manager package + Takamasa Horibe + Tomoya Kimura + Takayuki Murooka + Apache License 2.0 + + Takamasa Horibe + + autoware_cmake + 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 + rclcpp + rclcpp_components + std_srvs + tier4_control_msgs + tier4_system_msgs + tier4_vehicle_msgs + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json b/control/autoware_operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json similarity index 100% rename from control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json rename to control/autoware_operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json diff --git a/control/operation_mode_transition_manager/src/compatibility.cpp b/control/autoware_operation_mode_transition_manager/src/compatibility.cpp similarity index 97% rename from control/operation_mode_transition_manager/src/compatibility.cpp rename to control/autoware_operation_mode_transition_manager/src/compatibility.cpp index 66a5fbd6b1b13..df22581dd361e 100644 --- a/control/operation_mode_transition_manager/src/compatibility.cpp +++ b/control/autoware_operation_mode_transition_manager/src/compatibility.cpp @@ -16,7 +16,7 @@ #include -namespace operation_mode_transition_manager +namespace autoware::operation_mode_transition_manager { Compatibility::Compatibility(rclcpp::Node * node) : node_(node) @@ -140,4 +140,4 @@ void Compatibility::set_mode(const OperationMode mode) } } -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager diff --git a/control/operation_mode_transition_manager/src/compatibility.hpp b/control/autoware_operation_mode_transition_manager/src/compatibility.hpp similarity index 90% rename from control/operation_mode_transition_manager/src/compatibility.hpp rename to control/autoware_operation_mode_transition_manager/src/compatibility.hpp index a69b15e69960c..34d4c7a24dd30 100644 --- a/control/operation_mode_transition_manager/src/compatibility.hpp +++ b/control/autoware_operation_mode_transition_manager/src/compatibility.hpp @@ -17,12 +17,12 @@ #include "data.hpp" -#include +#include #include #include #include -namespace operation_mode_transition_manager +namespace autoware::operation_mode_transition_manager { class Compatibility @@ -33,7 +33,7 @@ class Compatibility std::optional get_mode() const; private: - using AutowareEngage = autoware_auto_vehicle_msgs::msg::Engage; + using AutowareEngage = autoware_vehicle_msgs::msg::Engage; using GateMode = tier4_control_msgs::msg::GateMode; using SelectorModeMsg = tier4_control_msgs::msg::ExternalCommandSelectorMode; using SelectorModeSrv = tier4_control_msgs::srv::ExternalCommandSelect; @@ -54,6 +54,6 @@ class Compatibility SelectorModeMsg::ConstSharedPtr selector_mode_; }; -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager #endif // COMPATIBILITY_HPP_ diff --git a/control/operation_mode_transition_manager/src/data.cpp b/control/autoware_operation_mode_transition_manager/src/data.cpp similarity index 94% rename from control/operation_mode_transition_manager/src/data.cpp rename to control/autoware_operation_mode_transition_manager/src/data.cpp index 8734b6d1c8e86..1fac496f3c71c 100644 --- a/control/operation_mode_transition_manager/src/data.cpp +++ b/control/autoware_operation_mode_transition_manager/src/data.cpp @@ -14,7 +14,7 @@ #include "data.hpp" -namespace operation_mode_transition_manager +namespace autoware::operation_mode_transition_manager { std::string toString(const std::optional mode) @@ -67,4 +67,4 @@ OperationModeValue toMsg(const OperationMode mode) return OperationModeState::UNKNOWN; } -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager diff --git a/control/operation_mode_transition_manager/src/data.hpp b/control/autoware_operation_mode_transition_manager/src/data.hpp similarity index 86% rename from control/operation_mode_transition_manager/src/data.hpp rename to control/autoware_operation_mode_transition_manager/src/data.hpp index 9b822936a0252..63ccc90b3af85 100644 --- a/control/operation_mode_transition_manager/src/data.hpp +++ b/control/autoware_operation_mode_transition_manager/src/data.hpp @@ -19,23 +19,23 @@ #include #include -#include -#include +#include +#include #include #include #include #include -namespace operation_mode_transition_manager +namespace autoware::operation_mode_transition_manager { using ServiceResponse = autoware_adapi_v1_msgs::srv::ChangeOperationMode::Response; using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; using OperationModeValue = OperationModeState::_mode_type; using ChangeOperationMode = tier4_system_msgs::srv::ChangeOperationMode; -using ControlModeCommand = autoware_auto_vehicle_msgs::srv::ControlModeCommand; -using ControlModeReport = autoware_auto_vehicle_msgs::msg::ControlModeReport; +using ControlModeCommand = autoware_vehicle_msgs::srv::ControlModeCommand; +using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport; enum class OperationMode { STOP, AUTONOMOUS, LOCAL, REMOTE }; @@ -81,6 +81,6 @@ std::string toString(const std::optional mode); std::optional toEnum(const ChangeOperationMode::Request & request); OperationModeValue toMsg(const OperationMode mode); -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager #endif // DATA_HPP_ diff --git a/control/autoware_operation_mode_transition_manager/src/node.cpp b/control/autoware_operation_mode_transition_manager/src/node.cpp new file mode 100644 index 0000000000000..a93c0b93741a1 --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/src/node.cpp @@ -0,0 +1,296 @@ +// Copyright 2022 Autoware Foundation +// +// 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 "node.hpp" + +#include + +namespace autoware::operation_mode_transition_manager +{ + +OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::NodeOptions & options) +: Node("autoware_operation_mode_transition_manager", options), compatibility_(this) +{ + cli_control_mode_ = create_client("control_mode_request"); + pub_debug_info_ = create_publisher("~/debug_info", 1); + + // component interface + { + const auto node = component_interface_utils::NodeAdaptor(this); + node.init_srv( + srv_autoware_control_, this, &OperationModeTransitionManager::onChangeAutowareControl); + node.init_srv( + srv_operation_mode_, this, &OperationModeTransitionManager::onChangeOperationMode); + node.init_pub(pub_operation_mode_); + } + + // timer + { + const auto period_ns = rclcpp::Rate(declare_parameter("frequency_hz")).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&OperationModeTransitionManager::onTimer, this)); + } + + // initialize state + current_mode_ = OperationMode::STOP; + transition_ = nullptr; + gate_operation_mode_.mode = OperationModeState::UNKNOWN; + gate_operation_mode_.is_in_transition = false; + control_mode_report_.mode = ControlModeReport::NO_COMMAND; + transition_timeout_ = declare_parameter("transition_timeout"); + { + // check `transition_timeout` value + const auto stable_duration = declare_parameter("stable_check.duration"); + const double TIMEOUT_MARGIN = 0.5; + if (transition_timeout_ < stable_duration + TIMEOUT_MARGIN) { + transition_timeout_ = stable_duration + TIMEOUT_MARGIN; + RCLCPP_WARN( + get_logger(), "`transition_timeout` must be somewhat larger than `stable_check.duration`"); + RCLCPP_WARN_STREAM(get_logger(), "transition_timeout is set to " << transition_timeout_); + } + } + + // modes + modes_[OperationMode::STOP] = std::make_unique(); + modes_[OperationMode::AUTONOMOUS] = std::make_unique(this); + modes_[OperationMode::LOCAL] = std::make_unique(); + modes_[OperationMode::REMOTE] = std::make_unique(); +} + +void OperationModeTransitionManager::onChangeAutowareControl( + const ChangeAutowareControlAPI::Service::Request::SharedPtr request, + const ChangeAutowareControlAPI::Service::Response::SharedPtr response) +{ + if (request->autoware_control) { + // Treat as a transition to the current operation mode. + changeOperationMode(std::nullopt); + } else { + // Allow mode transition to complete without canceling. + compatibility_transition_ = std::nullopt; + transition_.reset(); + changeControlMode(ControlModeCommand::Request::MANUAL); + } + response->status.success = true; +} + +void OperationModeTransitionManager::onChangeOperationMode( + const ChangeOperationModeAPI::Service::Request::SharedPtr request, + const ChangeOperationModeAPI::Service::Response::SharedPtr response) +{ + const auto mode = toEnum(*request); + if (!mode) { + throw component_interface_utils::ParameterError("The operation mode is invalid."); + } + changeOperationMode(mode.value()); + response->status.success = true; +} + +void OperationModeTransitionManager::changeControlMode(ControlModeCommandType mode) +{ + const auto callback = [this](rclcpp::Client::SharedFuture future) { + if (!future.get()->success) { + RCLCPP_WARN(get_logger(), "Autonomous mode change was rejected."); + if (transition_) { + transition_->is_engage_failed = true; + } + } + }; + + const auto request = std::make_shared(); + request->stamp = now(); + request->mode = mode; + cli_control_mode_->async_send_request(request, callback); +} + +void OperationModeTransitionManager::changeOperationMode(std::optional request_mode) +{ + // NOTE: If request_mode is nullopt, indicates to enable autoware control + + const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; + const bool request_control = request_mode ? false : true; + + if (current_mode_ == request_mode) { + throw component_interface_utils::NoEffectWarning("The mode is the same as the current."); + } + + if (current_control && request_control) { + throw component_interface_utils::NoEffectWarning("Autoware already controls the vehicle."); + } + + // TODO(Takagi, Isamu): Consider mode change request during transition. + if (transition_ && request_mode != OperationMode::STOP) { + throw component_interface_utils::ServiceException( + ServiceResponse::ERROR_IN_TRANSITION, "The mode transition is in progress."); + } + + // Enter transition mode if the vehicle is being or will be controlled by Autoware. + if (current_control || request_control) { + if (!available_mode_change_[request_mode.value_or(current_mode_)]) { + throw component_interface_utils::ServiceException( + ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change condition is not satisfied."); + } + if (request_control) { + transition_ = std::make_unique(now(), request_control, std::nullopt); + } else { + transition_ = std::make_unique(now(), request_control, current_mode_); + } + } + compatibility_transition_ = now(); + current_mode_ = request_mode.value_or(current_mode_); +} + +void OperationModeTransitionManager::cancelTransition() +{ + const auto & previous = transition_->previous; + if (previous) { + compatibility_transition_ = now(); + current_mode_ = previous.value(); + } else { + compatibility_transition_ = std::nullopt; + changeControlMode(ControlModeCommand::Request::MANUAL); + } + transition_.reset(); +} + +void OperationModeTransitionManager::processTransition() +{ + const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; + + // Check timeout. + if (transition_timeout_ < (now() - transition_->time).seconds()) { + return cancelTransition(); + } + + // Check engage failure. + if (transition_->is_engage_failed) { + return cancelTransition(); + } + + // Check override. + if (current_control) { + transition_->is_engage_completed = true; + } else { + if (transition_->is_engage_completed) { + return cancelTransition(); + } + } + + // Check reflection of mode change to the compatible interface. + if (current_mode_ != compatibility_.get_mode()) { + return; + } + + // Check completion when engaged, otherwise engage after the gate reflects transition. + if (current_control) { + if (modes_.at(current_mode_)->isModeChangeCompleted()) { + return transition_.reset(); + } + } else { + if (transition_->is_engage_requested && gate_operation_mode_.is_in_transition) { + transition_->is_engage_requested = false; + return changeControlMode(ControlModeCommand::Request::AUTONOMOUS); + } + } +} + +void OperationModeTransitionManager::onTimer() +{ + const auto control_mode_report_ptr = sub_control_mode_report_.takeData(); + if (!control_mode_report_ptr) { + return; + } + const auto gate_operation_mode_ptr = sub_gate_operation_mode_.takeData(); + if (!gate_operation_mode_ptr) { + return; + } + control_mode_report_ = *control_mode_report_ptr; + gate_operation_mode_ = *gate_operation_mode_ptr; + + for (const auto & [type, mode] : modes_) { + mode->update(current_mode_ == type && transition_); + } + + for (const auto & [type, mode] : modes_) { + available_mode_change_[type] = mode->isModeChangeAvailable(); + } + + // Check sync timeout to the compatible interface. + if (compatibility_transition_) { + if (compatibility_timeout_ < (now() - compatibility_transition_.value()).seconds()) { + compatibility_transition_ = std::nullopt; + } + } + + // Reflects the mode when changed by the compatible interface. + if (compatibility_transition_) { + compatibility_.set_mode(current_mode_); + } else { + current_mode_ = compatibility_.get_mode().value_or(current_mode_); + } + + // Reset sync timeout when it is completed. + if (compatibility_transition_) { + if (current_mode_ == compatibility_.get_mode()) { + compatibility_transition_ = std::nullopt; + } + } + + if (transition_) { + processTransition(); + } + + publishData(); +} + +void OperationModeTransitionManager::publishData() +{ + const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; + const auto time = now(); + + OperationModeStateAPI::Message state; + state.mode = toMsg(current_mode_); + state.is_autoware_control_enabled = current_control; + state.is_in_transition = transition_ ? true : false; + state.is_stop_mode_available = available_mode_change_[OperationMode::STOP]; + state.is_autonomous_mode_available = available_mode_change_[OperationMode::AUTONOMOUS]; + state.is_local_mode_available = available_mode_change_[OperationMode::LOCAL]; + state.is_remote_mode_available = available_mode_change_[OperationMode::REMOTE]; + + if (prev_state_ != state) { + prev_state_ = state; + state.stamp = time; + pub_operation_mode_->publish(state); + } + + const auto status_str = [&]() { + if (!current_control) return "DISENGAGE (autoware mode = " + toString(current_mode_) + ")"; + if (transition_) + return toString(current_mode_) + " (in transition from " + toString(transition_->previous) + + ")"; + return toString(current_mode_); + }(); + + ModeChangeBase::DebugInfo debug_info = modes_.at(OperationMode::AUTONOMOUS)->getDebugInfo(); + debug_info.stamp = time; + debug_info.status = status_str; + debug_info.in_autoware_control = current_control; + debug_info.in_transition = transition_ ? true : false; + pub_debug_info_->publish(debug_info); +} + +} // namespace autoware::operation_mode_transition_manager + +#include +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::operation_mode_transition_manager::OperationModeTransitionManager) diff --git a/control/autoware_operation_mode_transition_manager/src/node.hpp b/control/autoware_operation_mode_transition_manager/src/node.hpp new file mode 100644 index 0000000000000..d69d79f9640d8 --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/src/node.hpp @@ -0,0 +1,83 @@ +// Copyright 2022 Autoware Foundation +// +// 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 NODE_HPP_ +#define NODE_HPP_ + +#include "compatibility.hpp" +#include "state.hpp" + +#include +#include +#include +#include + +#include +#include + +namespace autoware::operation_mode_transition_manager +{ + +class OperationModeTransitionManager : public rclcpp::Node +{ +public: + explicit OperationModeTransitionManager(const rclcpp::NodeOptions & options); + +private: + using ChangeAutowareControlAPI = system_interface::ChangeAutowareControl; + using ChangeOperationModeAPI = system_interface::ChangeOperationMode; + using OperationModeStateAPI = system_interface::OperationModeState; + component_interface_utils::Service::SharedPtr srv_autoware_control_; + component_interface_utils::Service::SharedPtr srv_operation_mode_; + component_interface_utils::Publisher::SharedPtr pub_operation_mode_; + void onChangeAutowareControl( + const ChangeAutowareControlAPI::Service::Request::SharedPtr request, + const ChangeAutowareControlAPI::Service::Response::SharedPtr response); + void onChangeOperationMode( + const ChangeOperationModeAPI::Service::Request::SharedPtr request, + const ChangeOperationModeAPI::Service::Response::SharedPtr response); + + using ControlModeCommandType = ControlModeCommand::Request::_mode_type; + 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_; + void onTimer(); + void publishData(); + void changeControlMode(ControlModeCommandType mode); + void changeOperationMode(std::optional mode); + void cancelTransition(); + void processTransition(); + + double transition_timeout_; + OperationMode current_mode_; + std::unique_ptr transition_; + std::unordered_map> modes_; + std::unordered_map available_mode_change_; + OperationModeState gate_operation_mode_; + ControlModeReport control_mode_report_; + + std::optional prev_state_; + + static constexpr double compatibility_timeout_ = 1.0; + Compatibility compatibility_; + std::optional compatibility_transition_; +}; + +} // namespace autoware::operation_mode_transition_manager + +#endif // NODE_HPP_ diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/autoware_operation_mode_transition_manager/src/state.cpp similarity index 90% rename from control/operation_mode_transition_manager/src/state.cpp rename to control/autoware_operation_mode_transition_manager/src/state.cpp index d57cb8c78c740..31cd32e6311a9 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/autoware_operation_mode_transition_manager/src/state.cpp @@ -16,32 +16,30 @@ #include "util.hpp" -#include -#include -#include +#include +#include +#include #include #include -namespace operation_mode_transition_manager +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()) { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); - sub_control_cmd_ = node->create_subscription( - "control_cmd", 1, - [this](const AckermannControlCommand::SharedPtr msg) { control_cmd_ = *msg; }); - sub_trajectory_follower_control_cmd_ = node->create_subscription( - "trajectory_follower_control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { - trajectory_follower_control_cmd_ = *msg; - }); + sub_control_cmd_ = node->create_subscription( + "control_cmd", 1, [this](const Control::SharedPtr msg) { control_cmd_ = *msg; }); + sub_trajectory_follower_control_cmd_ = node->create_subscription( + "trajectory_follower_control_cmd", 1, + [this](const Control::SharedPtr msg) { trajectory_follower_control_cmd_ = *msg; }); sub_kinematics_ = node->create_subscription( "kinematics", 1, [this](const Odometry::SharedPtr msg) { kinematics_ = *msg; }); @@ -128,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(); @@ -140,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(); @@ -220,7 +218,7 @@ bool AutonomousMode::isModeChangeAvailable() } const auto current_speed = kinematics_.twist.twist.linear.x; - const auto target_control_speed = control_cmd_.longitudinal.speed; + const auto target_control_speed = control_cmd_.longitudinal.velocity; const auto & param = engage_acceptable_param_; if (!enable_engage_on_driving_ && std::fabs(current_speed) > 1.0e-2) { @@ -267,7 +265,7 @@ bool AutonomousMode::isModeChangeAvailable() // No engagement if the vehicle is moving but the target speed is zero. const bool is_stop_cmd_indicated = std::abs(target_control_speed) < 0.01 || - std::abs(trajectory_follower_control_cmd_.longitudinal.speed) < 0.01; + std::abs(trajectory_follower_control_cmd_.longitudinal.velocity) < 0.01; const bool stop_ok = !(std::abs(current_speed) > 0.1 && is_stop_cmd_indicated); // No engagement if the large acceleration is commanded. @@ -318,4 +316,4 @@ bool AutonomousMode::isModeChangeAvailable() return is_all_ok; } -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/autoware_operation_mode_transition_manager/src/state.hpp similarity index 75% rename from control/operation_mode_transition_manager/src/state.hpp rename to control/autoware_operation_mode_transition_manager/src/state.hpp index e10d64e367f8d..a8f20ebf7369a 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/autoware_operation_mode_transition_manager/src/state.hpp @@ -15,20 +15,20 @@ #ifndef STATE_HPP_ #define STATE_HPP_ +#include "autoware_operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" #include "data.hpp" -#include "operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" +#include #include -#include -#include -#include +#include +#include #include #include #include -namespace operation_mode_transition_manager +namespace autoware::operation_mode_transition_manager { class ModeChangeBase @@ -39,7 +39,8 @@ class ModeChangeBase virtual bool isModeChangeCompleted() = 0; virtual bool isModeChangeAvailable() = 0; - using DebugInfo = operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; + using DebugInfo = + autoware_operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; virtual DebugInfo getDebugInfo() { return DebugInfo{}; } }; @@ -63,11 +64,11 @@ class AutonomousMode : public ModeChangeBase bool hasDangerAcceleration(); std::pair hasDangerLateralAcceleration(); - using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; + using Control = autoware_control_msgs::msg::Control; using Odometry = nav_msgs::msg::Odometry; - using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; - rclcpp::Subscription::SharedPtr sub_control_cmd_; - rclcpp::Subscription::SharedPtr sub_trajectory_follower_control_cmd_; + using Trajectory = autoware_planning_msgs::msg::Trajectory; + rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_trajectory_follower_control_cmd_; rclcpp::Subscription::SharedPtr sub_kinematics_; rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Logger logger_; @@ -79,11 +80,11 @@ class AutonomousMode : public ModeChangeBase double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index EngageAcceptableParam engage_acceptable_param_; StableCheckParam stable_check_param_; - AckermannControlCommand control_cmd_; - AckermannControlCommand trajectory_follower_control_cmd_; + Control control_cmd_; + Control trajectory_follower_control_cmd_; Odometry kinematics_; Trajectory trajectory_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; DebugInfo debug_info_; std::shared_ptr stable_start_time_; // Reset every transition start. @@ -105,6 +106,6 @@ class RemoteMode : public ModeChangeBase bool isModeChangeAvailable() override { return true; } }; -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager #endif // STATE_HPP_ diff --git a/control/autoware_operation_mode_transition_manager/src/util.hpp b/control/autoware_operation_mode_transition_manager/src/util.hpp new file mode 100644 index 0000000000000..2e7e97a9e90d7 --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/src/util.hpp @@ -0,0 +1,38 @@ +// Copyright 2022 Autoware Foundation +// +// 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 UTIL_HPP_ +#define UTIL_HPP_ + +#include "autoware_operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" + +using DebugInfo = + autoware_operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; + +void setAllOk(DebugInfo & debug_info) +{ + debug_info.is_all_ok = true; + debug_info.engage_allowed_for_stopped_vehicle = true; + debug_info.large_acceleration_ok = true; + debug_info.large_lateral_acceleration_diff_ok = true; + debug_info.large_lateral_acceleration_ok = true; + debug_info.lateral_deviation_ok = true; + debug_info.speed_lower_deviation_ok = true; + debug_info.speed_upper_deviation_ok = true; + debug_info.stop_ok = true; + debug_info.trajectory_available_ok = true; + debug_info.yaw_deviation_ok = true; +} + +#endif // UTIL_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/CMakeLists.txt b/control/autoware_pid_longitudinal_controller/CMakeLists.txt new file mode 100644 index 0000000000000..f0dce81eb54e1 --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_pid_longitudinal_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(PID_LON_CON_LIB ${PROJECT_NAME}_lib) +ament_auto_add_library(${PID_LON_CON_LIB} SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + set(TEST_LON_SOURCES + test/test_pid.cpp + test/test_smooth_stop.cpp + test/test_longitudinal_controller_utils.cpp + ) + set(TEST_LONGITUDINAL_CONTROLLER_EXE test_longitudinal_controller) + ament_add_ros_isolated_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES}) + target_link_libraries(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${PID_LON_CON_LIB}) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param +) diff --git a/control/autoware_pid_longitudinal_controller/README.md b/control/autoware_pid_longitudinal_controller/README.md new file mode 100644 index 0000000000000..b147b3b795391 --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/README.md @@ -0,0 +1,266 @@ +# PID Longitudinal Controller + +## Purpose / Use cases + +The longitudinal_controller computes the target acceleration to achieve the target velocity set at each point of the target trajectory using a feed-forward/back control. + +It also contains a slope force correction that takes into account road slope information, and a delay compensation function. +It is assumed that the target acceleration calculated here will be properly realized by the vehicle interface. + +Note that the use of this module is not mandatory for Autoware if the vehicle supports the "target speed" interface. + +## Design / Inner-workings / Algorithms + +### States + +This module has four state transitions as shown below in order to handle special processing in a specific situation. + +- **DRIVE** + - Executes target velocity tracking by PID control. + - It also applies the delay compensation and slope compensation. +- **STOPPING** + - Controls the motion just before stopping. + - Special sequence is performed to achieve accurate and smooth stopping. +- **STOPPED** + - Performs operations in the stopped state (e.g. brake hold) +- **EMERGENCY**. + - Enters an emergency state when certain conditions are met (e.g., when the vehicle has crossed a certain distance of a stop line). + - The recovery condition (whether or not to keep emergency state until the vehicle completely stops) or the deceleration in the emergency state are defined by parameters. + +The state transition diagram is shown below. + +![LongitudinalControllerStateTransition](./media/LongitudinalControllerStateTransition.drawio.svg) + +### Logics + +#### Control Block Diagram + +![LongitudinalControllerDiagram](./media/LongitudinalControllerDiagram.drawio.svg) + +#### FeedForward (FF) + +The reference acceleration set in the trajectory and slope compensation terms are output as a feedforward. Under ideal conditions with no modeling error, this FF term alone should be sufficient for velocity tracking. + +Tracking errors causing modeling or discretization errors are removed by the feedback control (now using PID). + +##### Brake keeping + +From the viewpoint of ride comfort, stopping with 0 acceleration is important because it reduces the impact of braking. However, if the target acceleration when stopping is 0, the vehicle may cross over the stop line or accelerate a little in front of the stop line due to vehicle model error or gradient estimation error. + +For reliable stopping, the target acceleration calculated by the FeedForward system is limited to a negative acceleration when stopping. + +![BrakeKeepingDiagram](./media/BrakeKeeping.drawio.svg) + +#### Slope compensation + +Based on the slope information, a compensation term is added to the target acceleration. + +There are two sources of the slope information, which can be switched by a parameter. + +- Pitch of the estimated ego-pose (default) + - Calculates the current slope from the pitch angle of the estimated ego-pose + - Pros: Easily available + - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. +- Z coordinate on the trajectory + - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory + - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained + - Pros: Can be used in combination with delay compensation (not yet implemented) + - Cons: z-coordinates of high-precision map is needed. + - Cons: Does not support free space planning (for now) + +**Notation:** This function works correctly only in a vehicle system that does not have acceleration feedback in the low-level control system. + +This compensation adds gravity correction to the target acceleration, resulting in an output value that is no longer equal to the target acceleration that the autonomous driving system desires. Therefore, it conflicts with the role of the acceleration feedback in the low-level controller. +For instance, if the vehicle is attempting to start with an acceleration of `1.0 m/s^2` and a gravity correction of `-1.0 m/s^2` is applied, the output value will be `0`. If this output value is mistakenly treated as the target acceleration, the vehicle will not start. + +A suitable example of a vehicle system for the slope compensation function is one in which the output acceleration from the longitudinal_controller is converted into target accel/brake pedal input without any feedbacks. In this case, the output acceleration is just used as a feedforward term to calculate the target pedal, and hence the issue mentioned above does not arise. + +Note: The angle of the slope is defined as positive for an uphill slope, while the pitch angle of the ego pose is defined as negative when facing upward. They have an opposite definition. + +![slope_definition](./media/slope_definition.drawio.svg) + +#### PID control + +For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system. + +This PID control calculates the target acceleration from the deviation between the current ego-velocity and the target velocity. + +This PID logic has a maximum value for the output of each term. This is to prevent the following: + +- Large integral terms may cause unintended behavior by users. +- Unintended noise may cause the output of the derivative term to be very large. + +Note: by default, the integral term in the control system is not accumulated when the vehicle is stationary. This precautionary measure aims to prevent unintended accumulation of the integral term in scenarios where Autoware assumes the vehicle is engaged, but an external system has immobilized the vehicle to initiate startup procedures. + +However, certain situations may arise, such as when the vehicle encounters a depression in the road surface during startup or if the slope compensation is inaccurately estimated (lower than necessary), leading to a failure to initiate motion. To address these scenarios, it is possible to activate error integration even when the vehicle is at rest by setting the `enable_integration_at_low_speed` parameter to true. + +When `enable_integration_at_low_speed` is set to true, the PID controller will initiate integration of the acceleration error after a specified duration defined by the `time_threshold_before_pid_integration` parameter has elapsed without the vehicle surpassing a minimum velocity set by the `current_vel_threshold_pid_integration` parameter. + +The presence of the `time_threshold_before_pid_integration` parameter is important for practical PID tuning. Integrating the error when the vehicle is stationary or at low speed can complicate PID tuning. This parameter effectively introduces a delay before the integral part becomes active, preventing it from kicking in immediately. This delay allows for more controlled and effective tuning of the PID controller. + +At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance. +This may be replaced by a higher performance controller (adaptive control or robust control) in future development. + +#### Time delay compensation + +At high speeds, the delay of actuator systems such as gas pedals and brakes has a significant impact on driving accuracy. +Depending on the actuating principle of the vehicle, the mechanism that physically controls the gas pedal and brake typically has a delay of about a hundred millisecond. + +In this controller, the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem. + +### Slope compensation + +Based on the slope information, a compensation term is added to the target acceleration. + +There are two sources of the slope information, which can be switched by a parameter. + +- Pitch of the estimated ego-pose (default) + - Calculates the current slope from the pitch angle of the estimated ego-pose + - Pros: Easily available + - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. +- Z coordinate on the trajectory + - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory + - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained + - Pros: Can be used in combination with delay compensation (not yet implemented) + - Cons: z-coordinates of high-precision map is needed. + - Cons: Does not support free space planning (for now) + +## Assumptions / Known limits + +1. Smoothed target velocity and its acceleration shall be set in the trajectory + 1. The velocity command is not smoothed inside the controller (only noise may be removed). + 2. For step-like target signal, tracking is performed as fast as possible. +2. The vehicle velocity must be an appropriate value + 1. The ego-velocity must be a signed-value corresponding to the forward/backward direction + 2. The ego-velocity should be given with appropriate noise processing. + 3. If there is a large amount of noise in the ego-velocity, the tracking performance will be significantly reduced. +3. The output of this controller must be achieved by later modules (e.g. vehicle interface). + 1. If the vehicle interface does not have the target velocity or acceleration interface (e.g., the vehicle only has a gas pedal and brake interface), an appropriate conversion must be done after this controller. + +## Inputs / Outputs / API + +### Input + +Set the following from the [controller_node](../autoware_trajectory_follower_node/README.md) + +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry + +### Output + +Return LongitudinalOutput which contains the following to the controller node + +- `autoware_control_msgs/Longitudinal`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. +- LongitudinalSyncData + - velocity convergence(currently not used) + +### PIDController class + +The `PIDController` class is straightforward to use. +First, gains and limits must be set (using `setGains()` and `setLimits()`) for the proportional (P), integral (I), and derivative (D) components. +Then, the velocity can be calculated by providing the current error and time step duration to the `calculate()` function. + +## Parameter description + +The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the +AutonomouStuff Lexus RX 450h for under 40 km/h driving. + +| Name | Type | Description | Default value | +| :------------------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| delay_compensation_time | double | delay for longitudinal control [s] | 0.17 | +| enable_smooth_stop | bool | flag to enable transition to STOPPING | true | +| enable_overshoot_emergency | bool | flag to enable transition to EMERGENCY when the ego is over the stop line with a certain distance. See `emergency_state_overshoot_stop_dist`. | true | +| enable_large_tracking_error_emergency | bool | flag to enable transition to EMERGENCY when the closest trajectory point search is failed due to a large deviation between trajectory and ego pose. | true | +| enable_slope_compensation | bool | flag to modify output acceleration for slope compensation. The source of the slope angle can be selected from ego-pose or trajectory angle. See `use_trajectory_for_pitch_calculation`. | true | +| enable_brake_keeping_before_stop | bool | flag to keep a certain acceleration during DRIVE state before the ego stops. See [Brake keeping](#brake-keeping). | false | +| enable_keep_stopped_until_steer_convergence | bool | flag to keep stopped condition until until the steer converges. | true | +| max_acc | double | max value of output acceleration [m/s^2] | 3.0 | +| min_acc | double | min value of output acceleration [m/s^2] | -5.0 | +| max_jerk | double | max value of jerk of output acceleration [m/s^3] | 2.0 | +| min_jerk | double | min value of jerk of output acceleration [m/s^3] | -5.0 | +| use_trajectory_for_pitch_calculation | bool | If true, the slope is estimated from trajectory z-level. Otherwise the pitch angle of the ego pose is used. | false | +| lpf_pitch_gain | double | gain of low-pass filter for pitch estimation | 0.95 | +| max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 | +| min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 | + +### State transition + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| drive_state_stop_dist | double | The state will transit to DRIVE when the distance to the stop point is longer than `drive_state_stop_dist` + `drive_state_offset_stop_dist` [m] | 0.5 | +| drive_state_offset_stop_dist | double | The state will transit to DRIVE when the distance to the stop point is longer than `drive_state_stop_dist` + `drive_state_offset_stop_dist` [m] | 1.0 | +| stopping_state_stop_dist | double | The state will transit to STOPPING when the distance to the stop point is shorter than `stopping_state_stop_dist` [m] | 0.5 | +| stopped_state_entry_vel | double | threshold of the ego velocity in transition to the STOPPED state [m/s] | 0.01 | +| stopped_state_entry_acc | double | threshold of the ego acceleration in transition to the STOPPED state [m/s^2] | 0.1 | +| emergency_state_overshoot_stop_dist | double | If `enable_overshoot_emergency` is true and the ego is `emergency_state_overshoot_stop_dist`-meter ahead of the stop point, the state will transit to EMERGENCY. [m] | 1.5 | +| emergency_state_traj_trans_dev | double | If the ego's position is `emergency_state_traj_tran_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m] | 3.0 | +| emergency_state_traj_rot_dev | double | If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad] | 0.784 | + +### DRIVE Parameter + +| Name | Type | Description | Default value | +| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| kp | double | p gain for longitudinal control | 1.0 | +| ki | double | i gain for longitudinal control | 0.1 | +| kd | double | d gain for longitudinal control | 0.0 | +| max_out | double | max value of PID's output acceleration during DRIVE state [m/s^2] | 1.0 | +| min_out | double | min value of PID's output acceleration during DRIVE state [m/s^2] | -1.0 | +| max_p_effort | double | max value of acceleration with p gain | 1.0 | +| min_p_effort | double | min value of acceleration with p gain | -1.0 | +| max_i_effort | double | max value of acceleration with i gain | 0.3 | +| min_i_effort | double | min value of acceleration with i gain | -0.3 | +| 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. | 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 | + +### STOPPING Parameter (smooth stop) + +Smooth stop is enabled if `enable_smooth_stop` is true. +In smooth stop, strong acceleration (`strong_acc`) will be output first to decrease the ego velocity. +Then weak acceleration (`weak_acc`) will be output to stop smoothly by decreasing the ego jerk. +If the ego does not stop in a certain time or some-meter over the stop point, weak acceleration to stop right (`weak_stop_acc`) now will be output. +If the ego is still running, strong acceleration (`strong_stop_acc`) to stop right now will be output. + +| Name | Type | Description | Default value | +| :--------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| smooth_stop_max_strong_acc | double | max strong acceleration [m/s^2] | -0.5 | +| smooth_stop_min_strong_acc | double | min strong acceleration [m/s^2] | -0.8 | +| smooth_stop_weak_acc | double | weak acceleration [m/s^2] | -0.3 | +| smooth_stop_weak_stop_acc | double | weak acceleration to stop right now [m/s^2] | -0.8 | +| smooth_stop_strong_stop_acc | double | strong acceleration to be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m/s^2] | -3.4 | +| smooth_stop_max_fast_vel | double | max fast vel to judge the ego is running fast [m/s]. If the ego is running fast, strong acceleration will be output. | 0.5 | +| smooth_stop_min_running_vel | double | min ego velocity to judge if the ego is running or not [m/s] | 0.01 | +| smooth_stop_min_running_acc | double | min ego acceleration to judge if the ego is running or not [m/s^2] | 0.01 | +| smooth_stop_weak_stop_time | double | max time to output weak acceleration [s]. After this, strong acceleration will be output. | 0.8 | +| smooth_stop_weak_stop_dist | double | Weak acceleration will be output when the ego is `smooth_stop_weak_stop_dist`-meter before the stop point. [m] | -0.3 | +| smooth_stop_strong_stop_dist | double | Strong acceleration will be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m] | -0.5 | + +### STOPPED Parameter + +The `STOPPED` state assumes that the vehicle is completely stopped with the brakes fully applied. +Therefore, `stopped_acc` should be set to a value that allows the vehicle to apply the strongest possible brake. +If `stopped_acc` is not sufficiently low, there is a possibility of sliding down on steep slopes. + +| Name | Type | Description | Default value | +| :----------- | :----- | :------------------------------------------- | :------------ | +| stopped_vel | double | target velocity in STOPPED state [m/s] | 0.0 | +| stopped_acc | double | target acceleration in STOPPED state [m/s^2] | -3.4 | +| stopped_jerk | double | target jerk in STOPPED state [m/s^3] | -5.0 | + +### EMERGENCY Parameter + +| Name | Type | Description | Default value | +| :------------- | :----- | :------------------------------------------------ | :------------ | +| emergency_vel | double | target velocity in EMERGENCY state [m/s] | 0.0 | +| emergency_acc | double | target acceleration in an EMERGENCY state [m/s^2] | -5.0 | +| emergency_jerk | double | target jerk in an EMERGENCY state [m/s^3] | -3.0 | + +## References / External links + +## Future extensions / Unimplemented parts + +## Related issues diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp similarity index 92% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp index 409af46b16ed1..e8834665de3ec 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ #include +#include namespace autoware::motion::control::pid_longitudinal_controller { @@ -92,4 +93,4 @@ class DebugValues }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp similarity index 85% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp index ac4fec8dacb7d..a36a0b4eefccd 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ -#define 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 #include #include // NOLINT -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" #include @@ -37,8 +37,8 @@ namespace autoware::motion::control::pid_longitudinal_controller namespace longitudinal_utils { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Quaternion; @@ -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); { @@ -150,9 +151,9 @@ double applyDiffLimitFilter( */ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const size_t src_idx, const double distance, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory); + const autoware_planning_msgs::msg::Trajectory & trajectory); } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // 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 new file mode 100644 index 0000000000000..081e78e2de214 --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp @@ -0,0 +1,66 @@ +// Copyright 2018-2021 The Autoware Foundation +// +// 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__PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ + +#include +#include +#include +#include + +namespace autoware::motion::control::pid_longitudinal_controller +{ + +/** + * @brief Simple filter with gain on the first derivative of the value + */ +class LowpassFilter1d +{ +private: + double m_x; //!< @brief current filtered value + double m_gain; //!< @brief gain value of first-order low-pass filter + +public: + /** + * @brief constructor with initial value and first-order gain + * @param [in] x initial value + * @param [in] gain first-order gain + */ + LowpassFilter1d(const double x, const double gain) : m_x(x), m_gain(gain) {} + + /** + * @brief set the current value of the filter + * @param [in] x new value + */ + void reset(const double x) { m_x = x; } + + /** + * @brief get the current value of the filter + */ + double getValue() const { return m_x; } + + /** + * @brief filter a new value + * @param [in] u new value + */ + double filter(const double u) + { + const double ret = m_gain * m_x + (1.0 - m_gain) * u; + m_x = ret; + return ret; + } +}; +} // namespace autoware::motion::control::pid_longitudinal_controller +#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 new file mode 100644 index 0000000000000..42138ecc1fb8c --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid.hpp @@ -0,0 +1,94 @@ +// Copyright 2018-2021 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__PID_LONGITUDINAL_CONTROLLER__PID_HPP_ +#define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__PID_HPP_ + +#include + +namespace autoware::motion::control::pid_longitudinal_controller +{ + +/// @brief implementation of a PID controller +class PIDController +{ +public: + PIDController(); + + /** + * @brief calculate the output of this PID + * @param [in] error previous error + * @param [in] dt time step [s] + * @param [in] is_integrated if true, will use the integral component for calculation + * @param [out] pid_contributions values of the proportional, integral, and derivative components + * @return PID output + * @throw std::runtime_error if gains or limits have not been set + */ + double calculate( + const double error, const double dt, const bool is_integrated, + std::vector & pid_contributions); + /** + * @brief set the coefficients for the P (proportional) I (integral) D (derivative) terms + * @param [in] kp proportional coefficient + * @param [in] ki integral coefficient + * @param [in] kd derivative coefficient + */ + void setGains(const double kp, const double ki, const double kd); + /** + * @brief set limits on the total, proportional, integral, and derivative components + * @param [in] max_ret maximum return value of this PID + * @param [in] min_ret minimum return value of this PID + * @param [in] max_ret_p maximum value of the proportional component + * @param [in] min_ret_p minimum value of the proportional component + * @param [in] max_ret_i maximum value of the integral component + * @param [in] min_ret_i minimum value of the integral component + * @param [in] max_ret_d maximum value of the derivative component + * @param [in] min_ret_d minimum value of the derivative component + */ + void setLimits( + const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, + const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d); + /** + * @brief reset this PID to its initial state + */ + void reset(); + +private: + // PID parameters + struct Params + { + double kp; + double ki; + double kd; + double max_ret_p; + double min_ret_p; + double max_ret_i; + double min_ret_i; + double max_ret_d; + double min_ret_d; + double max_ret; + double min_ret; + }; + Params m_params; + + // state variables + double m_error_integral; + double m_prev_error; + bool m_is_first_time; + bool m_is_gains_set; + bool m_is_limits_set; +}; +} // namespace autoware::motion::control::pid_longitudinal_controller + +#endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__PID_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp similarity index 89% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 967b7c40fbd10..9291d538b022f 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -12,30 +12,29 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_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 "pid_longitudinal_controller/debug_values.hpp" -#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" -#include "pid_longitudinal_controller/lowpass_filter.hpp" -#include "pid_longitudinal_controller/pid.hpp" -#include "pid_longitudinal_controller/smooth_stop.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 "trajectory_follower_base/longitudinal_controller_base.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -51,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; @@ -88,7 +87,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro struct ControlData { bool is_far_from_trajectory{false}; - autoware_auto_planning_msgs::msg::Trajectory interpolated_traj{}; + autoware_planning_msgs::msg::Trajectory interpolated_traj{}; size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx size_t target_idx{0}; StateAfterDelay state_after_delay{0.0, 0.0, 0.0}; @@ -113,7 +112,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // pointers for ros topic nav_msgs::msg::Odometry m_current_kinematic_state; geometry_msgs::msg::AccelWithCovarianceStamped m_current_accel; - autoware_auto_planning_msgs::msg::Trajectory m_trajectory; + autoware_planning_msgs::msg::Trajectory m_trajectory; OperationModeState m_current_operation_mode; // vehicle info @@ -218,7 +217,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double m_ego_nearest_yaw_threshold; // buffer of send command - std::vector m_ctrl_cmd_vec; + std::vector m_ctrl_cmd_vec; // for calculating dt std::shared_ptr m_prev_control_time{nullptr}; @@ -270,7 +269,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @brief set reference trajectory with received message * @param [in] msg trajectory message */ - void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg); + void setTrajectory(const autoware_planning_msgs::msg::Trajectory & msg); bool isReady(const trajectory_follower::InputData & input_data) override; @@ -309,7 +308,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] ctrl_cmd calculated control command to control velocity * @param [in] current_vel current velocity of the vehicle */ - autoware_auto_control_msgs::msg::LongitudinalCommand createCtrlCmdMsg( + autoware_control_msgs::msg::Longitudinal createCtrlCmdMsg( const Motion & ctrl_cmd, const double & current_vel); /** @@ -371,9 +370,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] point vehicle position * @param [in] nearest_idx index of the trajectory point nearest to the vehicle position */ - std::pair + std::pair calcInterpolatedTrajPointAndSegment( - const autoware_auto_planning_msgs::msg::Trajectory & traj, + const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const; /** @@ -409,4 +408,4 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ diff --git a/control/pid_longitudinal_controller/include/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/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp index 9a0a36123e143..e84b44d95c886 100644 --- a/control/pid_longitudinal_controller/include/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 PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ -#define 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 // PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ diff --git a/control/pid_longitudinal_controller/media/BrakeKeeping.drawio.svg b/control/autoware_pid_longitudinal_controller/media/BrakeKeeping.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/BrakeKeeping.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/BrakeKeeping.drawio.svg diff --git a/control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg diff --git a/control/pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg diff --git a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg b/control/autoware_pid_longitudinal_controller/media/slope_definition.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/slope_definition.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/slope_definition.drawio.svg diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml new file mode 100644 index 0000000000000..91d77e454b7ff --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -0,0 +1,49 @@ + + + + autoware_pid_longitudinal_controller + 1.0.0 + PID-based longitudinal controller + + Takamasa Horibe + Takayuki Murooka + Mamoru Sobue + + Apache 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + 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 + diagnostic_updater + eigen + geometry_msgs + interpolation + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros + tier4_debug_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml similarity index 100% rename from control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml rename to control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp similarity index 91% rename from control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp rename to control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 28cd6e1824859..5eb6c87e063eb 100644 --- a/control/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 "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); } @@ -161,12 +161,12 @@ double applyDiffLimitFilter( geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const size_t src_idx, const double distance, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) + const autoware_planning_msgs::msg::Trajectory & trajectory) { 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 new file mode 100644 index 0000000000000..64ffdce8a4243 --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/src/pid.cpp @@ -0,0 +1,105 @@ +// Copyright 2018-2021 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 "autoware/pid_longitudinal_controller/pid.hpp" + +#include +#include +#include +#include +#include + +namespace autoware::motion::control::pid_longitudinal_controller +{ +PIDController::PIDController() +: m_error_integral(0.0), + m_prev_error(0.0), + m_is_first_time(true), + m_is_gains_set(false), + m_is_limits_set(false) +{ +} + +double PIDController::calculate( + const double error, const double dt, const bool enable_integration, + std::vector & pid_contributions) +{ + if (!m_is_gains_set || !m_is_limits_set) { + throw std::runtime_error("Trying to calculate uninitialized PID"); + } + + const auto & p = m_params; + + double ret_p = p.kp * error; + ret_p = std::min(std::max(ret_p, p.min_ret_p), p.max_ret_p); + + if (enable_integration) { + m_error_integral += error * dt; + m_error_integral = std::min(std::max(m_error_integral, p.min_ret_i / p.ki), p.max_ret_i / p.ki); + } + const double ret_i = p.ki * m_error_integral; + + double error_differential; + if (m_is_first_time) { + error_differential = 0; + m_is_first_time = false; + } else { + error_differential = (error - m_prev_error) / dt; + } + double ret_d = p.kd * error_differential; + ret_d = std::min(std::max(ret_d, p.min_ret_d), p.max_ret_d); + + m_prev_error = error; + + pid_contributions.resize(3); + pid_contributions.at(0) = ret_p; + pid_contributions.at(1) = ret_i; + pid_contributions.at(2) = ret_d; + + double ret = ret_p + ret_i + ret_d; + ret = std::min(std::max(ret, p.min_ret), p.max_ret); + + return ret; +} + +void PIDController::setGains(const double kp, const double ki, const double kd) +{ + m_params.kp = kp; + m_params.ki = ki; + m_params.kd = kd; + m_is_gains_set = true; +} + +void PIDController::setLimits( + const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, + const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d) +{ + m_params.max_ret = max_ret; + m_params.min_ret = min_ret; + m_params.max_ret_p = max_ret_p; + m_params.min_ret_p = min_ret_p; + m_params.max_ret_d = max_ret_d; + m_params.min_ret_d = min_ret_d; + m_params.max_ret_i = max_ret_i; + m_params.min_ret_i = min_ret_i; + m_is_limits_set = true; +} + +void PIDController::reset() +{ + m_error_integral = 0.0; + m_prev_error = 0.0; + m_is_first_time = true; +} +} // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp similarity index 96% rename from control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp rename to control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index cc1c0c6383707..78c7ccf832514 100644 --- a/control/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 "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 @@ -38,9 +38,11 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // parameters timer m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double(); - m_wheel_base = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().wheel_base_m; - m_vehicle_width = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().vehicle_width_m; - m_front_overhang = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().front_overhang_m; + m_wheel_base = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().wheel_base_m; + m_vehicle_width = + autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().vehicle_width_m; + m_front_overhang = + autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().front_overhang_m; // parameters for delay compensation m_delay_compensation_time = node.declare_parameter("delay_compensation_time"); // [s] @@ -232,8 +234,7 @@ void PidLongitudinalController::setCurrentOperationMode(const OperationModeState m_current_operation_mode = msg; } -void PidLongitudinalController::setTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & msg) +void PidLongitudinalController::setTrajectory(const autoware_planning_msgs::msg::Trajectory & msg) { if (!longitudinal_utils::isValidTrajectory(msg)) { RCLCPP_ERROR_THROTTLE(logger_, *clock_, 3000, "received invalid trajectory. ignore."); @@ -464,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 = @@ -508,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); @@ -593,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 @@ -604,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"; @@ -820,13 +821,13 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( } // Do not use nearest_idx here -autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController::createCtrlCmdMsg( +autoware_control_msgs::msg::Longitudinal PidLongitudinalController::createCtrlCmdMsg( const Motion & ctrl_cmd, const double & current_vel) { // publish control command - autoware_auto_control_msgs::msg::LongitudinalCommand cmd{}; + autoware_control_msgs::msg::Longitudinal cmd{}; cmd.stamp = clock_->now(); - cmd.speed = static_cast(ctrl_cmd.vel); + cmd.velocity = static_cast(ctrl_cmd.vel); cmd.acceleration = static_cast(ctrl_cmd.acc); // store current velocity history @@ -926,7 +927,7 @@ void PidLongitudinalController::storeAccelCmd(const double accel) { if (m_control_state == ControlState::DRIVE) { // convert format - autoware_auto_control_msgs::msg::LongitudinalCommand cmd; + autoware_control_msgs::msg::Longitudinal cmd; cmd.stamp = clock_->now(); cmd.acceleration = static_cast(accel); @@ -970,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; } @@ -994,10 +995,9 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop return output_motion; } -std::pair +std::pair PidLongitudinalController::calcInterpolatedTrajPointAndSegment( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & pose) const + const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const { if (traj.points.size() == 1) { return std::make_pair(traj.points.at(0), 0); @@ -1150,7 +1150,7 @@ void PidLongitudinalController::updateDebugVelAcc(const ControlData & control_da void PidLongitudinalController::setupDiagnosticUpdater() { - diagnostic_updater_.setHardwareID("pid_longitudinal_controller"); + diagnostic_updater_.setHardwareID("autoware_pid_longitudinal_controller"); diagnostic_updater_.add("control_state", this, &PidLongitudinalController::checkControlState); } diff --git a/control/pid_longitudinal_controller/src/smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp similarity index 98% rename from control/pid_longitudinal_controller/src/smooth_stop.cpp rename to control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp index 27fab43fa506f..cb87157fe1114 100644 --- a/control/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 "pid_longitudinal_controller/smooth_stop.hpp" +#include "autoware/pid_longitudinal_controller/smooth_stop.hpp" #include // NOLINT diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp similarity index 96% rename from control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp rename to control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 02bcce8c91c4b..3c547e762bce6 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#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 "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -37,8 +37,8 @@ namespace longitudinal_utils = TEST(TestLongitudinalControllerUtils, isValidTrajectory) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; Trajectory traj; TrajectoryPoint point; EXPECT_FALSE(longitudinal_utils::isValidTrajectory(traj)); @@ -51,8 +51,8 @@ TEST(TestLongitudinalControllerUtils, isValidTrajectory) TEST(TestLongitudinalControllerUtils, calcStopDistance) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; Pose current_pose; current_pose.position.x = 0.0; @@ -115,8 +115,8 @@ TEST(TestLongitudinalControllerUtils, getPitchByPose) TEST(TestLongitudinalControllerUtils, getPitchByTraj) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; const double wheel_base = 0.9; /** * Trajectory: @@ -346,10 +346,10 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; const double abs_err = 1e-15; - decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points; + decltype(autoware_planning_msgs::msg::Trajectory::points) points; TrajectoryPoint p; p.pose.position.x = 0.0; p.pose.position.y = 0.0; @@ -505,8 +505,8 @@ TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter) TEST(TestLongitudinalControllerUtils, findTrajectoryPoseAfterDistance) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; const double abs_err = 1e-5; Trajectory traj; diff --git a/control/pid_longitudinal_controller/test/test_pid.cpp b/control/autoware_pid_longitudinal_controller/test/test_pid.cpp similarity index 97% rename from control/pid_longitudinal_controller/test/test_pid.cpp rename to control/autoware_pid_longitudinal_controller/test/test_pid.cpp index 82d01e0028a9c..a125707f3867f 100644 --- a/control/pid_longitudinal_controller/test/test_pid.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_pid.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/pid_longitudinal_controller/pid.hpp" #include "gtest/gtest.h" -#include "pid_longitudinal_controller/pid.hpp" #include diff --git a/control/pid_longitudinal_controller/test/test_smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp similarity index 98% rename from control/pid_longitudinal_controller/test/test_smooth_stop.cpp rename to control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp index d658a5271d249..a6d03b8032fe6 100644 --- a/control/pid_longitudinal_controller/test/test_smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/pid_longitudinal_controller/smooth_stop.hpp" #include "gtest/gtest.h" -#include "pid_longitudinal_controller/smooth_stop.hpp" #include "rclcpp/rclcpp.hpp" #include diff --git a/control/autoware_pure_pursuit/CMakeLists.txt b/control/autoware_pure_pursuit/CMakeLists.txt new file mode 100644 index 0000000000000..eac7c723901aa --- /dev/null +++ b/control/autoware_pure_pursuit/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_pure_pursuit) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIRS} +) + +# autoware_pure_pursuit_core +ament_auto_add_library(${PROJECT_NAME}_core SHARED + src/${PROJECT_NAME}_core/planning_utils.cpp + src/${PROJECT_NAME}_core/${PROJECT_NAME}.cpp + src/${PROJECT_NAME}_core/interpolate.cpp +) + +# autoware_pure_pursuit +ament_auto_add_library(${PROJECT_NAME}_lateral_controller SHARED + src/${PROJECT_NAME}/${PROJECT_NAME}_lateral_controller.cpp + src/${PROJECT_NAME}/${PROJECT_NAME}_viz.cpp +) + +target_link_libraries(${PROJECT_NAME}_lateral_controller + ${PROJECT_NAME}_core +) + +rclcpp_components_register_node(${PROJECT_NAME}_lateral_controller + PLUGIN "autoware::pure_pursuit::PurePursuitLateralController" + EXECUTABLE ${PROJECT_NAME}_lateral_controller_exe +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_pure_pursuit/README.md b/control/autoware_pure_pursuit/README.md new file mode 100644 index 0000000000000..3741abbf2a766 --- /dev/null +++ b/control/autoware_pure_pursuit/README.md @@ -0,0 +1,19 @@ +# Pure Pursuit Controller + +The Pure Pursuit Controller module calculates the steering angle for tracking a desired trajectory using the pure pursuit algorithm. This is used as a lateral controller plugin in the `autoware_trajectory_follower_node`. + +## Inputs + +Set the following from the [controller_node](../autoware_trajectory_follower_node/README.md) + +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current ego pose and velocity information + +## Outputs + +Return LateralOutput which contains the following to the controller node + +- `autoware_control_msgs/Lateral`: target steering angle +- LateralSyncData + - steer angle convergence +- `autoware_planning_msgs/Trajectory`: predicted path for ego vehicle diff --git a/control/pure_pursuit/config/pure_pursuit.param.yaml b/control/autoware_pure_pursuit/config/pure_pursuit.param.yaml similarity index 100% rename from control/pure_pursuit/config/pure_pursuit.param.yaml rename to control/autoware_pure_pursuit/config/pure_pursuit.param.yaml diff --git a/control/pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml b/control/autoware_pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml similarity index 100% rename from control/pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml rename to control/autoware_pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml 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 new file mode 100644 index 0000000000000..4b16d2bf5717d --- /dev/null +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit.hpp @@ -0,0 +1,88 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Copyright 2015-2019 Autoware Foundation. 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 AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_ +#define AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_ + +#include + +#include + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +namespace autoware::pure_pursuit +{ +class PurePursuit +{ +public: + PurePursuit() : lookahead_distance_(0.0), closest_thr_dist_(3.0), closest_thr_ang_(M_PI / 4) {} + ~PurePursuit() = default; + + rclcpp::Logger logger = rclcpp::get_logger("pure_pursuit"); + // setter + void setCurrentPose(const geometry_msgs::msg::Pose & msg); + void setWaypoints(const std::vector & msg); + void setLookaheadDistance(double ld) { lookahead_distance_ = ld; } + void setClosestThreshold(double closest_thr_dist, double closest_thr_ang) + { + closest_thr_dist_ = closest_thr_dist; + closest_thr_ang_ = closest_thr_ang; + } + + // getter + geometry_msgs::msg::Point getLocationOfNextWaypoint() const { return loc_next_wp_; } + geometry_msgs::msg::Point getLocationOfNextTarget() const { return loc_next_tgt_; } + + bool isDataReady(); + std::pair run(); // calculate curvature + +private: + // variables for debug + geometry_msgs::msg::Point loc_next_wp_; + geometry_msgs::msg::Point loc_next_tgt_; + + // variables got from outside + double lookahead_distance_, closest_thr_dist_, closest_thr_ang_; + std::shared_ptr> curr_wps_ptr_; + std::shared_ptr curr_pose_ptr_; + + // functions + int32_t findNextPointIdx(int32_t search_start_idx); + std::pair lerpNextTarget(int32_t next_wp_idx); +}; + +} // namespace autoware::pure_pursuit + +#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 new file mode 100644 index 0000000000000..16cb6471deb2c --- /dev/null +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp @@ -0,0 +1,179 @@ +// Copyright 2020-2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// +// 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. +/* + * Copyright 2015-2019 Autoware Foundation. 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 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 "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include +#include +#include + +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include // To be replaced by std::optional in C++17 + +#include +#include + +using autoware::motion::control::trajectory_follower::InputData; +using autoware::motion::control::trajectory_follower::LateralControllerBase; +using autoware::motion::control::trajectory_follower::LateralOutput; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +namespace autoware::pure_pursuit +{ + +struct PpOutput +{ + double curvature; + double velocity; +}; + +struct Param +{ + // Global Parameters + double wheel_base; + double max_steering_angle; // [rad] + + // Algorithm Parameters + double ld_velocity_ratio; + double ld_lateral_error_ratio; + double ld_curvature_ratio; + double min_lookahead_distance; + double max_lookahead_distance; + double reverse_min_lookahead_distance; // min_lookahead_distance in reverse gear + double converged_steer_rad_; + double prediction_ds; + double prediction_distance_length; // Total distance of prediction trajectory + double resampling_ds; + double curvature_calculation_distance; + double long_ld_lateral_error_threshold; + bool enable_path_smoothing; + int path_filter_moving_ave_num; +}; + +struct DebugData +{ + geometry_msgs::msg::Point next_target; +}; + +class PurePursuitLateralController : public LateralControllerBase +{ +public: + /// \param node Reference to the node used only for the component and parameter initialization. + explicit PurePursuitLateralController(rclcpp::Node & node); + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; + std::vector output_tp_array_; + autoware_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; + autoware_planning_msgs::msg::Trajectory trajectory_; + nav_msgs::msg::Odometry current_odometry_; + autoware_vehicle_msgs::msg::SteeringReport current_steering_; + boost::optional prev_cmd_; + + // Debug Publisher + rclcpp::Publisher::SharedPtr pub_debug_marker_; + rclcpp::Publisher::SharedPtr pub_debug_values_; + // Predicted Trajectory publish + rclcpp::Publisher::SharedPtr pub_predicted_trajectory_; + + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + + void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + void setResampledTrajectory(); + + // TF + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + geometry_msgs::msg::Pose current_pose_; + + void publishDebugMarker() const; + + /** + * @brief compute control command for path follow with a constant control period + */ + bool isReady([[maybe_unused]] const InputData & input_data) override; + LateralOutput run(const InputData & input_data) override; + + Lateral generateCtrlCmdMsg(const double target_curvature); + + // Parameter + Param param_{}; + + // Algorithm + std::unique_ptr pure_pursuit_; + + boost::optional calcTargetCurvature( + bool is_control_output, geometry_msgs::msg::Pose pose); + + /** + * @brief It takes current pose, control command, and delta distance. Then it calculates next pose + * of vehicle. + */ + + TrajectoryPoint calcNextPose(const double ds, TrajectoryPoint & point, Lateral cmd) const; + + boost::optional generatePredictedTrajectory(); + + Lateral generateOutputControlCmd(); + + bool calcIsSteerConverged(const Lateral & cmd); + + double calcLookaheadDistance( + const double lateral_error, const double curvature, const double velocity, const double min_ld, + const bool is_control_cmd); + + double calcCurvature(const size_t closest_idx); + + void averageFilterTrajectory(autoware_planning_msgs::msg::Trajectory & u); + + // Debug + mutable DebugData debug_data_; +}; + +} // namespace autoware::pure_pursuit + +#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 new file mode 100644 index 0000000000000..58ce03a0dbf26 --- /dev/null +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_node.hpp @@ -0,0 +1,129 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Copyright 2015-2019 Autoware Foundation. 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 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 +#include +#include + +#include +#include +#include +#include +#include + +#include // To be replaced by std::optional in C++17 + +#include +#include + +#include + +namespace autoware::pure_pursuit +{ +struct Param +{ + // Global Parameters + double wheel_base; + + // Node Parameters + double ctrl_period; + + // Algorithm Parameters + double lookahead_distance_ratio; + double min_lookahead_distance; + double reverse_min_lookahead_distance; // min_lookahead_distance in reverse gear +}; + +struct DebugData +{ + geometry_msgs::msg::Point next_target; +}; + +class PurePursuitNode : public rclcpp::Node +{ +public: + explicit PurePursuitNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscriber + autoware::universe_utils::SelfPoseListener self_pose_listener_{this}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_trajectory_{this, "input/reference_trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_current_odometry_{this, "input/current_odometry"}; + + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; + nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; + + bool isDataReady(); + + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + // TF + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + + // Publisher + rclcpp::Publisher::SharedPtr pub_ctrl_cmd_; + + void publishCommand(const double target_curvature); + + // Debug Publisher + rclcpp::Publisher::SharedPtr pub_debug_marker_; + + void publishDebugMarker() const; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void onTimer(); + + // Parameter + Param param_; + + // Algorithm + std::unique_ptr pure_pursuit_; + + boost::optional calcTargetCurvature(); + boost::optional calcTargetPoint() const; + + // Debug + mutable DebugData debug_data_; +}; + +} // namespace autoware::pure_pursuit + +#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 new file mode 100644 index 0000000000000..ffe328b225d61 --- /dev/null +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_viz.hpp @@ -0,0 +1,47 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Copyright 2015-2019 Autoware Foundation. 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 AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_ +#define AUTOWARE__PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_ + +#include +#include +#include + +#include +namespace autoware::pure_pursuit +{ +visualization_msgs::msg::Marker createNextTargetMarker( + const geometry_msgs::msg::Point & next_target); + +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_ diff --git a/control/pure_pursuit/include/pure_pursuit/util/interpolate.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/interpolate.hpp similarity index 88% rename from control/pure_pursuit/include/pure_pursuit/util/interpolate.hpp rename to control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/interpolate.hpp index b62a0336e5de7..01d64fe83c54f 100644 --- a/control/pure_pursuit/include/pure_pursuit/util/interpolate.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/interpolate.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ -#define PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ +#ifndef AUTOWARE__PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ +#define AUTOWARE__PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ #include #include #include -namespace pure_pursuit +namespace autoware::pure_pursuit { class LinearInterpolate { @@ -50,6 +50,6 @@ class SplineInterpolate const std::vector & return_index, std::vector & return_value); void getValueVector(const std::vector & s_v, std::vector & value_v); }; -} // namespace pure_pursuit +} // namespace autoware::pure_pursuit -#endif // 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 new file mode 100644 index 0000000000000..694a9f9ed0ce5 --- /dev/null +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/marker_helper.hpp @@ -0,0 +1,107 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ +#define AUTOWARE__PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ + +#include + +#include + +#include + +namespace autoware::pure_pursuit +{ +inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) +{ + geometry_msgs::msg::Point point; + + point.x = x; + point.y = y; + point.z = z; + + return point; +} + +inline geometry_msgs::msg::Quaternion createMarkerOrientation( + double x, double y, double z, double w) +{ + geometry_msgs::msg::Quaternion quaternion; + + quaternion.x = x; + quaternion.y = y; + quaternion.z = z; + quaternion.w = w; + + return quaternion; +} + +inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + + scale.x = x; + scale.y = y; + scale.z = z; + + return scale; +} + +inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + + color.r = r; + color.g = g; + color.b = b; + color.a = a; + + return color; +} + +inline visualization_msgs::msg::Marker createDefaultMarker( + const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type, + const geometry_msgs::msg::Vector3 & scale, const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = frame_id; + // ToDo + // marker.header.stamp = rclcpp::Node::now(); + marker.ns = ns; + marker.id = id; + marker.type = type; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0); + + marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); + marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); + marker.scale = scale; + marker.color = color; + marker.frame_locked = true; + + return marker; +} + +inline void appendMarkerArray( + const visualization_msgs::msg::MarkerArray & additional_marker_array, + visualization_msgs::msg::MarkerArray * marker_array) +{ + for (const auto & marker : additional_marker_array.markers) { + marker_array->markers.push_back(marker); + } +} +} // namespace autoware::pure_pursuit + +#endif // AUTOWARE__PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/planning_utils.hpp similarity index 90% rename from control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp rename to control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/planning_utils.hpp index a5b1e17ed983f..229cc6654c868 100644 --- a/control/pure_pursuit/include/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 PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ -#define 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 @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -45,13 +45,13 @@ #define PLANNING_UTILS_LOGGER "planning_utils" -namespace pure_pursuit +namespace autoware::pure_pursuit { namespace planning_utils { constexpr double ERROR = 1e-6; double calcArcLengthFromWayPoint( - const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, const size_t dst_idx); double calcCurvature( const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & curr_pose); @@ -66,7 +66,7 @@ double calcRadius( double convertCurvatureToSteeringAngle(double wheel_base, double kappa); std::vector extractPoses( - const autoware_auto_planning_msgs::msg::Trajectory & motions); + const autoware_planning_msgs::msg::Trajectory & motions); std::pair findClosestIdxWithDistAngThr( const std::vector & poses, @@ -139,6 +139,6 @@ geometry_msgs::msg::Point transformToRelativeCoordinate2D( geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double _yaw); } // namespace planning_utils -} // namespace pure_pursuit +} // namespace autoware::pure_pursuit -#endif // PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ +#endif // AUTOWARE__PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/util/tf_utils.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/tf_utils.hpp similarity index 92% rename from control/pure_pursuit/include/pure_pursuit/util/tf_utils.hpp rename to control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/tf_utils.hpp index b63bff5b6a6c6..05701f678dc3a 100644 --- a/control/pure_pursuit/include/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 PURE_PURSUIT__UTIL__TF_UTILS_HPP_ -#define PURE_PURSUIT__UTIL__TF_UTILS_HPP_ +#ifndef AUTOWARE__PURE_PURSUIT__UTIL__TF_UTILS_HPP_ +#define AUTOWARE__PURE_PURSUIT__UTIL__TF_UTILS_HPP_ #include @@ -31,7 +31,7 @@ #define TF_UTILS_LOGGER "tf_utils" -namespace pure_pursuit +namespace autoware::pure_pursuit { namespace tf_utils { @@ -92,6 +92,6 @@ inline boost::optional getCurrentPose( } } // namespace tf_utils -} // namespace pure_pursuit +} // namespace autoware::pure_pursuit -#endif // PURE_PURSUIT__UTIL__TF_UTILS_HPP_ +#endif // AUTOWARE__PURE_PURSUIT__UTIL__TF_UTILS_HPP_ diff --git a/control/pure_pursuit/launch/pure_pursuit.launch.xml b/control/autoware_pure_pursuit/launch/pure_pursuit.launch.xml similarity index 100% rename from control/pure_pursuit/launch/pure_pursuit.launch.xml rename to control/autoware_pure_pursuit/launch/pure_pursuit.launch.xml diff --git a/control/autoware_pure_pursuit/package.xml b/control/autoware_pure_pursuit/package.xml new file mode 100644 index 0000000000000..fc5c0e41a15a0 --- /dev/null +++ b/control/autoware_pure_pursuit/package.xml @@ -0,0 +1,44 @@ + + + + autoware_pure_pursuit + 0.1.0 + The pure_pursuit package + Takamasa Horibe + Takayuki Murooka + Apache License 2.0 + + Berkay Karaman + Takamasa Horibe + + ament_cmake_auto + 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 + nav_msgs + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_debug_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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 new file mode 100644 index 0000000000000..803a6ef1e8617 --- /dev/null +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp @@ -0,0 +1,490 @@ +// Copyright 2020-2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// +// 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. + +/* + * Copyright 2015-2019 Autoware Foundation. 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 "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 + +#include +#include +#include + +namespace +{ +enum TYPE { + VEL_LD = 0, + CURVATURE_LD = 1, + LATERAL_ERROR_LD = 2, + TOTAL_LD = 3, + CURVATURE = 4, + LATERAL_ERROR = 5, + VELOCITY = 6, + SIZE // this is the number of enum elements +}; +} // namespace + +namespace autoware::pure_pursuit +{ +PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) +: clock_(node.get_clock()), + logger_(node.get_logger().get_child("lateral_controller")), + tf_buffer_(clock_), + tf_listener_(tf_buffer_) +{ + pure_pursuit_ = std::make_unique(); + + // Vehicle Parameters + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); + param_.wheel_base = vehicle_info.wheel_base_m; + param_.max_steering_angle = vehicle_info.max_steer_angle_rad; + + // Algorithm Parameters + param_.ld_velocity_ratio = node.declare_parameter("ld_velocity_ratio"); + param_.ld_lateral_error_ratio = node.declare_parameter("ld_lateral_error_ratio"); + param_.ld_curvature_ratio = node.declare_parameter("ld_curvature_ratio"); + param_.long_ld_lateral_error_threshold = + node.declare_parameter("long_ld_lateral_error_threshold"); + param_.min_lookahead_distance = node.declare_parameter("min_lookahead_distance"); + param_.max_lookahead_distance = node.declare_parameter("max_lookahead_distance"); + param_.reverse_min_lookahead_distance = + node.declare_parameter("reverse_min_lookahead_distance"); + param_.converged_steer_rad_ = node.declare_parameter("converged_steer_rad"); + param_.prediction_ds = node.declare_parameter("prediction_ds"); + param_.prediction_distance_length = node.declare_parameter("prediction_distance_length"); + param_.resampling_ds = node.declare_parameter("resampling_ds"); + param_.curvature_calculation_distance = + node.declare_parameter("curvature_calculation_distance"); + param_.enable_path_smoothing = node.declare_parameter("enable_path_smoothing"); + param_.path_filter_moving_ave_num = node.declare_parameter("path_filter_moving_ave_num"); + + // Debug Publishers + pub_debug_marker_ = + node.create_publisher("~/debug/markers", 0); + pub_debug_values_ = node.create_publisher( + "~/debug/ld_outputs", rclcpp::QoS{1}); + + // Publish predicted trajectory + pub_predicted_trajectory_ = node.create_publisher( + "~/output/predicted_trajectory", 1); +} + +double PurePursuitLateralController::calcLookaheadDistance( + const double lateral_error, const double curvature, const double velocity, const double min_ld, + const bool is_control_cmd) +{ + const double vel_ld = abs(param_.ld_velocity_ratio * velocity); + const double curvature_ld = -abs(param_.ld_curvature_ratio * curvature); + double lateral_error_ld = 0.0; + + if (abs(lateral_error) >= param_.long_ld_lateral_error_threshold) { + // If lateral error is higher than threshold, we should make ld larger to prevent entering the + // road with high heading error. + lateral_error_ld = abs(param_.ld_lateral_error_ratio * lateral_error); + } + + const double total_ld = + std::clamp(vel_ld + curvature_ld + lateral_error_ld, min_ld, param_.max_lookahead_distance); + + auto pubDebugValues = [&]() { + tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + debug_msg.data.resize(TYPE::SIZE); + debug_msg.data.at(TYPE::VEL_LD) = static_cast(vel_ld); + debug_msg.data.at(TYPE::CURVATURE_LD) = static_cast(curvature_ld); + debug_msg.data.at(TYPE::LATERAL_ERROR_LD) = static_cast(lateral_error_ld); + debug_msg.data.at(TYPE::TOTAL_LD) = static_cast(total_ld); + debug_msg.data.at(TYPE::VELOCITY) = static_cast(velocity); + debug_msg.data.at(TYPE::CURVATURE) = static_cast(curvature); + debug_msg.data.at(TYPE::LATERAL_ERROR) = static_cast(lateral_error); + debug_msg.stamp = clock_->now(); + pub_debug_values_->publish(debug_msg); + }; + + if (is_control_cmd) { + pubDebugValues(); + } + + return total_ld; +} + +TrajectoryPoint PurePursuitLateralController::calcNextPose( + const double ds, TrajectoryPoint & point, Lateral cmd) const +{ + geometry_msgs::msg::Transform transform; + 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; + + tf2::Transform tf_pose; + tf2::Transform tf_offset; + tf2::fromMsg(transform, tf_offset); + tf2::fromMsg(point.pose, tf_pose); + tf2::toMsg(tf_pose * tf_offset, output_p.pose); + return output_p; +} + +void PurePursuitLateralController::setResampledTrajectory() +{ + // Interpolate with constant interval distance. + std::vector out_arclength; + 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( + 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_ = autoware::motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_); +} + +double PurePursuitLateralController::calcCurvature(const size_t closest_idx) +{ + // Calculate current curvature + const size_t idx_dist = static_cast( + std::max(static_cast((param_.curvature_calculation_distance) / param_.resampling_ds), 1)); + + // Find the points in trajectory to calculate curvature + size_t next_idx = trajectory_resampled_->points.size() - 1; + size_t prev_idx = 0; + + if (static_cast(closest_idx) >= idx_dist) { + prev_idx = closest_idx - idx_dist; + } else { + // return zero curvature when backward distance is not long enough in the trajectory + return 0.0; + } + + if (trajectory_resampled_->points.size() - 1 >= closest_idx + idx_dist) { + next_idx = closest_idx + idx_dist; + } else { + // return zero curvature when forward distance is not long enough in the trajectory + return 0.0; + } + // TODO(k.sugahara): shift the center point of the curvature calculation to allow sufficient + // distance, because if sufficient distance cannot be obtained in front or behind, the curvature + // will be zero in the current implementation. + + // Calculate curvature assuming the trajectory points interval is constant + double current_curvature = 0.0; + + try { + 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()); + current_curvature = 0.0; + } + return current_curvature; +} + +void PurePursuitLateralController::averageFilterTrajectory( + autoware_planning_msgs::msg::Trajectory & u) +{ + if (static_cast(u.points.size()) <= 2 * param_.path_filter_moving_ave_num) { + RCLCPP_ERROR(logger_, "Cannot smooth path! Trajectory size is too low!"); + return; + } + + autoware_planning_msgs::msg::Trajectory filtered_trajectory(u); + + for (int64_t i = 0; i < static_cast(u.points.size()); ++i) { + TrajectoryPoint tmp{}; + int64_t num_tmp = param_.path_filter_moving_ave_num; + int64_t count = 0; + double yaw = 0.0; + if (i - num_tmp < 0) { + num_tmp = i; + } + if (i + num_tmp > static_cast(u.points.size()) - 1) { + num_tmp = static_cast(u.points.size()) - i - 1; + } + for (int64_t j = -num_tmp; j <= num_tmp; ++j) { + const auto & p = u.points.at(static_cast(i + j)); + + tmp.pose.position.x += p.pose.position.x; + tmp.pose.position.y += p.pose.position.y; + tmp.pose.position.z += p.pose.position.z; + tmp.longitudinal_velocity_mps += p.longitudinal_velocity_mps; + tmp.acceleration_mps2 += p.acceleration_mps2; + tmp.front_wheel_angle_rad += p.front_wheel_angle_rad; + tmp.heading_rate_rps += p.heading_rate_rps; + yaw += tf2::getYaw(p.pose.orientation); + tmp.lateral_velocity_mps += p.lateral_velocity_mps; + tmp.rear_wheel_angle_rad += p.rear_wheel_angle_rad; + ++count; + } + auto & p = filtered_trajectory.points.at(static_cast(i)); + + p.pose.position.x = tmp.pose.position.x / count; + p.pose.position.y = tmp.pose.position.y / count; + p.pose.position.z = tmp.pose.position.z / count; + p.longitudinal_velocity_mps = tmp.longitudinal_velocity_mps / count; + p.acceleration_mps2 = tmp.acceleration_mps2 / count; + p.front_wheel_angle_rad = tmp.front_wheel_angle_rad / count; + p.heading_rate_rps = tmp.heading_rate_rps / count; + p.lateral_velocity_mps = tmp.lateral_velocity_mps / count; + p.rear_wheel_angle_rad = tmp.rear_wheel_angle_rad / count; + p.pose.orientation = autoware::pure_pursuit::planning_utils::getQuaternionFromYaw(yaw / count); + } + trajectory_resampled_ = std::make_shared(filtered_trajectory); +} + +boost::optional PurePursuitLateralController::generatePredictedTrajectory() +{ + 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; + } + + const double remaining_distance = planning_utils::calcArcLengthFromWayPoint( + *trajectory_resampled_, *closest_idx_result, trajectory_resampled_->points.size() - 1); + + const auto num_of_iteration = std::max( + static_cast(std::ceil( + std::min(remaining_distance, param_.prediction_distance_length) / param_.prediction_ds)), + 1); + Trajectory predicted_trajectory; + + // Iterative prediction: + for (int i = 0; i < num_of_iteration; i++) { + if (i == 0) { + // For first point, use the odometry for velocity, and use the current_pose for prediction. + + TrajectoryPoint p; + p.pose = current_odometry_.pose.pose; + p.longitudinal_velocity_mps = current_odometry_.twist.twist.linear.x; + predicted_trajectory.points.push_back(p); + + const auto pp_output = calcTargetCurvature(true, predicted_trajectory.points.at(i).pose); + Lateral tmp_msg; + + if (pp_output) { + tmp_msg = generateCtrlCmdMsg(pp_output->curvature); + predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; + } else { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); + tmp_msg = generateCtrlCmdMsg(0.0); + } + TrajectoryPoint p2; + p2 = calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg); + predicted_trajectory.points.push_back(p2); + + } else { + const auto pp_output = calcTargetCurvature(false, predicted_trajectory.points.at(i).pose); + Lateral tmp_msg; + + if (pp_output) { + tmp_msg = generateCtrlCmdMsg(pp_output->curvature); + predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; + } else { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); + tmp_msg = generateCtrlCmdMsg(0.0); + } + predicted_trajectory.points.push_back( + calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg)); + } + } + + // for last point + predicted_trajectory.points.back().longitudinal_velocity_mps = 0.0; + predicted_trajectory.header.frame_id = trajectory_resampled_->header.frame_id; + predicted_trajectory.header.stamp = trajectory_resampled_->header.stamp; + + return predicted_trajectory; +} + +bool PurePursuitLateralController::isReady([[maybe_unused]] const InputData & input_data) +{ + return true; +} + +LateralOutput PurePursuitLateralController::run(const InputData & input_data) +{ + current_pose_ = input_data.current_odometry.pose.pose; + trajectory_ = input_data.current_trajectory; + current_odometry_ = input_data.current_odometry; + current_steering_ = input_data.current_steering; + + setResampledTrajectory(); + if (param_.enable_path_smoothing) { + averageFilterTrajectory(*trajectory_resampled_); + } + const auto cmd_msg = generateOutputControlCmd(); + + LateralOutput output; + output.control_cmd = cmd_msg; + output.sync_data.is_steer_converged = calcIsSteerConverged(cmd_msg); + + // calculate predicted trajectory with iterative calculation + const auto predicted_trajectory = generatePredictedTrajectory(); + if (!predicted_trajectory) { + RCLCPP_ERROR(logger_, "Failed to generate predicted trajectory."); + } else { + pub_predicted_trajectory_->publish(*predicted_trajectory); + } + + return output; +} + +bool PurePursuitLateralController::calcIsSteerConverged(const Lateral & cmd) +{ + return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) < + static_cast(param_.converged_steer_rad_); +} + +Lateral PurePursuitLateralController::generateOutputControlCmd() +{ + // Generate the control command + const auto pp_output = calcTargetCurvature(true, current_odometry_.pose.pose); + Lateral output_cmd; + + if (pp_output) { + output_cmd = generateCtrlCmdMsg(pp_output->curvature); + prev_cmd_ = boost::optional(output_cmd); + publishDebugMarker(); + } else { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, "failed to solve pure_pursuit for control command calculation"); + if (prev_cmd_) { + output_cmd = *prev_cmd_; + } else { + output_cmd = generateCtrlCmdMsg(0.0); + } + } + return output_cmd; +} + +Lateral PurePursuitLateralController::generateCtrlCmdMsg(const double target_curvature) +{ + const double tmp_steering = + planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); + Lateral cmd; + cmd.stamp = clock_->now(); + cmd.steering_tire_angle = static_cast( + std::min(std::max(tmp_steering, -param_.max_steering_angle), param_.max_steering_angle)); + + // pub_ctrl_cmd_->publish(cmd); + return cmd; +} + +void PurePursuitLateralController::publishDebugMarker() const +{ + visualization_msgs::msg::MarkerArray marker_array; + + marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target)); + marker_array.markers.push_back( + createTrajectoryCircleMarker(debug_data_.next_target, current_odometry_.pose.pose)); +} + +boost::optional PurePursuitLateralController::calcTargetCurvature( + bool is_control_output, geometry_msgs::msg::Pose pose) +{ + // Ignore invalid trajectory + if (trajectory_resampled_->points.size() < 3) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "received path size is < 3, ignored"); + return {}; + } + + // Calculate target point for velocity/acceleration + + const auto closest_idx_result = + 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 {}; + } + + const double target_vel = + trajectory_resampled_->points.at(*closest_idx_result).longitudinal_velocity_mps; + + // calculate the lateral error + + const double lateral_error = + autoware::motion_utils::calcLateralOffset(trajectory_resampled_->points, pose.position); + + // calculate the current curvature + + const double current_curvature = calcCurvature(*closest_idx_result); + + // Calculate lookahead distance + + const bool is_reverse = (target_vel < 0); + const double min_lookahead_distance = + is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance; + double lookahead_distance = min_lookahead_distance; + if (is_control_output) { + lookahead_distance = calcLookaheadDistance( + lateral_error, current_curvature, current_odometry_.twist.twist.linear.x, + min_lookahead_distance, is_control_output); + } else { + lookahead_distance = calcLookaheadDistance( + lateral_error, current_curvature, target_vel, min_lookahead_distance, is_control_output); + } + + // Set PurePursuit data + pure_pursuit_->setCurrentPose(pose); + pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_resampled_)); + pure_pursuit_->setLookaheadDistance(lookahead_distance); + + // Run PurePursuit + const auto pure_pursuit_result = pure_pursuit_->run(); + if (!pure_pursuit_result.first) { + return {}; + } + + const auto kappa = pure_pursuit_result.second; + + // Set debug data + if (is_control_output) { + debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget(); + } + PpOutput output{}; + output.curvature = kappa; + if (!is_control_output) { + output.velocity = current_odometry_.twist.twist.linear.x; + } else { + output.velocity = target_vel; + } + + return output; +} +} // namespace autoware::pure_pursuit 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 new file mode 100644 index 0000000000000..9a3b90b86b98e --- /dev/null +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp @@ -0,0 +1,214 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Copyright 2015-2019 Autoware Foundation. 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 "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 + +#include +#include +#include + +namespace +{ +double calcLookaheadDistance( + const double velocity, const double lookahead_distance_ratio, const double min_lookahead_distance) +{ + const double lookahead_distance = lookahead_distance_ratio * std::abs(velocity); + return std::max(lookahead_distance, min_lookahead_distance); +} + +} // namespace + +namespace autoware::pure_pursuit +{ +PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) +: Node("pure_pursuit", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) +{ + pure_pursuit_ = std::make_unique(); + + // Vehicle Parameters + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); + param_.wheel_base = vehicle_info.wheel_base_m; + + // Node Parameters + param_.ctrl_period = this->declare_parameter("control_period"); + + // Algorithm Parameters + param_.lookahead_distance_ratio = this->declare_parameter("lookahead_distance_ratio"); + param_.min_lookahead_distance = this->declare_parameter("min_lookahead_distance"); + param_.reverse_min_lookahead_distance = + this->declare_parameter("reverse_min_lookahead_distance"); + + // Publishers + pub_ctrl_cmd_ = + this->create_publisher("output/control_raw", 1); + + // Debug Publishers + pub_debug_marker_ = + this->create_publisher("~/debug/markers", 0); + + // Timer + { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(param_.ctrl_period)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&PurePursuitNode::onTimer, this)); + } + + // Wait for first current pose + tf_utils::waitForTransform(tf_buffer_, "map", "base_link"); +} + +bool PurePursuitNode::isDataReady() +{ + if (!current_odometry_) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_odometry..."); + return false; + } + + if (!trajectory_) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for trajectory..."); + return false; + } + + if (!current_pose_) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_pose..."); + return false; + } + + return true; +} + +void PurePursuitNode::onTimer() +{ + current_pose_ = self_pose_listener_.getCurrentPose(); + + current_odometry_ = sub_current_odometry_.takeData(); + trajectory_ = sub_trajectory_.takeData(); + if (!isDataReady()) { + return; + } + + const auto target_curvature = calcTargetCurvature(); + + if (target_curvature) { + publishCommand(*target_curvature); + publishDebugMarker(); + } else { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "failed to solve pure_pursuit"); + publishCommand({0.0}); + } +} + +void PurePursuitNode::publishCommand(const double target_curvature) +{ + autoware_control_msgs::msg::Lateral cmd; + cmd.stamp = get_clock()->now(); + cmd.steering_tire_angle = + planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); + pub_ctrl_cmd_->publish(cmd); +} + +void PurePursuitNode::publishDebugMarker() const +{ + visualization_msgs::msg::MarkerArray marker_array; + + marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target)); + marker_array.markers.push_back( + createTrajectoryCircleMarker(debug_data_.next_target, current_pose_->pose)); + + pub_debug_marker_->publish(marker_array); +} + +boost::optional PurePursuitNode::calcTargetCurvature() +{ + // Ignore invalid trajectory + if (trajectory_->points.size() < 3) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "received path size is < 3, ignored"); + return {}; + } + + // Calculate target point for velocity/acceleration + const auto target_point = calcTargetPoint(); + if (!target_point) { + return {}; + } + + const double target_vel = target_point->longitudinal_velocity_mps; + + // Calculate lookahead distance + const bool is_reverse = (target_vel < 0); + const double min_lookahead_distance = + is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance; + const double lookahead_distance = calcLookaheadDistance( + current_odometry_->twist.twist.linear.x, param_.lookahead_distance_ratio, + min_lookahead_distance); + + // Set PurePursuit data + pure_pursuit_->setCurrentPose(current_pose_->pose); + pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_)); + pure_pursuit_->setLookaheadDistance(lookahead_distance); + + // Run PurePursuit + const auto pure_pursuit_result = pure_pursuit_->run(); + if (!pure_pursuit_result.first) { + return {}; + } + + const auto kappa = pure_pursuit_result.second; + + // Set debug data + debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget(); + + return kappa; +} + +boost::optional PurePursuitNode::calcTargetPoint() + const +{ + const auto closest_idx_result = planning_utils::findClosestIdxWithDistAngThr( + planning_utils::extractPoses(*trajectory_), current_pose_->pose, 3.0, M_PI_4); + + if (!closest_idx_result.first) { + RCLCPP_ERROR(get_logger(), "cannot find closest waypoint"); + return {}; + } + + return trajectory_->points.at(closest_idx_result.second); +} +} // namespace autoware::pure_pursuit + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pure_pursuit::PurePursuitNode) 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 new file mode 100644 index 0000000000000..ceb4601ce2d1e --- /dev/null +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_viz.cpp @@ -0,0 +1,93 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Copyright 2015-2019 Autoware Foundation. 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 "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 + +namespace +{ +std::vector generateTrajectoryCircle( + const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose) +{ + constexpr double theta_range = M_PI / 10; + constexpr double step_rad = 0.005; + + const double radius = autoware::pure_pursuit::planning_utils::calcRadius(target, current_pose); + + std::vector trajectory_circle; + for (double theta = -theta_range; theta <= theta_range; theta += step_rad) { + geometry_msgs::msg::Point p; + p.x = radius * sin(theta); + p.y = radius * (1 - cos(theta)); + p.z = target.z; + + trajectory_circle.push_back( + autoware::pure_pursuit::planning_utils::transformToAbsoluteCoordinate2D(p, current_pose)); + } + + return trajectory_circle; +} + +} // namespace + +namespace autoware::pure_pursuit +{ +visualization_msgs::msg::Marker createNextTargetMarker( + const geometry_msgs::msg::Point & next_target) +{ + auto marker = createDefaultMarker( + "map", "next_target", 0, visualization_msgs::msg::Marker::SPHERE, + createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(0.0, 1.0, 0.0, 1.0)); + + marker.pose.position = next_target; + + return marker; +} + +visualization_msgs::msg::Marker createTrajectoryCircleMarker( + const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose) +{ + auto marker = createDefaultMarker( + "map", "trajectory_circle", 0, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.05, 0, 0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + + const auto trajectory_circle = generateTrajectoryCircle(target, current_pose); + for (auto p : trajectory_circle) { + marker.points.push_back(p); + marker.colors.push_back(marker.color); + } + + return marker; +} +} // namespace autoware::pure_pursuit 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 new file mode 100644 index 0000000000000..ed9579f296fea --- /dev/null +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp @@ -0,0 +1,212 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Copyright 2015-2019 Autoware Foundation. 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 "autoware/pure_pursuit/autoware_pure_pursuit.hpp" + +#include "autoware/pure_pursuit/util/planning_utils.hpp" + +#include +#include +#include +#include + +namespace autoware::pure_pursuit +{ +bool PurePursuit::isDataReady() +{ + if (!curr_wps_ptr_) { + return false; + } + if (!curr_pose_ptr_) { + return false; + } + return true; +} + +std::pair PurePursuit::run() +{ + if (!isDataReady()) { + return std::make_pair(false, std::numeric_limits::quiet_NaN()); + } + + auto closest_pair = planning_utils::findClosestIdxWithDistAngThr( + *curr_wps_ptr_, *curr_pose_ptr_, closest_thr_dist_, closest_thr_ang_); + + if (!closest_pair.first) { + RCLCPP_WARN( + logger, "cannot find, curr_bool: %d, closest_idx: %d", closest_pair.first, + closest_pair.second); + return std::make_pair(false, std::numeric_limits::quiet_NaN()); + } + + int32_t next_wp_idx = findNextPointIdx(closest_pair.second); + if (next_wp_idx == -1) { + RCLCPP_WARN(logger, "lost next waypoint"); + return std::make_pair(false, std::numeric_limits::quiet_NaN()); + } + + loc_next_wp_ = curr_wps_ptr_->at(next_wp_idx).position; + + geometry_msgs::msg::Point next_tgt_pos; + // if next waypoint is first + if (next_wp_idx == 0) { + next_tgt_pos = curr_wps_ptr_->at(next_wp_idx).position; + } else { + // linear interpolation + std::pair lerp_pair = lerpNextTarget(next_wp_idx); + + if (!lerp_pair.first) { + RCLCPP_WARN(logger, "lost target! "); + return std::make_pair(false, std::numeric_limits::quiet_NaN()); + } + + next_tgt_pos = lerp_pair.second; + } + loc_next_tgt_ = next_tgt_pos; + + double kappa = planning_utils::calcCurvature(next_tgt_pos, *curr_pose_ptr_); + + return std::make_pair(true, kappa); +} + +// linear interpolation of next target +std::pair PurePursuit::lerpNextTarget(int32_t next_wp_idx) +{ + constexpr double ERROR2 = 1e-5; // 0.00001 + const geometry_msgs::msg::Point & vec_end = curr_wps_ptr_->at(next_wp_idx).position; + const geometry_msgs::msg::Point & vec_start = curr_wps_ptr_->at(next_wp_idx - 1).position; + const geometry_msgs::msg::Pose & curr_pose = *curr_pose_ptr_; + + Eigen::Vector3d vec_a( + (vec_end.x - vec_start.x), (vec_end.y - vec_start.y), (vec_end.z - vec_start.z)); + + if (vec_a.norm() < ERROR2) { + RCLCPP_ERROR(logger, "waypoint interval is almost 0"); + return std::make_pair(false, geometry_msgs::msg::Point()); + } + + const double lateral_error = + planning_utils::calcLateralError2D(vec_start, vec_end, curr_pose.position); + + if (fabs(lateral_error) > lookahead_distance_) { + RCLCPP_ERROR(logger, "lateral error is larger than lookahead distance"); + RCLCPP_ERROR( + logger, "lateral error: %lf, lookahead distance: %lf", lateral_error, lookahead_distance_); + return std::make_pair(false, geometry_msgs::msg::Point()); + } + + /* calculate the position of the foot of a perpendicular line */ + Eigen::Vector2d uva2d(vec_a.x(), vec_a.y()); + uva2d.normalize(); + Eigen::Rotation2Dd rot = + (lateral_error > 0) ? Eigen::Rotation2Dd(-M_PI / 2.0) : Eigen::Rotation2Dd(M_PI / 2.0); + Eigen::Vector2d uva2d_rot = rot * uva2d; + + geometry_msgs::msg::Point h; + h.x = curr_pose.position.x + fabs(lateral_error) * uva2d_rot.x(); + h.y = curr_pose.position.y + fabs(lateral_error) * uva2d_rot.y(); + h.z = curr_pose.position.z; + + // if there is a intersection + if (fabs(fabs(lateral_error) - lookahead_distance_) < ERROR2) { + return std::make_pair(true, h); + } else { + // if there are two intersection + // get intersection in front of vehicle + const double s = sqrt(pow(lookahead_distance_, 2) - pow(lateral_error, 2)); + geometry_msgs::msg::Point res; + res.x = h.x + s * uva2d.x(); + res.y = h.y + s * uva2d.y(); + res.z = curr_pose.position.z; + return std::make_pair(true, res); + } +} + +int32_t PurePursuit::findNextPointIdx(int32_t search_start_idx) +{ + // if waypoints are not given, do nothing. + if (curr_wps_ptr_->empty() || search_start_idx == -1) { + return -1; + } + + // look for the next waypoint. + for (int32_t i = search_start_idx; i < (int32_t)curr_wps_ptr_->size(); i++) { + // if search waypoint is the last + if (i == ((int32_t)curr_wps_ptr_->size() - 1)) { + return i; + } + + // if waypoint direction is forward + const auto gld = planning_utils::getLaneDirection(*curr_wps_ptr_, 0.05); + if (gld == 0) { + // if waypoint is not in front of ego, skip + auto ret = planning_utils::transformToRelativeCoordinate2D( + curr_wps_ptr_->at(i).position, *curr_pose_ptr_); + if (ret.x < 0) { + continue; + } + } else if (gld == 1) { + // waypoint direction is backward + + // if waypoint is in front of ego, skip + auto ret = planning_utils::transformToRelativeCoordinate2D( + curr_wps_ptr_->at(i).position, *curr_pose_ptr_); + if (ret.x > 0) { + continue; + } + } else { + return -1; + } + + const geometry_msgs::msg::Point & curr_motion_point = curr_wps_ptr_->at(i).position; + const geometry_msgs::msg::Point & curr_pose_point = curr_pose_ptr_->position; + // if there exists an effective waypoint + const double ds = planning_utils::calcDistSquared2D(curr_motion_point, curr_pose_point); + if (ds > std::pow(lookahead_distance_, 2)) { + return i; + } + } + + // if this program reaches here , it means we lost the waypoint! + return -1; +} + +void PurePursuit::setCurrentPose(const geometry_msgs::msg::Pose & msg) +{ + curr_pose_ptr_ = std::make_shared(); + *curr_pose_ptr_ = msg; +} + +void PurePursuit::setWaypoints(const std::vector & msg) +{ + curr_wps_ptr_ = std::make_shared>(); + *curr_wps_ptr_ = msg; +} + +} // namespace autoware::pure_pursuit diff --git a/control/pure_pursuit/src/pure_pursuit_core/interpolate.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp similarity index 98% rename from control/pure_pursuit/src/pure_pursuit_core/interpolate.cpp rename to control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp index 8890e0c7da25d..62f5f7a5737c9 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/interpolate.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pure_pursuit/util/interpolate.hpp" +#include "autoware/pure_pursuit/util/interpolate.hpp" #include #include -namespace pure_pursuit +namespace autoware::pure_pursuit { bool LinearInterpolate::interpolate( const std::vector & base_index, const std::vector & base_value, @@ -234,4 +234,4 @@ bool SplineInterpolate::interpolate( } return true; } -} // namespace pure_pursuit +} // namespace autoware::pure_pursuit diff --git a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp similarity index 96% rename from control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp rename to control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp index 2597d7087ad61..71c48fb35e998 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pure_pursuit/util/planning_utils.hpp" +#include "autoware/pure_pursuit/util/planning_utils.hpp" #include #include #include -namespace pure_pursuit +namespace autoware::pure_pursuit { namespace planning_utils { double calcArcLengthFromWayPoint( - const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, const size_t dst_idx) { double length = 0; @@ -101,7 +101,7 @@ double convertCurvatureToSteeringAngle(double wheel_base, double kappa) } std::vector extractPoses( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) + const autoware_planning_msgs::msg::Trajectory & trajectory) { std::vector poses; @@ -266,4 +266,4 @@ geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double _yaw) } } // namespace planning_utils -} // namespace pure_pursuit +} // namespace autoware::pure_pursuit diff --git a/control/autoware_shift_decider/CMakeLists.txt b/control/autoware_shift_decider/CMakeLists.txt new file mode 100644 index 0000000000000..269b0fcc33690 --- /dev/null +++ b/control/autoware_shift_decider/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_shift_decider) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/${PROJECT_NAME}.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "autoware::shift_decider::ShiftDecider" + EXECUTABLE ${PROJECT_NAME} +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_shift_decider/README.md b/control/autoware_shift_decider/README.md new file mode 100644 index 0000000000000..84767abceff6b --- /dev/null +++ b/control/autoware_shift_decider/README.md @@ -0,0 +1,56 @@ +# Shift Decider + +## Purpose + +`autoware_shift_decider` is a module to decide shift from ackermann control command. + +## Inner-workings / Algorithms + +### Flow chart + +```plantuml +@startuml +skinparam monochrome true + +title update current shift +start +if (absolute target velocity is less than threshold) then (yes) + :set previous shift; +else(no) +if (target velocity is positive) then (yes) + :set shift DRIVE; +else + :set shift REVERSE; +endif +endif + :publish current shift; +note right + publish shift for constant interval +end note +stop +@enduml +``` + +### Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------------- | ------------------------------------- | ---------------------------- | +| `~/input/control_cmd` | `autoware_control_msgs::msg::Control` | Control command for vehicle. | + +### Output + +| Name | Type | Description | +| ------------------ | ----------------------------------------- | ---------------------------------- | +| `~output/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Gear for drive forward / backward. | + +## Parameters + +none. + +## Assumptions / Known limits + +TBD. diff --git a/control/shift_decider/config/shift_decider.param.yaml b/control/autoware_shift_decider/config/shift_decider.param.yaml similarity index 100% rename from control/shift_decider/config/shift_decider.param.yaml rename to control/autoware_shift_decider/config/shift_decider.param.yaml 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 new file mode 100644 index 0000000000000..c7ce822d1ac18 --- /dev/null +++ b/control/autoware_shift_decider/include/autoware/shift_decider/autoware_shift_decider.hpp @@ -0,0 +1,65 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ +#define AUTOWARE__SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ + +#include "autoware/universe_utils/ros/polling_subscriber.hpp" + +#include + +#include +#include +#include +#include + +#include + +namespace autoware::shift_decider +{ + +class ShiftDecider : public rclcpp::Node +{ +public: + explicit ShiftDecider(const rclcpp::NodeOptions & node_options); + +private: + void onTimer(); + void onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg); + void onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg); + void onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg); + void updateCurrentShiftCmd(); + void initTimer(double period_s); + + rclcpp::Publisher::SharedPtr pub_shift_cmd_; + autoware::universe_utils::InterProcessPollingSubscriber + sub_control_cmd_{this, "input/control_cmd"}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_autoware_state_{this, "input/state"}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_current_gear_{this, "input/current_gear"}; + + rclcpp::TimerBase::SharedPtr timer_; + + autoware_control_msgs::msg::Control::ConstSharedPtr control_cmd_; + autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_; + autoware_vehicle_msgs::msg::GearCommand shift_cmd_; + autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr current_gear_ptr_; + uint8_t prev_shift_command = autoware_vehicle_msgs::msg::GearCommand::PARK; + + bool park_on_goal_; +}; +} // namespace autoware::shift_decider + +#endif // AUTOWARE__SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ diff --git a/control/autoware_shift_decider/launch/shift_decider.launch.xml b/control/autoware_shift_decider/launch/shift_decider.launch.xml new file mode 100644 index 0000000000000..d04af22ec828d --- /dev/null +++ b/control/autoware_shift_decider/launch/shift_decider.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/control/autoware_shift_decider/package.xml b/control/autoware_shift_decider/package.xml new file mode 100644 index 0000000000000..ebe86ddaac6d6 --- /dev/null +++ b/control/autoware_shift_decider/package.xml @@ -0,0 +1,30 @@ + + + + autoware_shift_decider + 0.1.0 + The autoware_shift_decider package + Takamasa Horibe + Takayuki Murooka + Apache License 2.0 + + Takamasa Horibe + + ament_cmake + autoware_cmake + + autoware_control_msgs + autoware_system_msgs + autoware_universe_utils + autoware_vehicle_msgs + rclcpp + rclcpp_components + + ament_cmake_cppcheck + ament_cmake_cpplint + ament_lint_auto + + + ament_cmake + + diff --git a/control/shift_decider/schema/shift_decider.schema.json b/control/autoware_shift_decider/schema/shift_decider.schema.json similarity index 100% rename from control/shift_decider/schema/shift_decider.schema.json rename to control/autoware_shift_decider/schema/shift_decider.schema.json diff --git a/control/autoware_shift_decider/src/autoware_shift_decider.cpp b/control/autoware_shift_decider/src/autoware_shift_decider.cpp new file mode 100644 index 0000000000000..846eaabee4b88 --- /dev/null +++ b/control/autoware_shift_decider/src/autoware_shift_decider.cpp @@ -0,0 +1,93 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/shift_decider/autoware_shift_decider.hpp" + +#include + +#include +#include + +namespace autoware::shift_decider +{ + +ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) +: Node("shift_decider", node_options) +{ + using std::placeholders::_1; + + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); + + park_on_goal_ = declare_parameter("park_on_goal"); + + pub_shift_cmd_ = + create_publisher("output/gear_cmd", durable_qos); + + initTimer(0.1); +} + +void ShiftDecider::onTimer() +{ + control_cmd_ = sub_control_cmd_.takeData(); + autoware_state_ = sub_autoware_state_.takeData(); + current_gear_ptr_ = sub_current_gear_.takeData(); + if (!autoware_state_ || !control_cmd_ || !current_gear_ptr_) { + return; + } + + updateCurrentShiftCmd(); + pub_shift_cmd_->publish(shift_cmd_); +} + +void ShiftDecider::updateCurrentShiftCmd() +{ + using autoware_system_msgs::msg::AutowareState; + using autoware_vehicle_msgs::msg::GearCommand; + + shift_cmd_.stamp = now(); + static constexpr double vel_threshold = 0.01; // to prevent chattering + if (autoware_state_->state == AutowareState::DRIVING) { + if (control_cmd_->longitudinal.velocity > vel_threshold) { + shift_cmd_.command = GearCommand::DRIVE; + } else if (control_cmd_->longitudinal.velocity < -vel_threshold) { + shift_cmd_.command = GearCommand::REVERSE; + } else { + shift_cmd_.command = prev_shift_command; + } + } else { + if ( + (autoware_state_->state == AutowareState::ARRIVED_GOAL || + autoware_state_->state == AutowareState::WAITING_FOR_ROUTE) && + park_on_goal_) { + shift_cmd_.command = GearCommand::PARK; + } else { + shift_cmd_.command = current_gear_ptr_->report; + } + } + prev_shift_command = shift_cmd_.command; +} + +void ShiftDecider::initTimer(double period_s) +{ + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = + rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&ShiftDecider::onTimer, this)); +} +} // namespace autoware::shift_decider + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::shift_decider::ShiftDecider) diff --git a/control/smart_mpc_trajectory_follower/.gitignore b/control/autoware_smart_mpc_trajectory_follower/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/.gitignore diff --git a/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt new file mode 100644 index 0000000000000..57e7b596790c2 --- /dev/null +++ b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_smart_mpc_trajectory_follower) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +install(PROGRAMS + autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py + DESTINATION lib/${PROJECT_NAME} +) +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/control/autoware_smart_mpc_trajectory_follower/README.md b/control/autoware_smart_mpc_trajectory_follower/README.md new file mode 100644 index 0000000000000..fd135319ff900 --- /dev/null +++ b/control/autoware_smart_mpc_trajectory_follower/README.md @@ -0,0 +1,338 @@ +

+ + + +

+ + +# Smart MPC Trajectory Follower + +Smart MPC (Model Predictive Control) is a control algorithm that combines model predictive control and machine learning. While inheriting the advantages of model predictive control, it solves its disadvantage of modeling difficulty with a data-driven method using machine learning. + +This technology makes it relatively easy to operate model predictive control, which is expensive to implement, as long as an environment for collecting data can be prepared. + +

+ + + +

+ +## Setup + +After building autoware, move to `control/autoware_smart_mpc_trajectory_follower` and run the following command: + +```bash +pip3 install . +``` + +If you have already installed and want to update the package, run the following command instead: + +```bash +pip3 install -U . +``` + +## Provided features + +This package provides smart MPC logic for path-following control as well as mechanisms for learning and evaluation. These features are described below. + +### Trajectory following control based on iLQR/MPPI + +The control mode can be selected from "ilqr", "mppi", or "mppi_ilqr", and can be set as `mpc_parameter:system:mode` in [mpc_param.yaml](./autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml). +In "mppi_ilqr" mode, the initial value of iLQR is given by the MPPI solution. + +> [!NOTE] +> With the default settings, the performance of "mppi" mode is limited due to an insufficient number of samples. This issue is being addressed with ongoing work to introduce GPU support. + +To perform a simulation, run the following command: + +```bash +ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit trajectory_follower_mode:=smart_mpc_trajectory_follower +``` + +> [!NOTE] +> When running with the nominal model set in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml), set `trained_model_parameter:control_application:use_trained_model` to `false` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). To run using the trained model, set `trained_model_parameter:control_application:use_trained_model` to `true`, but the trained model must have been generated according to the following procedure. + +### Training of model and reflection in control + +To obtain training data, start autoware, perform a drive, and record rosbag data with the following commands. + +```bash +ros2 bag record /localization/kinematic_state /localization/acceleration /vehicle/status/steering_status /control/command/control_cmd /control/trajectory_follower/control_cmd /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw /system/operation_mode/state /vehicle/status/control_mode /sensing/imu/imu_data /debug_mpc_x_des /debug_mpc_y_des /debug_mpc_v_des /debug_mpc_yaw_des /debug_mpc_acc_des /debug_mpc_steer_des /debug_mpc_X_des_converted /debug_mpc_x_current /debug_mpc_error_prediction /debug_mpc_max_trajectory_err /debug_mpc_emergency_stop_mode /debug_mpc_goal_stop_mode /debug_mpc_total_ctrl_time /debug_mpc_calc_u_opt_time +``` + +Move [rosbag2.bash](./autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash) to the rosbag directory recorded above and execute the following command on the directory + +```bash +bash rosbag2.bash +``` + +This converts rosbag data into CSV format for training models. + +> [!NOTE] +> Note that a large number of terminals are automatically opened at runtime, but they are automatically closed after rosbag data conversion is completed. +> From the time you begin this process until all terminals are closed, autoware should not be running. + +Instead, the same result can be obtained by executing the following command in a python environment: + +```python +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +model_trainer = train_drive_NN_model.train_drive_NN_model() +model_trainer.transform_rosbag_to_csv(rosbag_dir) +``` + +Here, `rosbag_dir` represents the rosbag directory. +At this time, all CSV files in `rosbag_dir` are automatically deleted first. + +The paths of the rosbag directories used for training, `dir_0`, `dir_1`, `dir_2`,... and the directory `save_dir` where you save the models, the model can be saved in the python environment as follows: + +```python +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +model_trainer = train_drive_NN_model.train_drive_NN_model() +model_trainer.add_data_from_csv(dir_0) +model_trainer.add_data_from_csv(dir_1) +model_trainer.add_data_from_csv(dir_2) +... +model_trainer.get_trained_model() +model_trainer.save_models(save_dir) +``` + +After performing the polynomial regression, the NN can be trained on the residuals as follows: + +```python +model_trainer.get_trained_model(use_polynomial_reg=True) +``` + +> [!NOTE] +> In the default setting, regression is performed by several preselected polynomials. +> When `use_selected_polynomial=False` is set as the argument of get_trained_model, the `deg` argument allows setting the maximum degree of the polynomial to be used. + +If only polynomial regression is performed and no NN model is used, run the following command: + +```python +model_trainer.get_trained_model(use_polynomial_reg=True,force_NN_model_to_zero=True) +``` + +Move `model_for_test_drive.pth` and `polynomial_reg_info.npz` saved in `save_dir` to the home directory and set `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) to `true` to reflect the trained model in the control. + +### Performance evaluation + +Here, as an example, we describe the verification of the adaptive performance when the wheel base of the sample_vehicle is 2.79 m, but an incorrect value of 2.0 m is given to the controller side. +To give the controller 2.0 m as the wheel base, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.0, and run the following command: + +```bash +python3 -m smart_mpc_trajectory_follower.clear_pycache +``` + +#### Test on autoware + +To perform a control test on autoware with the nominal model before training, make sure that `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) is `false` and launch autoware in the manner described in "Trajectory following control based on iLQR/MPPI". This time, the following route will be used for the test: + +

+ +Record rosbag and train the model in the manner described in "Training of model and reflection in control", and move the generated files `model_for_test_drive.pth` and `polynomial_reg_info.npz` to the home directory. + +> [!NOTE] +> Although the data used for training is small, for the sake of simplicity, we will see how much performance can be improved with this amount of data. + +To control using the trained model obtained here, set `trained_model_parameter:control_application:use_trained_model` to `true`, start autoware in the same way, and drive the same route recording rosbag. +After the driving is complete, convert the rosbag file to CSV format using the method described in "Training of model and reflection in control". +A plot of the lateral deviation is obtained by running the `lateral_error_visualize` function in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb` for the nominal and training model rosbag files `rosbag_nominal` and `rosbag_trained`, respectively, as follows: + +```python +lateral_error_visualize(dir_name=rosbag_nominal,ylim=[-1.2,1.2]) +lateral_error_visualize(dir_name=rosbag_trained,ylim=[-1.2,1.2]) +``` + +The following results were obtained. + +
+ + +
+ +#### Test on python simulator + +First, to give wheel base 2.79 m in the python simulator, create the following file and save it in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator` with the name `sim_setting.json`: + +```json +{ "wheel_base": 2.79 } +``` + +Next, run the following commands to test the slalom driving on the python simulator with the nominal model: + +```python +import python_simulator +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +initial_error = [0.0, 0.03, 0.01, -0.01, 0.0, 0.0] +save_dir = "test_python_sim" +python_simulator.slalom_drive(save_dir=save_dir,use_trained_model=False,initial_error=initial_error) +``` + +Here, `initial_error` is the initial error from the target trajectory, in the order of x-coordinate, y-coordinate, longitudinal velocity, yaw angle, longitudinal acceleration, and steer angle, +and `save_dir` is the directory where the driving test results are saved. + +> [!NOTE] +> The value of `use_trained_model` given as the argument of `python_simulator.slalom_drive` takes precedence over the value of `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). + +Run the following commands to perform training using driving data of the nominal model. + +```python +model_trainer = train_drive_NN_model.train_drive_NN_model() +model_trainer.add_data_from_csv(save_dir) +model_trainer.save_train_data(save_dir) +model_trainer.get_trained_model(use_polynomial_reg=True) +model_trainer.save_models(save_dir=save_dir) +``` + +This way, files `model_for_test_drive.pth` and `polynomial_reg_info.npz` are saved in `save_dir`. +The following results were obtained. + +

+ +

+ +The center of the upper row represents the lateral deviation. + +Finally, to drive with the training model, run the following commands: + +```python +load_dir = save_dir +save_dir = "test_python_trained_sim" +python_simulator.slalom_drive(save_dir=save_dir,load_dir=load_dir,use_trained_model=True,initial_error=initial_error) +``` + +The following results were obtained. + +

+ +

+ +It can be seen that the lateral deviation has improved significantly. + +Here we have described wheel base, but the parameters that can be passed to the python simulator are as follows. + +| Parameter | Type | Description | +| ------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| steer_bias | float | steer bias [rad] | +| steer_rate_lim | float | steer rate limit [rad/s] | +| vel_rate_lim | float | acceleration limit [m/s^2] | +| wheel_base | float | wheel base [m] | +| steer_dead_band | float | steer dead band [rad] | +| adaptive_gear_ratio_coef | list[float] | List of floats of length 6 specifying information on speed-dependent gear ratios from tire angle to steering wheel angle. | +| acc_time_delay | float | acceleration time delay [s] | +| steer_time_delay | float | steer time delay [s] | +| acc_time_constant | float | acceleration time constant [s] | +| steer_time_constant | float | steer time constant [s] | +| accel_map_scale | float | Parameter that magnifies the corresponding distortion from acceleration input values to actual acceleration realizations.
Correspondence information is kept in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv`. | +| acc_scaling | float | acceleration scaling | +| steer_scaling | float | steer scaling | + +For example, to give the simulation side 0.01 [rad] of steer bias and 0.001 [rad] of steer dead band, edit the `sim_setting.json` as follows. + +```json +{ "steer_bias": 0.01, "steer_dead_band": 0.001 } +``` + +#### Auto test on python simulator + +Here, we describe a method for testing adaptive performance by giving the simulation side a predefined range of model parameters while the control side is given constant model parameters. + +First, to restore nominal model settings to default values, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.79, and run the following command: + +```bash +python3 -m smart_mpc_trajectory_follower.clear_pycache +``` + +To run a driving experiment within the parameter change range set in [run_sim.py](./autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py), for example, move to `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator` and run the following command: + +```bash +python3 run_sim.py --param_name steer_bias +``` + +Here we described the experimental procedure for steer bias, and the same method can be used for other parameters. + +If you want to do it for all parameters at once, run the following command: + +```bash +yes | python3 run_sim.py +``` + +In `run_sim.py`, the following parameters can be set: + +| Parameter | Type | Description | +| ------------------------- | ---- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| USE_TRAINED_MODEL_DIFF | bool | Whether the derivative of the trained model is reflected in the control | +| DATA_COLLECTION_MODE | str | Which method will be used to collect the training data　
"ff": Straight line driving with feed-forward input
"pp": Figure eight driving with pure pursuit control
"mpc": Slalom driving with mpc | +| USE_POLYNOMIAL_REGRESSION | bool | Whether to perform polynomial regression before NN | +| USE_SELECTED_POLYNOMIAL | bool | When USE_POLYNOMIAL_REGRESSION is True, perform polynomial regression using only some preselected polynomials.
The choice of polynomials is intended to be able to absorb the contribution of some parameter shifts based on the nominal model of the vehicle. | +| FORCE_NN_MODEL_TO_ZERO | bool | Whether to force the NN model to zero (i.e., erase the contribution of the NN model).
When USE_POLYNOMIAL_REGRESSION is True, setting FORCE_MODEL_TO_ZERO to True allows the control to reflect the results of polynomial regression only, without using NN models. | +| FIT_INTERCEPT | bool | Whether to include bias in polynomial regression.
If it is False, perform the regression with a polynomial of the first degree or higher. | +| USE_INTERCEPT | bool | When a polynomial regression including bias is performed, whether to use or discard the resulting bias information.
It is meaningful only if FIT_INTERCEPT is True.
If it is False, discard the bias in the polynomial regression in the hope that the NN model can remove the bias term, even if the polynomial regression is performed with the bias included. | + +> [!NOTE] +> When `run_sim.py` is run, the `use_trained_model_diff` set in `run_sim.py` takes precedence over the `trained_model_parameter:control_application:use_trained_model_diff` set in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). + +## Change of nominal parameters and their reloading + +The nominal parameters of vehicle model can be changed by editing the file [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml). +After changing the nominal parameters, the cache must be deleted by running the following command: + +```bash +python3 -m smart_mpc_trajectory_follower.clear_pycache +``` + +The nominal parameters include the following: + +| Parameter | Type | Description | +| ------------------------------------------------ | ----- | ------------------------------ | +| nominal_parameter:vehicle_info:wheel_base | float | wheel base [m] | +| nominal_parameter:acceleration:acc_time_delay | float | acceleration time delay [s] | +| nominal_parameter:acceleration:acc_time_constant | float | acceleration time constant [s] | +| nominal_parameter:steering:steer_time_delay | float | steer time delay [s] | +| nominal_parameter:steering:steer_time_constant | float | steer time constant [s] | + +## Change of control parameters and their reloading + +The control parameters can be changed by editing files [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml) and [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). +Although it is possible to reflect parameter changes by restarting autoware, the following command allows us to do so without leaving autoware running: + +```bash +ros2 topic pub /pympc_reload_mpc_param_trigger std_msgs/msg/String "data: ''" --once +``` + +The main parameters among the control parameters are as follows. + +### `mpc_param.yaml` + +| Parameter | Type | Description | +| ------------------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| mpc_parameter:system:mode | str | control mode
"ilqr": iLQR mode
"mppi": MPPI mode
"mppi_ilqr": the initial value of iLQR is given by the MPPI solution. | +| mpc_parameter:cost_parameters:Q | list[float] | Stage cost for states.
List of length 8, in order: straight deviation, lateral deviation, velocity deviation, yaw angle deviation, acceleration deviation, steer deviation, acceleration input deviation, steer input deviation cost weights. | +| mpc_parameter:cost_parameters:Q_c | list[float] | Cost in the horizon corresponding to the following timing_Q_c for the states.
The correspondence of the components of the list is the same as for Q. | +| mpc_parameter:cost_parameters:Q_f | list[float] | Termination cost for the states.
The correspondence of the components of the list is the same as for Q. | +| mpc_parameter:cost_parameters:R | list[float] | A list of length 2 where R[0] is weight of cost for the change rate of acceleration input value and R[1] is weight of cost for the change rate of steer input value. | +| mpc_parameter:mpc_setting:timing_Q_c | list[int] | Horizon numbers such that the stage cost for the states is set to Q_c. | + +### `trained_model_param.yaml` + +| Parameter | Type | Description | +| ------------------------------------------------------------------ | ---- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| trained_model_parameter:control_application:use_trained_model | bool | Whether the trained model is reflected in the control or not. | +| trained_model_parameter:control_application:use_trained_model_diff | bool | Whether the derivative of the trained model is reflected on the control or not.
It is meaningful only when use_trained_model is True, and if False, the nominal model is used for the derivative of the dynamics, and trained model is used only for prediction. | + +## Request to release the slow stop mode + +If the predicted trajectory deviates too far from the target trajectory, the system enters a slow stop mode and the vehicle stops moving. +To cancel the slow stop mode and make the vehicle ready to run again, run the following command: + +```bash +ros2 topic pub /pympc_stop_mode_reset_request std_msgs/msg/String "data: ''" --once +``` + +## Limitation + +- May not be able to start when initial position/posture is far from the target. + +- It may take some time until the end of the planning to compile numba functions at the start of the first control. + +- In the stopping action near the goal our control switches to another simple control law. As a result, the stopping action may not work except near the goal. Stopping is also difficult if the acceleration map is significantly shifted. diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py similarity index 88% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py index fafcb5bdecf05..2458de3a80f99 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py @@ -16,10 +16,10 @@ from pathlib import Path import shutil -import smart_mpc_trajectory_follower +import autoware_smart_mpc_trajectory_follower if __name__ == "__main__": - package_dir = str(Path(smart_mpc_trajectory_follower.__file__).parent) + package_dir = str(Path(autoware_smart_mpc_trajectory_follower.__file__).parent) remove_dirs = [ package_dir + "/__pycache__", diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/mpc_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/mpc_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/nominal_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/nominal_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/trained_model_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/trained_model_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py index d706c342f04ec..0eaa6ce1c0fbf 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py @@ -18,14 +18,14 @@ import datetime import os +from autoware_smart_mpc_trajectory_follower.scripts import drive_controller +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions import matplotlib.pyplot as plt from numba import njit import numpy as np import pandas as pd import scipy.interpolate import simplejson as json -from smart_mpc_trajectory_follower.scripts import drive_controller -from smart_mpc_trajectory_follower.scripts import drive_functions print("\n\n### import python_simulator.py ###") diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py similarity index 92% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py index 6b477bb804ac9..68d8a9f64e73f 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model import numpy as np import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model initial_error = np.array( [0.001, 0.03, 0.01, -0.001, 0, 2 * python_simulator.measurement_steer_bias] diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py index 3125719556559..d2bb4ea26f0a6 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py @@ -23,9 +23,9 @@ import traceback from typing import Dict +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model import numpy as np import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model parser = argparse.ArgumentParser() parser.add_argument("--param_name", default=None) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py index e75eedcb4bf65..4d36afd3d4324 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py @@ -16,9 +16,9 @@ from functools import partial +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions sqrt_mpc_time_step = np.sqrt(drive_functions.mpc_time_step) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py index f35789ee39fd3..f13aa07b0f795 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py @@ -13,9 +13,9 @@ # limitations under the License. +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.scripts import proxima_calc import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.scripts import proxima_calc import torch from torch import nn diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py index b21fcf72da931..3877f7177720b 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py @@ -21,14 +21,14 @@ import threading import time +from autoware_smart_mpc_trajectory_follower.scripts import drive_GP +from autoware_smart_mpc_trajectory_follower.scripts import drive_NN +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.scripts import drive_iLQR +from autoware_smart_mpc_trajectory_follower.scripts import drive_mppi import numpy as np import scipy.interpolate # type: ignore from sklearn.preprocessing import PolynomialFeatures -from smart_mpc_trajectory_follower.scripts import drive_GP -from smart_mpc_trajectory_follower.scripts import drive_NN -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.scripts import drive_iLQR -from smart_mpc_trajectory_follower.scripts import drive_mppi import torch ctrl_index_for_polynomial_reg = np.concatenate( @@ -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/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py index c11b5c886b49a..fb96e134f7746 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py @@ -31,7 +31,9 @@ with open(package_path_json, "r") as file: package_path = json.load(file) -mpc_param_path = package_path["path"] + "/smart_mpc_trajectory_follower/param/mpc_param.yaml" +mpc_param_path = ( + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml" +) with open(mpc_param_path, "r") as yml: mpc_param = yaml.safe_load(yml) @@ -101,7 +103,7 @@ cap_pred_error = np.array(mpc_param["mpc_parameter"]["preprocessing"]["cap_pred_error"]) nominal_param_path = ( - package_path["path"] + "/smart_mpc_trajectory_follower/param/nominal_param.yaml" + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml" ) with open(nominal_param_path, "r") as yml: nominal_param = yaml.safe_load(yml) @@ -172,7 +174,7 @@ mpc_param["mpc_parameter"]["preprocessing"]["sg_window_size_for_nominal_inputs"] ) trained_model_param_path = ( - package_path["path"] + "/smart_mpc_trajectory_follower/param/trained_model_param.yaml" + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml" ) with open(trained_model_param_path, "r") as yml: trained_model_param = yaml.safe_load(yml) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py index 261a72e680778..2307586f23552 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py @@ -21,9 +21,9 @@ import time from typing import Callable +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions index_cost = np.concatenate( ( diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py index ca6e6f15f42ef..5e88c79172097 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py @@ -16,9 +16,9 @@ from typing import Callable +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions index_cost = np.concatenate( ( diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/proxima_calc.cpp b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/proxima_calc.cpp rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index 409b6bf594c9e..78a120fe601f2 100755 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -19,10 +19,12 @@ import time from autoware_adapi_v1_msgs.msg import OperationModeState -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_planning_msgs.msg import Trajectory -from autoware_auto_planning_msgs.msg import TrajectoryPoint -from autoware_auto_vehicle_msgs.msg import SteeringReport +from autoware_control_msgs.msg import Control +from autoware_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import TrajectoryPoint +from autoware_smart_mpc_trajectory_follower.scripts import drive_controller +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_vehicle_msgs.msg import SteeringReport from builtin_interfaces.msg import Duration from geometry_msgs.msg import AccelWithCovarianceStamped from geometry_msgs.msg import PoseStamped @@ -34,8 +36,6 @@ import scipy.interpolate from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Slerp -from smart_mpc_trajectory_follower.scripts import drive_controller -from smart_mpc_trajectory_follower.scripts import drive_functions from std_msgs.msg import String from tier4_debug_msgs.msg import BoolStamped from tier4_debug_msgs.msg import Float32MultiArrayStamped @@ -128,7 +128,7 @@ def __init__(self): self.sub_reload_mpc_param_trigger_ self.sub_control_command_control_cmd_ = self.create_subscription( - AckermannControlCommand, + Control, "/control/command/control_cmd", self.onControlCommandControlCmd, 3, @@ -136,7 +136,7 @@ def __init__(self): self.sub_control_command_control_cmd_ self.control_cmd_pub_ = self.create_publisher( - AckermannControlCommand, + Control, "/control/trajectory_follower/control_cmd", 1, ) @@ -699,7 +699,7 @@ def control(self): else: steer_cmd = 0.0 - cmd_msg = AckermannControlCommand() + cmd_msg = Control() cmd_msg.stamp = cmd_msg.lateral.stamp = cmd_msg.longitudinal.stamp = ( self.get_clock().now().to_msg() ) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py index 3f6ef7a9f78d1..5338f3cbaddce 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py @@ -23,11 +23,11 @@ import os from pathlib import Path +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions import numpy as np import scipy.interpolate from scipy.ndimage import gaussian_filter from scipy.spatial.transform import Rotation -from smart_mpc_trajectory_follower.scripts import drive_functions def data_smoothing(data: np.ndarray, sigma: float) -> np.ndarray: @@ -76,7 +76,7 @@ def transform_rosbag_to_csv(self, dir_name: str, delete_csv_first: bool = True) os.system( "cp " + package_path["path"] - + "/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash " + + "/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash " + dir_name ) os.chdir(dir_name) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash similarity index 88% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash index 524dfe5a169df..d2687b352a17d 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash @@ -2,16 +2,16 @@ gnome-terminal -- bash -c 'ros2 topic echo /localization/kinematic_state nav_msgs/msg/Odometry --csv --qos-history keep_all --qos-reliability reliable > kinematic_state.csv' gnome-terminal -- bash -c 'ros2 topic echo /localization/acceleration geometry_msgs/msg/AccelWithCovarianceStamped --csv --qos-history keep_all --qos-reliability reliable > acceleration.csv' -gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/steering_status autoware_auto_vehicle_msgs/msg/SteeringReport --csv --qos-history keep_all --qos-reliability reliable > steering_status.csv' +gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/steering_status autoware_vehicle_msgs/msg/SteeringReport --csv --qos-history keep_all --qos-reliability reliable > steering_status.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/command/control_cmd autoware_auto_control_msgs/msg/AckermannControlCommand --csv --qos-history keep_all --qos-reliability reliable > control_cmd.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/control_cmd autoware_auto_control_msgs/msg/AckermannControlCommand --csv --qos-history keep_all --qos-reliability reliable > control_cmd_orig.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/command/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd_orig.csv' gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > lateral_error.csv' gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > yaw_error.csv' gnome-terminal -- bash -c 'ros2 topic echo /system/operation_mode/state autoware_adapi_v1_msgs/msg/OperationModeState --csv --qos-history keep_all --qos-reliability reliable > system_operation_mode_state.csv' -gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/control_mode autoware_auto_vehicle_msgs/msg/ControlModeReport --csv --qos-history keep_all --qos-reliability reliable > vehicle_status_control_mode.csv' +gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/control_mode autoware_vehicle_msgs/msg/ControlModeReport --csv --qos-history keep_all --qos-reliability reliable > vehicle_status_control_mode.csv' gnome-terminal -- bash -c 'ros2 topic echo /sensing/imu/imu_data sensor_msgs/msg/Imu --csv --qos-history keep_all --qos-reliability reliable > imu.csv' gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_des.csv' diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py index 269b577f83c25..449ddda45f460 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py @@ -15,13 +15,15 @@ # cspell: ignore optim savez suptitle """Class for training neural nets from driving data.""" +from autoware_smart_mpc_trajectory_follower.scripts import drive_NN +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.training_and_data_check import ( + add_training_data_from_csv, +) import matplotlib.pyplot as plt import numpy as np from sklearn import linear_model from sklearn.preprocessing import PolynomialFeatures -from smart_mpc_trajectory_follower.scripts import drive_NN -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.training_and_data_check import add_training_data_from_csv import torch from torch import nn from torch.utils.data import DataLoader diff --git a/control/smart_mpc_trajectory_follower/images/autoware_smart_mpc.png b/control/autoware_smart_mpc_trajectory_follower/images/autoware_smart_mpc.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/autoware_smart_mpc.png rename to control/autoware_smart_mpc_trajectory_follower/images/autoware_smart_mpc.png diff --git a/control/smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png diff --git a/control/smart_mpc_trajectory_follower/images/lateral_error_trained_model.png b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/lateral_error_trained_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png diff --git a/control/smart_mpc_trajectory_follower/images/proxima_logo.png b/control/autoware_smart_mpc_trajectory_follower/images/proxima_logo.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/proxima_logo.png rename to control/autoware_smart_mpc_trajectory_follower/images/proxima_logo.png diff --git a/control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png diff --git a/control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png diff --git a/control/smart_mpc_trajectory_follower/images/test_route.png b/control/autoware_smart_mpc_trajectory_follower/images/test_route.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/test_route.png rename to control/autoware_smart_mpc_trajectory_follower/images/test_route.png diff --git a/control/autoware_smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml new file mode 100644 index 0000000000000..01b9478aeeaee --- /dev/null +++ b/control/autoware_smart_mpc_trajectory_follower/package.xml @@ -0,0 +1,46 @@ + + + + autoware_smart_mpc_trajectory_follower + 1.0.0 + Nodes to follow a trajectory by generating control commands using smart mpc + + + Masayuki Aino + Kosuke Takeuchi + Takamasa Horibe + Takayuki Murooka + + Apache License 2.0 + + Masayuki Aino + + ament_cmake_auto + ament_cmake_python + autoware_cmake + + autoware_adapi_v1_msgs + autoware_control_msgs + autoware_motion_utils + autoware_planning_msgs + autoware_system_msgs + autoware_universe_utils + autoware_vehicle_msgs + pybind11_vendor + python3-scipy + rclcpp + rclcpp_components + + ros2launch + + ament_cmake_ros + ament_index_python + ament_lint_auto + autoware_lint_common + autoware_testing + ros_testing + + + ament_cmake + + diff --git a/control/autoware_smart_mpc_trajectory_follower/setup.py b/control/autoware_smart_mpc_trajectory_follower/setup.py new file mode 100644 index 0000000000000..fee1d04e826c0 --- /dev/null +++ b/control/autoware_smart_mpc_trajectory_follower/setup.py @@ -0,0 +1,37 @@ +# cspell: ignore numba +import glob +import json +import os +from pathlib import Path + +from setuptools import find_packages +from setuptools import setup + +os.system("pip3 install numba==0.58.1 --force-reinstall") +os.system("pip3 install pybind11") +os.system("pip3 install GPy") +os.system("pip3 install torch") +package_path = {} +package_path["path"] = str(Path(__file__).parent) +with open("autoware_smart_mpc_trajectory_follower/package_path.json", "w") as f: + json.dump(package_path, f) +build_cpp_command = "g++ -Ofast -Wall -shared -std=c++11 -fPIC $(python3 -m pybind11 --includes) " +build_cpp_command += "autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp " +build_cpp_command += "-o autoware_smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) " +build_cpp_command += "-lrt -I/usr/include/eigen3" +os.system(build_cpp_command) + +so_path = ( + "scripts/" + + glob.glob("autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.*.so")[0].split("/")[ + -1 + ] +) +setup( + name="smart_mpc_trajectory_follower", + version="1.0.0", + packages=find_packages(), + package_data={ + "autoware_smart_mpc_trajectory_follower": ["package_path.json", so_path], + }, +) diff --git a/control/autoware_trajectory_follower_base/CMakeLists.txt b/control/autoware_trajectory_follower_base/CMakeLists.txt new file mode 100644 index 0000000000000..74916a3d2e22f --- /dev/null +++ b/control/autoware_trajectory_follower_base/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_trajectory_follower_base) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/longitudinal_controller_base.cpp + src/lateral_controller_base.cpp +) + +ament_auto_package() diff --git a/control/autoware_trajectory_follower_base/README.md b/control/autoware_trajectory_follower_base/README.md new file mode 100644 index 0000000000000..3c79784f4b95b --- /dev/null +++ b/control/autoware_trajectory_follower_base/README.md @@ -0,0 +1,53 @@ +# Trajectory Follower + +This is the design document for the `trajectory_follower` package. + +## Purpose / Use cases + + + + +This package provides the interface of longitudinal and lateral controllers used by the node of the `autoware_trajectory_follower_node` package. +We can implement a detailed controller by deriving the longitudinal and lateral base interfaces. + +## Design + +There are lateral and longitudinal base interface classes and each algorithm inherits from this class to implement. +The interface class has the following base functions. + +- `isReady()`: Check if the control is ready to compute. +- `run()`: Compute control commands and return to [Trajectory Follower Nodes](../autoware_trajectory_follower_node/README.md). This must be implemented by inherited algorithms. +- `sync()`: Input the result of running the other controller. + - steer angle convergence + - allow keeping stopped until steer is converged. + - velocity convergence(currently not used) + +See [the Design of Trajectory Follower Nodes](../autoware_trajectory_follower_node/README.md#Design) for how these functions work in the node. + +## Separated lateral (steering) and longitudinal (velocity) controls + +This longitudinal controller assumes that the roles of lateral and longitudinal control are separated as follows. + +- Lateral control computes a target steering to keep the vehicle on the trajectory, assuming perfect velocity tracking. +- Longitudinal control computes a target velocity/acceleration to keep the vehicle velocity on the trajectory speed, assuming perfect trajectory tracking. + +Ideally, dealing with the lateral and longitudinal control as a single mixed problem can achieve high performance. In contrast, there are two reasons to provide velocity controller as a stand-alone function, described below. + +### Complex requirements for longitudinal motion + +The longitudinal vehicle behavior that humans expect is difficult to express in a single logic. For example, the expected behavior just before stopping differs depending on whether the ego-position is ahead/behind of the stop line, or whether the current speed is higher/lower than the target speed to achieve a human-like movement. + +In addition, some vehicles have difficulty measuring the ego-speed at extremely low speeds. In such cases, a configuration that can improve the functionality of the longitudinal control without affecting the lateral control is important. + +There are many characteristics and needs that are unique to longitudinal control. Designing them separately from the lateral control keeps the modules less coupled and improves maintainability. + +### Nonlinear coupling of lateral and longitudinal motion + +The lat-lon mixed control problem is very complex and uses nonlinear optimization to achieve high performance. Since it is difficult to guarantee the convergence of the nonlinear optimization, a simple control logic is also necessary for development. + +Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed. + +## Related issues + + 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 new file mode 100644 index 0000000000000..25ff7c21c38c0 --- /dev/null +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/input_data.hpp @@ -0,0 +1,36 @@ +// Copyright 2022 The Autoware Foundation +// +// 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__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" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" + +namespace autoware::motion::control::trajectory_follower +{ +struct InputData +{ + autoware_planning_msgs::msg::Trajectory current_trajectory; + nav_msgs::msg::Odometry current_odometry; + autoware_vehicle_msgs::msg::SteeringReport current_steering; + geometry_msgs::msg::AccelWithCovarianceStamped current_accel; + autoware_adapi_v1_msgs::msg::OperationModeState current_operation_mode; +}; +} // namespace autoware::motion::control::trajectory_follower + +#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 new file mode 100644 index 0000000000000..891b3982ddf49 --- /dev/null +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/lateral_controller_base.hpp @@ -0,0 +1,46 @@ +// Copyright 2022 The Autoware Foundation +// +// 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__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 "rclcpp/rclcpp.hpp" + +#include "autoware_control_msgs/msg/lateral.hpp" + +#include +namespace autoware::motion::control::trajectory_follower +{ +struct LateralOutput +{ + autoware_control_msgs::msg::Lateral control_cmd; + LateralSyncData sync_data; +}; + +class LateralControllerBase +{ +public: + virtual bool isReady(const InputData & input_data) = 0; + virtual LateralOutput run(InputData const & input_data) = 0; + void sync(LongitudinalSyncData const & longitudinal_sync_data); + +protected: + LongitudinalSyncData longitudinal_sync_data_; +}; + +} // namespace autoware::motion::control::trajectory_follower + +#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 new file mode 100644 index 0000000000000..176141572d6a8 --- /dev/null +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/longitudinal_controller_base.hpp @@ -0,0 +1,49 @@ +// Copyright 2022 The Autoware Foundation +// +// 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__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 "rclcpp/rclcpp.hpp" + +#include "autoware_control_msgs/msg/longitudinal.hpp" + +#include + +namespace autoware::motion::control::trajectory_follower +{ +struct LongitudinalOutput +{ + autoware_control_msgs::msg::Longitudinal control_cmd; + LongitudinalSyncData sync_data; +}; +class LongitudinalControllerBase +{ +public: + virtual bool isReady(const InputData & input_data) = 0; + virtual LongitudinalOutput run(InputData const & input_data) = 0; + void sync(LateralSyncData const & lateral_sync_data); + // NOTE: This reset function should be called when the trajectory is replanned by changing ego + // pose or goal pose. + void reset(); + +protected: + LateralSyncData lateral_sync_data_; +}; + +} // namespace autoware::motion::control::trajectory_follower + +#endif // AUTOWARE__TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower_base/include/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/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp rename to control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/sync_data.hpp index 60c91019d10c3..494423c7f01ac 100644 --- a/control/trajectory_follower_base/include/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 TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ -#define 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 // 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 new file mode 100644 index 0000000000000..7eb71d6d0d9aa --- /dev/null +++ b/control/autoware_trajectory_follower_base/include/autoware/trajectory_follower_base/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 The Autoware Foundation +// +// 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__TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) +#if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport) +#define TRAJECTORY_FOLLOWER_LOCAL +#else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport) +#define TRAJECTORY_FOLLOWER_LOCAL +#endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#elif defined(__linux__) +#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) +#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) +#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) +#else +#error "Unsupported Build Configuration" +#endif + +#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 new file mode 100644 index 0000000000000..fa8f5847d6c31 --- /dev/null +++ b/control/autoware_trajectory_follower_base/package.xml @@ -0,0 +1,50 @@ + + + + autoware_trajectory_follower_base + 1.0.0 + Library for generating lateral and longitudinal controls following a trajectory + + + Takamasa Horibe + + Takayuki Murooka + + Apache 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + 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 + diagnostic_updater + eigen + geometry_msgs + interpolation + osqp_interface + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros + tier4_debug_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/trajectory_follower_base/src/lateral_controller_base.cpp b/control/autoware_trajectory_follower_base/src/lateral_controller_base.cpp similarity index 92% rename from control/trajectory_follower_base/src/lateral_controller_base.cpp rename to control/autoware_trajectory_follower_base/src/lateral_controller_base.cpp index 6acdbc4a8f5eb..f5839fb4bc944 100644 --- a/control/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 "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/trajectory_follower_base/src/longitudinal_controller_base.cpp b/control/autoware_trajectory_follower_base/src/longitudinal_controller_base.cpp similarity index 92% rename from control/trajectory_follower_base/src/longitudinal_controller_base.cpp rename to control/autoware_trajectory_follower_base/src/longitudinal_controller_base.cpp index f7779158f5791..a8303fc82235e 100644 --- a/control/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 "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 new file mode 100644 index 0000000000000..945d3224d76e5 --- /dev/null +++ b/control/autoware_trajectory_follower_node/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_trajectory_follower_node) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(CONTROLLER_NODE controller_node) +ament_auto_add_library(${CONTROLLER_NODE} SHARED + include/autoware/trajectory_follower_node/controller_node.hpp + src/controller_node.cpp +) + +rclcpp_components_register_node(${CONTROLLER_NODE} + PLUGIN "autoware::motion::control::trajectory_follower_node::Controller" + EXECUTABLE ${CONTROLLER_NODE}_exe +) + +# 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 + src/simple_trajectory_follower.cpp +) + +rclcpp_components_register_node(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} + PLUGIN "simple_trajectory_follower::SimpleTrajectoryFollower" + EXECUTABLE ${SIMPLE_TRAJECTORY_FOLLOWER_NODE}_exe +) + +if(BUILD_TESTING) + set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_node) + ament_add_ros_isolated_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} + test/trajectory_follower_test_utils.hpp + test/test_controller_node.cpp + ) + ament_target_dependencies(${TRAJECTORY_FOLLOWER_NODES_TEST} fake_test_node) + target_link_libraries( + ${TRAJECTORY_FOLLOWER_NODES_TEST} ${CONTROLLER_NODE}) + + find_package(autoware_testing REQUIRED) + # smoke test for MPC controller + add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe + PARAM_FILENAMES "lateral/mpc.param.yaml longitudinal/pid.param.yaml +trajectory_follower_node.param.yaml" + TEST_PARAM_FILENAMES "test_controller_mpc.param.yaml test_controller_pid.param.yaml +test_vehicle_info.param.yaml test_nearest_search.param.yaml" + TARGET_INFIX "mpc" + ) + # smoke test for pure pursuit controller + add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe + PARAM_FILENAMES "lateral/pure_pursuit.param.yaml longitudinal/pid.param.yaml +trajectory_follower_node.param.yaml" + TEST_PARAM_FILENAMES "test_controller_pure_pursuit.param.yaml test_controller_pid.param.yaml +test_vehicle_info.param.yaml test_nearest_search.param.yaml" + TARGET_INFIX "pure_pursuit" + ) + +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param + test + launch + config +) diff --git a/control/autoware_trajectory_follower_node/README.md b/control/autoware_trajectory_follower_node/README.md new file mode 100644 index 0000000000000..aa692c323f6d0 --- /dev/null +++ b/control/autoware_trajectory_follower_node/README.md @@ -0,0 +1,156 @@ +# Trajectory Follower Nodes + +## Purpose + +Generate control commands to follow a given Trajectory. + +## Design + +This is a node of the functionalities implemented in the controller class derived from [autoware_trajectory_follower_base](../autoware_trajectory_follower_base/README.md#trajectory-follower) package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands. + +By default, the controller instance with the `Controller` class as follows is used. + +```plantuml +@startuml +package autoware_trajectory_follower_base { +abstract class LateralControllerBase { +longitudinal_sync_data_ + + virtual isReady(InputData) + virtual run(InputData) + sync(LongitudinalSyncData) + reset() + +} +abstract class LongitudinalControllerBase { +lateral_sync_data_ + + virtual isReady(InputData) + virtual run(InputData) + sync(LateralSyncData) + reset() + +} + +struct InputData { +trajectory +odometry +steering +accel +} +struct LongitudinalSyncData { +is_steer_converged +} +struct LateralSyncData { +} +} + +package autoware_mpc_lateral_controller { +class MPCLateralController { +isReady(InputData) override +run(InputData) override +} +} +package pure_pursuit { +class PurePursuitLateralController { +isReady(InputData) override +run(InputData) override +} +} +package pid_longitudinal_controller { +class PIDLongitudinalController { +isReady(InputData) override +run(InputData) override +} +} + +package autoware_trajectory_follower_node { +class Controller { +longitudinal_controller_ +lateral_controller_ +onTimer() +createInputData(): InputData +} +} + +MPCLateralController --|> LateralControllerBase +PurePursuitLateralController --|> LateralControllerBase +PIDLongitudinalController --|> LongitudinalControllerBase + +LateralSyncData --> LongitudinalControllerBase +LateralSyncData --> LateralControllerBase +LongitudinalSyncData --> LongitudinalControllerBase +LongitudinalSyncData --> LateralControllerBase +InputData ..> LateralControllerBase +InputData ..> LongitudinalControllerBase + +LateralControllerBase --o Controller +LongitudinalControllerBase --o Controller +InputData ..> Controller +@enduml +``` + +The process flow of `Controller` class is as follows. + +```cpp +// 1. create input data +const auto input_data = createInputData(*get_clock()); +if (!input_data) { + return; +} + +// 2. check if controllers are ready +const bool is_lat_ready = lateral_controller_->isReady(*input_data); +const bool is_lon_ready = longitudinal_controller_->isReady(*input_data); +if (!is_lat_ready || !is_lon_ready) { + return; +} + +// 3. run controllers +const auto lat_out = lateral_controller_->run(*input_data); +const auto lon_out = longitudinal_controller_->run(*input_data); + +// 4. sync with each other controllers +longitudinal_controller_->sync(lat_out.sync_data); +lateral_controller_->sync(lon_out.sync_data); + +// 5. publish control command +control_cmd_pub_->publish(out); +``` + +Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true` + +- lateral controller + - `keep_steer_control_until_converged` +- longitudinal controller + - `enable_keep_stopped_until_steer_convergence` + +### Inputs / Outputs / API + +#### Inputs + +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry +- `autoware_vehicle_msgs/SteeringReport` current steering + +#### Outputs + +- `autoware_control_msgs/Control`: message containing both lateral and longitudinal commands. + +#### Parameter + +- `ctrl_period`: control commands publishing period +- `timeout_thr_sec`: duration in second after which input messages are discarded. + - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `Control` if the following two conditions are met. + 1. Both commands have been received. + 2. The last received commands are not older than defined by `timeout_thr_sec`. +- `lateral_controller_mode`: `mpc` or `pure_pursuit` + - (currently there is only `PID` for longitudinal controller) + +## Debugging + +Debug information are published by the lateral and longitudinal controller using `tier4_debug_msgs/Float32MultiArrayStamped` messages. + +A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging. + +In addition, the predicted MPC trajectory is published on topic `output/lateral/predicted_trajectory` and can be visualized in Rviz. diff --git a/control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml similarity index 100% rename from control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml rename to control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml diff --git a/control/trajectory_follower_node/config/simple_trajectory_follower.param.yaml b/control/autoware_trajectory_follower_node/config/simple_trajectory_follower.param.yaml similarity index 100% rename from control/trajectory_follower_node/config/simple_trajectory_follower.param.yaml rename to control/autoware_trajectory_follower_node/config/simple_trajectory_follower.param.yaml diff --git a/control/trajectory_follower_node/design/media/Controller.drawio.svg b/control/autoware_trajectory_follower_node/design/media/Controller.drawio.svg similarity index 98% rename from control/trajectory_follower_node/design/media/Controller.drawio.svg rename to control/autoware_trajectory_follower_node/design/media/Controller.drawio.svg index 1152ef2b17f59..a09758690ccdc 100644 --- a/control/trajectory_follower_node/design/media/Controller.drawio.svg +++ b/control/autoware_trajectory_follower_node/design/media/Controller.drawio.svg @@ -83,12 +83,12 @@
- trajectory_follower_node/controller_node + autoware_trajectory_follower_node/controller_node
- trajectory_follower_node/controller_node + autoware_trajectory_follower_node/controller_node @@ -140,7 +140,7 @@
trajectory_follower/
- mpc_lateral_controller + autoware_mpc_lateral_controller
diff --git a/control/trajectory_follower_node/design/simple_trajectory_follower-design.md b/control/autoware_trajectory_follower_node/design/simple_trajectory_follower-design.md similarity index 84% rename from control/trajectory_follower_node/design/simple_trajectory_follower-design.md rename to control/autoware_trajectory_follower_node/design/simple_trajectory_follower-design.md index 24ffe3166bbe4..4c1ee1b422484 100644 --- a/control/trajectory_follower_node/design/simple_trajectory_follower-design.md +++ b/control/autoware_trajectory_follower_node/design/simple_trajectory_follower-design.md @@ -10,10 +10,10 @@ Provide a base trajectory follower code that is simple and flexible to use. This Inputs -- `input/reference_trajectory` [autoware_auto_planning_msgs::msg::Trajectory] : reference trajectory to follow. +- `input/reference_trajectory` [autoware_planning_msgs::msg::Trajectory] : reference trajectory to follow. - `input/current_kinematic_state` [nav_msgs::msg::Odometry] : current state of the vehicle (position, velocity, etc). - Output -- `output/control_cmd` [autoware_auto_control_msgs::msg::AckermannControlCommand] : generated control command. +- `output/control_cmd` [autoware_control_msgs::msg::Control] : generated control command. ### Parameters 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 new file mode 100644 index 0000000000000..916f921a40e7b --- /dev/null +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp @@ -0,0 +1,146 @@ +// Copyright 2021 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 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 +#include +#include + +#include "autoware_control_msgs/msg/control.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include + +#include +#include +#include +#include + +namespace autoware::motion::control +{ +using trajectory_follower::LateralOutput; +using trajectory_follower::LongitudinalOutput; +namespace trajectory_follower_node +{ + +using autoware::universe_utils::StopWatch; +using autoware_adapi_v1_msgs::msg::OperationModeState; +using tier4_debug_msgs::msg::Float64Stamped; + +namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; + +/// \classController +/// \brief The node class used for generating longitudinal control commands (velocity/acceleration) +class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node +{ +public: + explicit Controller(const rclcpp::NodeOptions & node_options); + virtual ~Controller() {} + +private: + rclcpp::TimerBase::SharedPtr timer_control_; + double timeout_thr_sec_; + boost::optional longitudinal_output_{boost::none}; + + std::shared_ptr longitudinal_controller_; + std::shared_ptr lateral_controller_; + + // Subscribers + autoware::universe_utils::InterProcessPollingSubscriber + sub_ref_path_{this, "~/input/reference_trajectory"}; + + autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ + this, "~/input/current_odometry"}; + + autoware::universe_utils::InterProcessPollingSubscriber< + autoware_vehicle_msgs::msg::SteeringReport> + sub_steering_{this, "~/input/current_steering"}; + + autoware::universe_utils::InterProcessPollingSubscriber< + geometry_msgs::msg::AccelWithCovarianceStamped> + sub_accel_{this, "~/input/current_accel"}; + + autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ + this, "~/input/current_operation_mode"}; + + // Publishers + rclcpp::Publisher::SharedPtr control_cmd_pub_; + rclcpp::Publisher::SharedPtr pub_processing_time_lat_ms_; + rclcpp::Publisher::SharedPtr pub_processing_time_lon_ms_; + rclcpp::Publisher::SharedPtr debug_marker_pub_; + + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr current_trajectory_ptr_; + nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr_; + autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr current_steering_ptr_; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_accel_ptr_; + OperationModeState::ConstSharedPtr current_operation_mode_ptr_; + + enum class LateralControllerMode { + INVALID = 0, + MPC = 1, + PURE_PURSUIT = 2, + }; + enum class LongitudinalControllerMode { + INVALID = 0, + PID = 1, + }; + + /** + * @brief compute control command, and publish periodically + */ + boost::optional createInputData(rclcpp::Clock & clock); + void callbackTimerControl(); + bool processData(rclcpp::Clock & clock); + bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out); + LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; + LongitudinalControllerMode getLongitudinalControllerMode( + const std::string & algorithm_name) const; + void publishDebugMarker( + const trajectory_follower::InputData & input_data, + const trajectory_follower::LateralOutput & lat_out) const; + + std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; + + void publishProcessingTime( + const double t_ms, const rclcpp::Publisher::SharedPtr pub); + StopWatch stop_watch_; + + static constexpr double logger_throttle_interval = 5000; +}; +} // namespace trajectory_follower_node +} // namespace autoware::motion::control + +#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 new file mode 100644 index 0000000000000..c23128ebfb695 --- /dev/null +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/simple_trajectory_follower.hpp @@ -0,0 +1,70 @@ +// Copyright 2022 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 AUTOWARE__TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ + +#include "autoware/universe_utils/ros/polling_subscriber.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace simple_trajectory_follower +{ +using autoware_control_msgs::msg::Control; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; + +class SimpleTrajectoryFollower : public rclcpp::Node +{ +public: + explicit SimpleTrajectoryFollower(const rclcpp::NodeOptions & options); + ~SimpleTrajectoryFollower() = default; + +private: + autoware::universe_utils::InterProcessPollingSubscriber sub_kinematics_{ + this, "~/input/kinematics"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_trajectory_{ + this, "~/input/trajectory"}; + rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::TimerBase::SharedPtr timer_; + + Trajectory::ConstSharedPtr trajectory_; + Odometry::ConstSharedPtr odometry_; + TrajectoryPoint closest_traj_point_; + bool use_external_target_vel_; + double external_target_vel_; + double lateral_deviation_; + + void onTimer(); + bool processData(); + void updateClosest(); + double calcSteerCmd(); + double calcAccCmd(); +}; + +} // namespace simple_trajectory_follower + +#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 new file mode 100644 index 0000000000000..c38c2ab03629c --- /dev/null +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 The Autoware Foundation +// +// 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__TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) +#if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport) +#define TRAJECTORY_FOLLOWER_LOCAL +#else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport) +#define TRAJECTORY_FOLLOWER_LOCAL +#endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#elif defined(__linux__) +#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) +#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) +#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) +#else +#error "Unsupported Build Configuration" +#endif + +#endif // AUTOWARE__TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ diff --git a/control/autoware_trajectory_follower_node/launch/simple_trajectory_follower.launch.xml b/control/autoware_trajectory_follower_node/launch/simple_trajectory_follower.launch.xml new file mode 100644 index 0000000000000..521638dd7a54d --- /dev/null +++ b/control/autoware_trajectory_follower_node/launch/simple_trajectory_follower.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/control/autoware_trajectory_follower_node/package.xml b/control/autoware_trajectory_follower_node/package.xml new file mode 100644 index 0000000000000..55ac989ad4a58 --- /dev/null +++ b/control/autoware_trajectory_follower_node/package.xml @@ -0,0 +1,50 @@ + + + + autoware_trajectory_follower_node + 1.0.0 + Nodes to follow a trajectory by generating control commands separated into lateral and longitudinal commands + + + Takamasa Horibe + + Takayuki Murooka + + Apache License 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + 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 + rclcpp + rclcpp_components + visualization_msgs + + ros2launch + + ament_cmake_ros + ament_index_python + ament_lint_auto + autoware_lint_common + autoware_testing + fake_test_node + ros_testing + + + ament_cmake + + diff --git a/control/trajectory_follower_node/param/lateral/mpc.param.yaml b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml similarity index 100% rename from control/trajectory_follower_node/param/lateral/mpc.param.yaml rename to control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml diff --git a/control/trajectory_follower_node/param/lateral/pure_pursuit.param.yaml b/control/autoware_trajectory_follower_node/param/lateral/pure_pursuit.param.yaml similarity index 100% rename from control/trajectory_follower_node/param/lateral/pure_pursuit.param.yaml rename to control/autoware_trajectory_follower_node/param/lateral/pure_pursuit.param.yaml diff --git a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml similarity index 92% rename from control/trajectory_follower_node/param/longitudinal/pid.param.yaml rename to control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml index c39088753fe64..ad6217663fbae 100644 --- a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -69,8 +69,9 @@ max_jerk: 2.0 min_jerk: -5.0 - # pitch - use_trajectory_for_pitch_calculation: false + # slope compensation lpf_pitch_gain: 0.95 + slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive + adaptive_trajectory_velocity_th: 1.0 max_pitch_rad: 0.1 min_pitch_rad: -0.1 diff --git a/control/trajectory_follower_node/param/trajectory_follower_node.param.yaml b/control/autoware_trajectory_follower_node/param/trajectory_follower_node.param.yaml similarity index 100% rename from control/trajectory_follower_node/param/trajectory_follower_node.param.yaml rename to control/autoware_trajectory_follower_node/param/trajectory_follower_node.param.yaml diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp new file mode 100644 index 0000000000000..60f7481f862a9 --- /dev/null +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -0,0 +1,258 @@ +// Copyright 2021 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 "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 "autoware/universe_utils/ros/marker_helper.hpp" + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion::control::trajectory_follower_node +{ +Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("controller", node_options) +{ + using std::placeholders::_1; + + const double ctrl_period = declare_parameter("ctrl_period"); + timeout_thr_sec_ = declare_parameter("timeout_thr_sec"); + + const auto lateral_controller_mode = + getLateralControllerMode(declare_parameter("lateral_controller_mode")); + switch (lateral_controller_mode) { + case LateralControllerMode::MPC: { + lateral_controller_ = std::make_shared(*this); + break; + } + case LateralControllerMode::PURE_PURSUIT: { + lateral_controller_ = + std::make_shared(*this); + break; + } + default: + throw std::domain_error("[LateralController] invalid algorithm"); + } + + const auto longitudinal_controller_mode = + getLongitudinalControllerMode(declare_parameter("longitudinal_controller_mode")); + switch (longitudinal_controller_mode) { + case LongitudinalControllerMode::PID: { + longitudinal_controller_ = + std::make_shared(*this); + break; + } + default: + throw std::domain_error("[LongitudinalController] invalid algorithm"); + } + + control_cmd_pub_ = create_publisher( + "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); + pub_processing_time_lat_ms_ = + create_publisher("~/lateral/debug/processing_time_ms", 1); + pub_processing_time_lon_ms_ = + create_publisher("~/longitudinal/debug/processing_time_ms", 1); + debug_marker_pub_ = + create_publisher("~/output/debug_marker", rclcpp::QoS{1}); + + // Timer + { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(ctrl_period)); + timer_control_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); + } + + logger_configure_ = std::make_unique(this); + + published_time_publisher_ = + std::make_unique(this); +} + +Controller::LateralControllerMode Controller::getLateralControllerMode( + const std::string & controller_mode) const +{ + if (controller_mode == "mpc") return LateralControllerMode::MPC; + if (controller_mode == "pure_pursuit") return LateralControllerMode::PURE_PURSUIT; + + return LateralControllerMode::INVALID; +} + +Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode( + const std::string & controller_mode) const +{ + if (controller_mode == "pid") return LongitudinalControllerMode::PID; + + return LongitudinalControllerMode::INVALID; +} + +bool Controller::processData(rclcpp::Clock & clock) +{ + bool is_ready = true; + + const auto & logData = [&clock, this](const std::string & data_type) { + std::string msg = "Waiting for " + data_type + " data"; + RCLCPP_INFO_THROTTLE(get_logger(), clock, logger_throttle_interval, msg.c_str()); + }; + + const auto & getData = [&logData](auto & dest, auto & sub, const std::string & data_type = "") { + const auto temp = sub.takeData(); + if (temp) { + dest = temp; + return true; + } + if (!data_type.empty()) logData(data_type); + return false; + }; + + is_ready &= getData(current_accel_ptr_, sub_accel_, "acceleration"); + is_ready &= getData(current_steering_ptr_, sub_steering_, "steering"); + is_ready &= getData(current_trajectory_ptr_, sub_ref_path_, "trajectory"); + is_ready &= getData(current_odometry_ptr_, sub_odometry_, "odometry"); + is_ready &= getData(current_operation_mode_ptr_, sub_operation_mode_, "operation mode"); + + return is_ready; +} + +bool Controller::isTimeOut( + const trajectory_follower::LongitudinalOutput & lon_out, + const trajectory_follower::LateralOutput & lat_out) +{ + const auto now = this->now(); + if ((now - lat_out.control_cmd.stamp).seconds() > timeout_thr_sec_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000 /*ms*/, + "Lateral control command too old, control_cmd will not be published."); + return true; + } + if ((now - lon_out.control_cmd.stamp).seconds() > timeout_thr_sec_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000 /*ms*/, + "Longitudinal control command too old, control_cmd will not be published."); + return true; + } + return false; +} + +boost::optional Controller::createInputData(rclcpp::Clock & clock) +{ + if (!processData(clock)) { + return {}; + } + + trajectory_follower::InputData input_data; + input_data.current_trajectory = *current_trajectory_ptr_; + input_data.current_odometry = *current_odometry_ptr_; + input_data.current_steering = *current_steering_ptr_; + input_data.current_accel = *current_accel_ptr_; + input_data.current_operation_mode = *current_operation_mode_ptr_; + + return input_data; +} + +void Controller::callbackTimerControl() +{ + // 1. create input data + const auto input_data = createInputData(*get_clock()); + if (!input_data) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, "Control is skipped since input data is not ready."); + return; + } + + // 2. check if controllers are ready + const bool is_lat_ready = lateral_controller_->isReady(*input_data); + const bool is_lon_ready = longitudinal_controller_->isReady(*input_data); + if (!is_lat_ready || !is_lon_ready) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Control is skipped since lateral and/or longitudinal controllers are not ready to run."); + return; + } + + // 3. run controllers + stop_watch_.tic("lateral"); + const auto lat_out = lateral_controller_->run(*input_data); + publishProcessingTime(stop_watch_.toc("lateral"), pub_processing_time_lat_ms_); + + stop_watch_.tic("longitudinal"); + const auto lon_out = longitudinal_controller_->run(*input_data); + publishProcessingTime(stop_watch_.toc("longitudinal"), pub_processing_time_lon_ms_); + + // 4. sync with each other controllers + longitudinal_controller_->sync(lat_out.sync_data); + lateral_controller_->sync(lon_out.sync_data); + + // TODO(Horibe): Think specification. This comes from the old implementation. + if (isTimeOut(lon_out, lat_out)) return; + + // 5. publish control command + autoware_control_msgs::msg::Control out; + out.stamp = this->now(); + out.lateral = lat_out.control_cmd; + out.longitudinal = lon_out.control_cmd; + control_cmd_pub_->publish(out); + + // 6. publish debug + published_time_publisher_->publish_if_subscribed(control_cmd_pub_, out.stamp); + publishDebugMarker(*input_data, lat_out); +} + +void Controller::publishDebugMarker( + const trajectory_follower::InputData & input_data, + const trajectory_follower::LateralOutput & lat_out) const +{ + visualization_msgs::msg::MarkerArray debug_marker_array{}; + + // steer converged marker + { + auto marker = autoware::universe_utils::createDefaultMarker( + "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + 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; + const double current = input_data.current_steering.steering_tire_angle; + const double cmd = lat_out.control_cmd.steering_tire_angle; + const double diff = current - cmd; + ss << "current:" << current << " cmd:" << cmd << " diff:" << diff + << (lat_out.sync_data.is_steer_converged ? " (converged)" : " (not converged)"); + marker.text = ss.str(); + + debug_marker_array.markers.push_back(marker); + } + + debug_marker_pub_->publish(debug_marker_array); +} + +void Controller::publishProcessingTime( + const double t_ms, const rclcpp::Publisher::SharedPtr pub) +{ + Float64Stamped msg{}; + msg.stamp = this->now(); + msg.data = t_ms; + pub->publish(msg); +} + +} // namespace autoware::motion::control::trajectory_follower_node + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::trajectory_follower_node::Controller) diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp similarity index 77% rename from control/trajectory_follower_node/src/simple_trajectory_follower.cpp rename to control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp index 5e9488bf2ca93..95082e608d50f 100644 --- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -12,29 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "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) { - pub_cmd_ = create_publisher("output/control_cmd", 1); - - sub_kinematics_ = create_subscription( - "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); - sub_trajectory_ = create_subscription( - "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; }); + pub_cmd_ = create_publisher("output/control_cmd", 1); use_external_target_vel_ = declare_parameter("use_external_target_vel"); external_target_vel_ = declare_parameter("external_target_vel"); @@ -47,18 +42,19 @@ SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & o void SimpleTrajectoryFollower::onTimer() { - if (!checkData()) { + if (!processData()) { RCLCPP_INFO(get_logger(), "data not ready"); return; } updateClosest(); - AckermannControlCommand cmd; + Control cmd; cmd.stamp = cmd.lateral.stamp = cmd.longitudinal.stamp = get_clock()->now(); cmd.lateral.steering_tire_angle = static_cast(calcSteerCmd()); - cmd.longitudinal.speed = use_external_target_vel_ ? static_cast(external_target_vel_) - : closest_traj_point_.longitudinal_velocity_mps; + cmd.longitudinal.velocity = use_external_target_vel_ + ? static_cast(external_target_vel_) + : closest_traj_point_.longitudinal_velocity_mps; cmd.longitudinal.acceleration = static_cast(calcAccCmd()); pub_cmd_->publish(cmd); } @@ -109,9 +105,18 @@ double SimpleTrajectoryFollower::calcAccCmd() return acc; } -bool SimpleTrajectoryFollower::checkData() +bool SimpleTrajectoryFollower::processData() { - return (trajectory_ && odometry_); + bool is_ready = true; + const auto & getData = [](auto & dest, auto & sub) { + const auto temp = sub.takeData(); + if (!temp) return false; + dest = temp; + return true; + }; + is_ready &= getData(odometry_, sub_kinematics_); + is_ready &= getData(trajectory_, sub_trajectory_); + return is_ready; } } // namespace simple_trajectory_follower diff --git a/control/trajectory_follower_node/test/test_controller_mpc.param.yaml b/control/autoware_trajectory_follower_node/test/test_controller_mpc.param.yaml similarity index 100% rename from control/trajectory_follower_node/test/test_controller_mpc.param.yaml rename to control/autoware_trajectory_follower_node/test/test_controller_mpc.param.yaml diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp similarity index 90% rename from control/trajectory_follower_node/test/test_controller_node.cpp rename to control/autoware_trajectory_follower_node/test/test_controller_node.cpp index 1aa4035e98d51..e0dc5ede906a9 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp @@ -13,18 +13,17 @@ // limitations under the License. #include "ament_index_cpp/get_package_share_directory.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" #include "rclcpp/time.hpp" -#include "trajectory_follower_node/controller_node.hpp" #include "trajectory_follower_test_utils.hpp" #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/control.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -35,11 +34,11 @@ #include using Controller = autoware::motion::control::trajectory_follower_node::Controller; -using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; -using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; +using Control = autoware_control_msgs::msg::Control; +using Trajectory = autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; using VehicleOdometry = nav_msgs::msg::Odometry; -using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; +using SteeringReport = autoware_vehicle_msgs::msg::SteeringReport; using autoware_adapi_v1_msgs::msg::OperationModeState; using geometry_msgs::msg::AccelWithCovarianceStamped; @@ -50,11 +49,12 @@ const rclcpp::Duration one_second(1, 0); rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_convergence = false) { // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_trajectory_follower_node"); const auto longitudinal_share_dir = - ament_index_cpp::get_package_share_directory("pid_longitudinal_controller"); + ament_index_cpp::get_package_share_directory("autoware_pid_longitudinal_controller"); const auto lateral_share_dir = - ament_index_cpp::get_package_share_directory("mpc_lateral_controller"); + ament_index_cpp::get_package_share_directory("autoware_mpc_lateral_controller"); rclcpp::NodeOptions node_options; node_options.append_parameter_override("lateral_controller_mode", "mpc"); node_options.append_parameter_override("longitudinal_controller_mode", "pid"); @@ -96,7 +96,7 @@ class ControllerTester FakeNodeFixture * fnf; std::shared_ptr node; - AckermannControlCommand::SharedPtr cmd_msg; + Control::SharedPtr cmd_msg; bool received_control_command = false; void publish_default_odom() @@ -178,13 +178,11 @@ class ControllerTester rclcpp::Publisher::SharedPtr operation_mode_pub = fnf->create_publisher("controller/input/current_operation_mode"); - rclcpp::Subscription::SharedPtr cmd_sub = - fnf->create_subscription( - "controller/output/control_cmd", *fnf->get_fake_node(), - [this](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); + rclcpp::Subscription::SharedPtr cmd_sub = fnf->create_subscription( + "controller/output/control_cmd", *fnf->get_fake_node(), [this](const Control::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); std::shared_ptr br = std::make_shared(fnf->get_fake_node()); @@ -255,7 +253,7 @@ TEST_F(FakeNodeFixture, straight_trajectory) // following conditions will pass even if the MPC solution does not converge EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, 0.0f); EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } @@ -369,14 +367,14 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0); EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); // Generate another control message tester.traj_pub->publish(traj_msg); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0); EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); } @@ -406,14 +404,14 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message tester.traj_pub->publish(traj); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -443,14 +441,14 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message tester.traj_pub->publish(traj); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -477,7 +475,7 @@ TEST_F(FakeNodeFixture, longitudinal_stopped) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_LT( tester.cmd_msg->longitudinal.acceleration, 0.0f); // when stopped negative acceleration to brake @@ -507,7 +505,7 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -535,7 +533,7 @@ TEST_F(FakeNodeFixture, longitudinal_emergency) ASSERT_TRUE(tester.received_control_command); // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -566,7 +564,7 @@ TEST_F(FakeNodeFixture, longitudinal_not_check_steer_converged) ASSERT_TRUE(tester.received_control_command); // Not keep stopped state when the lateral control is not converged. - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0f); } TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) @@ -597,5 +595,5 @@ TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) ASSERT_TRUE(tester.received_control_command); // Keep stopped state when the lateral control is not converged. - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); } diff --git a/control/trajectory_follower_node/test/test_controller_pid.param.yaml b/control/autoware_trajectory_follower_node/test/test_controller_pid.param.yaml similarity index 100% rename from control/trajectory_follower_node/test/test_controller_pid.param.yaml rename to control/autoware_trajectory_follower_node/test/test_controller_pid.param.yaml diff --git a/control/trajectory_follower_node/test/test_controller_pure_pursuit.param.yaml b/control/autoware_trajectory_follower_node/test/test_controller_pure_pursuit.param.yaml similarity index 100% rename from control/trajectory_follower_node/test/test_controller_pure_pursuit.param.yaml rename to control/autoware_trajectory_follower_node/test/test_controller_pure_pursuit.param.yaml diff --git a/planning/planning_test_utils/config/test_nearest_search.param.yaml b/control/autoware_trajectory_follower_node/test/test_nearest_search.param.yaml similarity index 100% rename from planning/planning_test_utils/config/test_nearest_search.param.yaml rename to control/autoware_trajectory_follower_node/test/test_nearest_search.param.yaml diff --git a/planning/planning_test_utils/config/test_vehicle_info.param.yaml b/control/autoware_trajectory_follower_node/test/test_vehicle_info.param.yaml similarity index 100% rename from planning/planning_test_utils/config/test_vehicle_info.param.yaml rename to control/autoware_trajectory_follower_node/test/test_vehicle_info.param.yaml diff --git a/control/trajectory_follower_node/test/trajectory_follower_test_utils.hpp b/control/autoware_trajectory_follower_node/test/trajectory_follower_test_utils.hpp similarity index 100% rename from control/trajectory_follower_node/test/trajectory_follower_test_utils.hpp rename to control/autoware_trajectory_follower_node/test/trajectory_follower_test_utils.hpp diff --git a/control/autoware_vehicle_cmd_gate/CMakeLists.txt b/control/autoware_vehicle_cmd_gate/CMakeLists.txt new file mode 100644 index 0000000000000..7233608a5d25f --- /dev/null +++ b/control/autoware_vehicle_cmd_gate/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_vehicle_cmd_gate) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(vehicle_cmd_gate_node SHARED + src/vehicle_cmd_gate.cpp + src/vehicle_cmd_filter.cpp + src/adapi_pause_interface.cpp + src/moderate_stop_interface.cpp +) + +rclcpp_components_register_node(vehicle_cmd_gate_node + PLUGIN "autoware::vehicle_cmd_gate::VehicleCmdGate" + EXECUTABLE vehicle_cmd_gate_exe +) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/IsFilterActivated.msg" + DEPENDENCIES builtin_interfaces +) + +# to use same package defined message +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(vehicle_cmd_gate_node + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(vehicle_cmd_gate_node "${cpp_typesupport_target}") +endif() + + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_vehicle_cmd_gate + test/src/test_main.cpp + test/src/test_vehicle_cmd_filter.cpp + test/src/test_filter_in_vehicle_cmd_gate_node.cpp + ) + ament_target_dependencies(test_vehicle_cmd_gate + rclcpp + tier4_control_msgs + ) + target_link_libraries(test_vehicle_cmd_gate + vehicle_cmd_gate_node + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_vehicle_cmd_gate/README.md b/control/autoware_vehicle_cmd_gate/README.md new file mode 100644 index 0000000000000..2d6f5c5f949af --- /dev/null +++ b/control/autoware_vehicle_cmd_gate/README.md @@ -0,0 +1,85 @@ +# vehicle_cmd_gate + +## Purpose + +`vehicle_cmd_gate` is the package to get information from emergency handler, planning module, external controller, and send a msg to vehicle. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------------------- | --------------------------------------------------- | -------------------------------------------------------------------- | +| `~/input/steering` | `autoware_vehicle_msgs::msg::SteeringReport` | steering status | +| `~/input/auto/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from planning module | +| `~/input/auto/turn_indicators_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from planning module | +| `~/input/auto/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from planning module | +| `~/input/auto/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command from planning module | +| `~/input/external/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from external | +| `~/input/external/turn_indicators_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from external | +| `~/input/external/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from external | +| `~/input/external/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command from external | +| `~/input/external_emergency_stop_heartbeat` | `tier4_external_api_msgs::msg::Heartbeat` | heartbeat | +| `~/input/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | +| `~/input/emergency/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from emergency handler | +| `~/input/emergency/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | +| `~/input/emergency/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | +| `~/input/engage` | `autoware_vehicle_msgs::msg::Engage` | engage signal | +| `~/input/operation_mode` | `autoware_adapi_v1_msgs::msg::OperationModeState` | operation mode of Autoware | + +### Output + +| Name | Type | Description | +| -------------------------------------- | --------------------------------------------------- | -------------------------------------------------------- | +| `~/output/vehicle_cmd_emergency` | `tier4_vehicle_msgs::msg::VehicleEmergencyStamped` | emergency state which was originally in vehicle command | +| `~/output/command/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity to vehicle | +| `~/output/command/turn_indicators_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command to vehicle | +| `~/output/command/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command to vehicle | +| `~/output/command/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command to vehicle | +| `~/output/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | +| `~/output/engage` | `autoware_vehicle_msgs::msg::Engage` | engage signal | +| `~/output/external_emergency` | `tier4_external_api_msgs::msg::Emergency` | external emergency signal | +| `~/output/operation_mode` | `tier4_system_msgs::msg::OperationMode` | current operation mode of the vehicle_cmd_gate | + +## Parameters + +| Parameter | Type | Description | +| ------------------------------------------- | -------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `update_period` | double | update period | +| `use_emergency_handling` | bool | true when emergency handler is used | +| `check_external_emergency_heartbeat` | bool | true when checking heartbeat for emergency stop | +| `system_emergency_heartbeat_timeout` | double | timeout for system emergency | +| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency | +| `filter_activated_count_threshold` | int | threshold for filter activation | +| `filter_activated_velocity_threshold` | double | velocity threshold for filter activation | +| `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop | +| `emergency_acceleration` | double | longitudinal acceleration cmd when vehicle stop with emergency | +| `moderate_stop_service_acceleration` | double | longitudinal acceleration cmd when vehicle stop with moderate stop service | +| `nominal.vel_lim` | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode) | +| `nominal.reference_speed_point` | | velocity point used as a reference when calculate control command limit (activated in AUTONOMOUS operation mode). The size of this array must be equivalent to the size of the limit array. | +| `nominal.lon_acc_lim` | | array of limits of longitudinal acceleration (activated in AUTONOMOUS operation mode) | +| `nominal.lon_jerk_lim` | | array of limits of longitudinal jerk (activated in AUTONOMOUS operation mode) | +| `nominal.lat_acc_lim` | | array of limits of lateral acceleration (activated in AUTONOMOUS operation mode) | +| `nominal.lat_jerk_lim` | | array of limits of lateral jerk (activated in AUTONOMOUS operation mode) | +| `on_transition.vel_lim` | double | limit of longitudinal velocity (activated in TRANSITION operation mode) | +| `on_transition.reference_speed_point` | | velocity point used as a reference when calculate control command limit (activated in TRANSITION operation mode). The size of this array must be equivalent to the size of the limit array. | +| `on_transition.lon_acc_lim` | | array of limits of longitudinal acceleration (activated in TRANSITION operation mode) | +| `on_transition.lon_jerk_lim` | | array of limits of longitudinal jerk (activated in TRANSITION operation mode) | +| `on_transition.lat_acc_lim` | | array of limits of lateral acceleration (activated in TRANSITION operation mode) | +| `on_transition.lat_jerk_lim` | | array of limits of lateral jerk (activated in TRANSITION operation mode) | + +## Filter function + +This module incorporates a limitation filter to the control command right before its published. Primarily for safety, this filter restricts the output range of all control commands published through Autoware. + +The limitation values are calculated based on the 1D interpolation of the limitation array parameters. Here is an example for the longitudinal jerk limit. + +![filter-example](./image/filter.png) + +Notation: this filter is not designed to enhance ride comfort. Its main purpose is to detect and remove abnormal values in the control outputs during the final stages of Autoware. If this filter is frequently active, it implies the control module may need tuning. If you're aiming to smoothen the signal via a low-pass filter or similar techniques, that should be handled in the control module. When the filter is activated, the topic `~/is_filter_activated` is published. + +## Assumptions / Known limits + +The parameter `check_external_emergency_heartbeat` (true by default) enables an emergency stop request from external modules. +This feature requires a `~/input/external_emergency_stop_heartbeat` topic for health monitoring of the external module, and the vehicle_cmd_gate module will not start without the topic. +The `check_external_emergency_heartbeat` parameter must be false when the "external emergency stop" function is not used. diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml similarity index 100% rename from control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml rename to control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml diff --git a/control/vehicle_cmd_gate/image/filter.png b/control/autoware_vehicle_cmd_gate/image/filter.png similarity index 100% rename from control/vehicle_cmd_gate/image/filter.png rename to control/autoware_vehicle_cmd_gate/image/filter.png diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml similarity index 89% rename from control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml rename to control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index c5368276b488a..c9df4e67c2647 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -1,11 +1,11 @@ - + - + - + diff --git a/control/vehicle_cmd_gate/msg/IsFilterActivated.msg b/control/autoware_vehicle_cmd_gate/msg/IsFilterActivated.msg similarity index 100% rename from control/vehicle_cmd_gate/msg/IsFilterActivated.msg rename to control/autoware_vehicle_cmd_gate/msg/IsFilterActivated.msg diff --git a/control/autoware_vehicle_cmd_gate/package.xml b/control/autoware_vehicle_cmd_gate/package.xml new file mode 100644 index 0000000000000..60bbf23f9d5a8 --- /dev/null +++ b/control/autoware_vehicle_cmd_gate/package.xml @@ -0,0 +1,50 @@ + + + + autoware_vehicle_cmd_gate + 0.1.0 + The vehicle_cmd_gate package + Takamasa Horibe + Tomoya Kimura + Apache License 2.0 + + Hiroki OTA + + ament_cmake + autoware_cmake + + rosidl_default_generators + + 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 + rclcpp + rclcpp_components + std_srvs + tier4_api_utils + tier4_control_msgs + tier4_debug_msgs + tier4_external_api_msgs + tier4_system_msgs + tier4_vehicle_msgs + visualization_msgs + + rosidl_default_runtime + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json b/control/autoware_vehicle_cmd_gate/schema/vehicle_cmd_gate.json similarity index 100% rename from control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json rename to control/autoware_vehicle_cmd_gate/schema/vehicle_cmd_gate.json diff --git a/control/vehicle_cmd_gate/script/delays_checker.py b/control/autoware_vehicle_cmd_gate/script/delays_checker.py similarity index 96% rename from control/vehicle_cmd_gate/script/delays_checker.py rename to control/autoware_vehicle_cmd_gate/script/delays_checker.py index 564b7c7ac94e5..a67c794877a92 100644 --- a/control/vehicle_cmd_gate/script/delays_checker.py +++ b/control/autoware_vehicle_cmd_gate/script/delays_checker.py @@ -14,8 +14,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_vehicle_msgs.msg import Engage +from autoware_control_msgs.msg import Control +from autoware_vehicle_msgs.msg import Engage from geometry_msgs.msg import AccelWithCovarianceStamped from nav_msgs.msg import Odometry import rclpy @@ -68,13 +68,13 @@ def __init__(self): ) self.sub_engage = self.create_subscription(Engage, engage_topic, self.CallBackEngage, 1) self.sub_in_gate_cmd = self.create_subscription( - AckermannControlCommand, + Control, in_gate_cmd_topic, self.CallBackInCmd, 1, ) self.sub_out_gate_cmd = self.create_subscription( - AckermannControlCommand, + Control, out_gate_cmd_topic, self.CallBackOutCmd, 1, diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp similarity index 89% rename from control/vehicle_cmd_gate/src/adapi_pause_interface.cpp rename to control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp index afc86f0b4fb20..f14a62fba9661 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp +++ b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp @@ -14,7 +14,7 @@ #include "adapi_pause_interface.hpp" -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { AdapiPauseInterface::AdapiPauseInterface(rclcpp::Node * node) : node_(node) @@ -53,9 +53,9 @@ void AdapiPauseInterface::publish() } } -void AdapiPauseInterface::update(const AckermannControlCommand & control) +void AdapiPauseInterface::update(const Control & control) { - is_start_requested_ = eps < std::abs(control.longitudinal.speed); + is_start_requested_ = eps < std::abs(control.longitudinal.velocity); } void AdapiPauseInterface::on_pause( @@ -65,4 +65,4 @@ void AdapiPauseInterface::on_pause( res->status.success = true; } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp similarity index 85% rename from control/vehicle_cmd_gate/src/adapi_pause_interface.hpp rename to control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp index c4294ee5f643d..d75b544752e3f 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp +++ b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp @@ -19,16 +19,16 @@ #include #include -#include +#include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { class AdapiPauseInterface { private: static constexpr double eps = 1e-3; - using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; + using Control = autoware_control_msgs::msg::Control; using SetPause = control_interface::SetPause; using IsPaused = control_interface::IsPaused; using IsStartRequested = control_interface::IsStartRequested; @@ -37,7 +37,7 @@ class AdapiPauseInterface explicit AdapiPauseInterface(rclcpp::Node * node); bool is_paused(); void publish(); - void update(const AckermannControlCommand & control); + void update(const Control & control); private: bool is_paused_; @@ -55,6 +55,6 @@ class AdapiPauseInterface const SetPause::Service::Response::SharedPtr res); }; -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #endif // ADAPI_PAUSE_INTERFACE_HPP_ diff --git a/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp b/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp new file mode 100644 index 0000000000000..c9d7e86fa23dd --- /dev/null +++ b/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp @@ -0,0 +1,119 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MARKER_HELPER_HPP_ +#define MARKER_HELPER_HPP_ + +#include + +#include + +#include + +namespace autoware::vehicle_cmd_gate +{ +inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) +{ + geometry_msgs::msg::Point point; + + point.x = x; + point.y = y; + point.z = z; + + return point; +} + +inline geometry_msgs::msg::Quaternion createMarkerOrientation( + double x, double y, double z, double w) +{ + geometry_msgs::msg::Quaternion quaternion; + + quaternion.x = x; + quaternion.y = y; + quaternion.z = z; + quaternion.w = w; + + return quaternion; +} + +inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + + scale.x = x; + scale.y = y; + scale.z = z; + + return scale; +} + +inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + + color.r = r; + color.g = g; + color.b = b; + color.a = a; + + return color; +} + +inline visualization_msgs::msg::Marker createMarker( + const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type, + geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = frame_id; + marker.header.stamp = rclcpp::Time(0); + marker.ns = ns; + marker.id = id; + marker.type = type; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.pose.position = point; + marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); + marker.scale = scale; + marker.color = color; + marker.frame_locked = false; + + return marker; +} + +inline visualization_msgs::msg::Marker createStringMarker( + const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type, + geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale, + const std_msgs::msg::ColorRGBA & color, const std::string text) +{ + visualization_msgs::msg::Marker marker; + + marker = createMarker(frame_id, ns, id, type, point, scale, color); + marker.text = text; + + return marker; +} + +inline void appendMarkerArray( + const visualization_msgs::msg::MarkerArray & additional_marker_array, + visualization_msgs::msg::MarkerArray * marker_array) +{ + for (const auto & marker : additional_marker_array.markers) { + marker_array->markers.push_back(marker); + } +} +} // namespace autoware::vehicle_cmd_gate + +#endif // MARKER_HELPER_HPP_ diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.cpp b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.cpp similarity index 95% rename from control/vehicle_cmd_gate/src/moderate_stop_interface.cpp rename to control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.cpp index 72dbdfd26a6b3..0fe90adfd0a72 100644 --- a/control/vehicle_cmd_gate/src/moderate_stop_interface.cpp +++ b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.cpp @@ -14,7 +14,7 @@ #include "moderate_stop_interface.hpp" -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { ModerateStopInterface::ModerateStopInterface(rclcpp::Node * node) : node_(node) @@ -66,4 +66,4 @@ void ModerateStopInterface::update_stop_state() } } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp similarity index 92% rename from control/vehicle_cmd_gate/src/moderate_stop_interface.hpp rename to control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp index 7ef30b4926b7e..012e75d0c207e 100644 --- a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp +++ b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp @@ -19,12 +19,10 @@ #include #include -#include - #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { class ModerateStopInterface @@ -55,6 +53,6 @@ class ModerateStopInterface void update_stop_state(); }; -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #endif // MODERATE_STOP_INTERFACE_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp similarity index 86% rename from control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp rename to control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index b289365b46b3b..cec23b05b8191 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -17,7 +17,7 @@ #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { VehicleCmdFilter::VehicleCmdFilter() @@ -95,24 +95,23 @@ VehicleCmdFilterParam VehicleCmdFilter::getParam() const return param_; } -void VehicleCmdFilter::limitLongitudinalWithVel(AckermannControlCommand & input) const +void VehicleCmdFilter::limitLongitudinalWithVel(Control & input) const { - input.longitudinal.speed = std::max( - std::min(static_cast(input.longitudinal.speed), param_.vel_lim), -param_.vel_lim); + input.longitudinal.velocity = std::max( + std::min(static_cast(input.longitudinal.velocity), param_.vel_lim), -param_.vel_lim); } -void VehicleCmdFilter::limitLongitudinalWithAcc( - const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLongitudinalWithAcc(const double dt, Control & input) const { const auto lon_acc_lim = getLonAccLim(); input.longitudinal.acceleration = std::max( std::min(static_cast(input.longitudinal.acceleration), lon_acc_lim), -lon_acc_lim); - input.longitudinal.speed = - limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, lon_acc_lim * dt); + input.longitudinal.velocity = + limitDiff(input.longitudinal.velocity, prev_cmd_.longitudinal.velocity, lon_acc_lim * dt); } void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( - const double dt, AckermannControlCommand & input) const + const double dt, Control & input) const { const auto lon_jerk_lim = getLonJerkLim(); input.longitudinal.acceleration = limitDiff( @@ -124,7 +123,7 @@ void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( // Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the // filtered steering angle oscillates if the input velocity oscillates. void VehicleCmdFilter::limitLateralWithLatAcc( - [[maybe_unused]] const double dt, AckermannControlCommand & input) const + [[maybe_unused]] const double dt, Control & input) const { const auto lat_acc_lim = getLatAccLim(); @@ -138,8 +137,7 @@ void VehicleCmdFilter::limitLateralWithLatAcc( // Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the // filtered steering angle oscillates if the input velocity oscillates. -void VehicleCmdFilter::limitLateralWithLatJerk( - const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralWithLatJerk(const double dt, Control & input) const { double curr_latacc = calcLatAcc(input, current_speed_); double prev_latacc = calcLatAcc(prev_cmd_, current_speed_); @@ -156,8 +154,7 @@ void VehicleCmdFilter::limitLateralWithLatJerk( } } -void VehicleCmdFilter::limitActualSteerDiff( - const double current_steer_angle, AckermannControlCommand & input) const +void VehicleCmdFilter::limitActualSteerDiff(const double current_steer_angle, Control & input) const { const auto actual_steer_diff_lim = getSteerDiffLim(); @@ -166,7 +163,7 @@ void VehicleCmdFilter::limitActualSteerDiff( input.lateral.steering_tire_angle = current_steer_angle + ds; } -void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralSteer(Control & input) const { const float steer_limit = getSteerLim(); @@ -185,7 +182,7 @@ void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const } } -void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralSteerRate(const double dt, Control & input) const { const float steer_rate_limit = getSteerRateLim(); @@ -201,7 +198,7 @@ void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCo } void VehicleCmdFilter::filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & cmd, + const double dt, const double current_steer_angle, Control & cmd, IsFilterActivated & is_activated) const { const auto cmd_orig = cmd; @@ -219,14 +216,14 @@ void VehicleCmdFilter::filterAll( } IsFilterActivated VehicleCmdFilter::checkIsActivated( - const AckermannControlCommand & c1, const AckermannControlCommand & c2, const double tol) + const Control & c1, const Control & c2, const double tol) { IsFilterActivated msg; msg.is_activated_on_steering = std::abs(c1.lateral.steering_tire_angle - c2.lateral.steering_tire_angle) > tol; msg.is_activated_on_steering_rate = std::abs(c1.lateral.steering_tire_rotation_rate - c2.lateral.steering_tire_rotation_rate) > tol; - msg.is_activated_on_speed = std::abs(c1.longitudinal.speed - c2.longitudinal.speed) > tol; + msg.is_activated_on_speed = std::abs(c1.longitudinal.velocity - c2.longitudinal.velocity) > tol; msg.is_activated_on_acceleration = std::abs(c1.longitudinal.acceleration - c2.longitudinal.acceleration) > tol; msg.is_activated_on_jerk = std::abs(c1.longitudinal.jerk - c2.longitudinal.jerk) > tol; @@ -244,13 +241,13 @@ double VehicleCmdFilter::calcSteerFromLatacc(const double v, const double latacc return std::atan(latacc * param_.wheel_base / v_sq); } -double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd) const +double VehicleCmdFilter::calcLatAcc(const Control & cmd) const { - double v = cmd.longitudinal.speed; + double v = cmd.longitudinal.velocity; return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; } -double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd, const double v) const +double VehicleCmdFilter::calcLatAcc(const Control & cmd, const double v) const { return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; } @@ -322,4 +319,4 @@ double VehicleCmdFilter::getSteerDiffLim() const return interpolateFromSpeed(param_.actual_steer_diff_lim); } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp new file mode 100644 index 0000000000000..96663474f47a4 --- /dev/null +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -0,0 +1,103 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 VEHICLE_CMD_FILTER_HPP_ +#define VEHICLE_CMD_FILTER_HPP_ + +#include +#include + +#include + +#include + +namespace autoware::vehicle_cmd_gate +{ +using autoware_control_msgs::msg::Control; +using autoware_vehicle_cmd_gate::msg::IsFilterActivated; +using LimitArray = std::vector; + +struct VehicleCmdFilterParam +{ + double wheel_base; + double vel_lim; + LimitArray reference_speed_points; + LimitArray lon_acc_lim; + LimitArray lon_jerk_lim; + LimitArray lat_acc_lim; + LimitArray lat_jerk_lim; + LimitArray steer_lim; + LimitArray steer_rate_lim; + LimitArray actual_steer_diff_lim; +}; +class VehicleCmdFilter +{ +public: + VehicleCmdFilter(); + ~VehicleCmdFilter() = default; + + void setWheelBase(double v) { param_.wheel_base = v; } + void setVelLim(double v) { param_.vel_lim = v; } + void setSteerLim(LimitArray v); + void setSteerRateLim(LimitArray v); + void setLonAccLim(LimitArray v); + void setLonJerkLim(LimitArray v); + void setLatAccLim(LimitArray v); + void setLatJerkLim(LimitArray v); + void setActualSteerDiffLim(LimitArray v); + void setCurrentSpeed(double v) { current_speed_ = v; } + void setParam(const VehicleCmdFilterParam & p); + VehicleCmdFilterParam getParam() const; + void setPrevCmd(const Control & v) { prev_cmd_ = v; } + + void limitLongitudinalWithVel(Control & input) const; + void limitLongitudinalWithAcc(const double dt, Control & input) const; + void limitLongitudinalWithJerk(const double dt, Control & input) const; + void limitLateralWithLatAcc(const double dt, Control & input) const; + void limitLateralWithLatJerk(const double dt, Control & input) const; + void limitActualSteerDiff(const double current_steer_angle, Control & input) const; + void limitLateralSteer(Control & input) const; + void limitLateralSteerRate(const double dt, Control & input) const; + void filterAll( + const double dt, const double current_steer_angle, Control & input, + IsFilterActivated & is_activated) const; + static IsFilterActivated checkIsActivated( + const Control & c1, const Control & c2, const double tol = 1.0e-3); + + Control getPrevCmd() { return prev_cmd_; } + +private: + VehicleCmdFilterParam param_; + Control prev_cmd_; + double current_speed_ = 0.0; + + bool setParameterWithValidation(const VehicleCmdFilterParam & p); + + double calcLatAcc(const Control & cmd) const; + double calcLatAcc(const Control & cmd, const double v) const; + double calcSteerFromLatacc(const double v, const double latacc) const; + double limitDiff(const double curr, const double prev, const double diff_lim) const; + + double interpolateFromSpeed(const LimitArray & limits) const; + double getLonAccLim() const; + double getLonJerkLim() const; + double getLatAccLim() const; + double getLatJerkLim() const; + double getSteerLim() const; + double getSteerRateLim() const; + double getSteerDiffLim() const; +}; +} // namespace autoware::vehicle_cmd_gate + +#endif // VEHICLE_CMD_FILTER_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp similarity index 88% rename from control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp rename to control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index ffd452db0337c..87e79f59bc356 100644 --- a/control/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 @@ -28,7 +28,7 @@ #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { namespace @@ -62,7 +62,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) // Publisher vehicle_cmd_emergency_pub_ = create_publisher("output/vehicle_cmd_emergency", durable_qos); - control_cmd_pub_ = create_publisher("output/control_cmd", durable_qos); + control_cmd_pub_ = create_publisher("output/control_cmd", durable_qos); gear_cmd_pub_ = create_publisher("output/gear_cmd", durable_qos); turn_indicator_cmd_pub_ = create_publisher("output/turn_indicators_cmd", durable_qos); @@ -108,49 +108,17 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1)); // Subscriber for auto - auto_control_cmd_sub_ = create_subscription( + auto_control_cmd_sub_ = create_subscription( "input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1)); - auto_turn_indicator_cmd_sub_ = create_subscription( - "input/auto/turn_indicators_cmd", 1, - [this](TurnIndicatorsCommand::ConstSharedPtr msg) { auto_commands_.turn_indicator = *msg; }); - - auto_hazard_light_cmd_sub_ = create_subscription( - "input/auto/hazard_lights_cmd", 1, - [this](HazardLightsCommand::ConstSharedPtr msg) { auto_commands_.hazard_light = *msg; }); - - auto_gear_cmd_sub_ = create_subscription( - "input/auto/gear_cmd", 1, - [this](GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; }); - // Subscriber for external - remote_control_cmd_sub_ = create_subscription( + remote_control_cmd_sub_ = create_subscription( "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1)); - remote_turn_indicator_cmd_sub_ = create_subscription( - "input/external/turn_indicators_cmd", 1, - [this](TurnIndicatorsCommand::ConstSharedPtr msg) { remote_commands_.turn_indicator = *msg; }); - - remote_hazard_light_cmd_sub_ = create_subscription( - "input/external/hazard_lights_cmd", 1, - [this](HazardLightsCommand::ConstSharedPtr msg) { remote_commands_.hazard_light = *msg; }); - - remote_gear_cmd_sub_ = create_subscription( - "input/external/gear_cmd", 1, - [this](GearCommand::ConstSharedPtr msg) { remote_commands_.gear = *msg; }); - // Subscriber for emergency - emergency_control_cmd_sub_ = create_subscription( + emergency_control_cmd_sub_ = create_subscription( "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1)); - emergency_hazard_light_cmd_sub_ = create_subscription( - "input/emergency/hazard_lights_cmd", 1, - [this](HazardLightsCommand::ConstSharedPtr msg) { emergency_commands_.hazard_light = *msg; }); - - emergency_gear_cmd_sub_ = create_subscription( - "input/emergency/gear_cmd", 1, - [this](GearCommand::ConstSharedPtr msg) { emergency_commands_.gear = *msg; }); - // Parameter use_emergency_handling_ = declare_parameter("use_emergency_handling"); check_external_emergency_heartbeat_ = @@ -170,7 +138,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) declare_parameter("filter_activated_velocity_threshold"); // Vehicle Parameter - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); { VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; @@ -242,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_ = @@ -254,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( @@ -276,7 +245,7 @@ rcl_interfaces::msg::SetParametersResult VehicleCmdGate::onParameter( parameters, "filter_activated_velocity_threshold", filter_activated_velocity_threshold_); // Vehicle Parameter - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); { VehicleCmdFilterParam p = filter_.getParam(); p.wheel_base = vehicle_info.wheel_base_m; @@ -354,7 +323,7 @@ bool VehicleCmdGate::isDataReady() } // for auto -void VehicleCmdGate::onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onAutoCtrlCmd(Control::ConstSharedPtr msg) { auto_commands_.control = *msg; @@ -364,7 +333,7 @@ void VehicleCmdGate::onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) } // for remote -void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onRemoteCtrlCmd(Control::ConstSharedPtr msg) { remote_commands_.control = *msg; @@ -374,7 +343,7 @@ void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg } // for emergency -void VehicleCmdGate::onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg) { emergency_commands_.control = *msg; @@ -385,6 +354,37 @@ void VehicleCmdGate::onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr void VehicleCmdGate::onTimer() { + // Subscriber for auto + 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; + + const auto msg_auto_command_hazard_light = auto_hazard_light_cmd_sub_.takeData(); + if (msg_auto_command_hazard_light) auto_commands_.hazard_light = *msg_auto_command_hazard_light; + + const auto msg_auto_command_gear = auto_gear_cmd_sub_.takeData(); + if (msg_auto_command_gear) auto_commands_.gear = *msg_auto_command_gear; + + // Subscribe for external + 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; + + const auto msg_remote_command_hazard_light = remote_hazard_light_cmd_sub_.takeData(); + if (msg_remote_command_hazard_light) + remote_commands_.hazard_light = *msg_remote_command_hazard_light; + + const auto msg_remote_command_gear = remote_gear_cmd_sub_.takeData(); + if (msg_remote_command_gear) remote_commands_.gear = *msg_remote_command_gear; + + // Subscribe for emergency + 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; + + const auto msg_emergency_command_gear = emergency_gear_cmd_sub_.takeData(); + if (msg_emergency_command_gear) emergency_commands_.gear = *msg_emergency_command_gear; + updater_.force_update(); if (!isDataReady()) { @@ -489,7 +489,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) } if (moderate_stop_interface_->is_stop_requested()) { // if stop requested, stop the vehicle - filtered_commands.control.longitudinal.speed = 0.0; + filtered_commands.control.longitudinal.velocity = 0.0; filtered_commands.control.longitudinal.acceleration = moderate_stop_service_acceleration_; } @@ -543,7 +543,7 @@ void VehicleCmdGate::publishEmergencyStopControlCommands() const auto stamp = this->now(); // ControlCommand - AckermannControlCommand control_cmd; + Control control_cmd; control_cmd.stamp = stamp; control_cmd = createEmergencyStopControlCmd(); @@ -600,9 +600,9 @@ void VehicleCmdGate::publishStatus() moderate_stop_interface_->publish(); } -AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannControlCommand & in) +Control VehicleCmdGate::filterControlCommand(const Control & in) { - AckermannControlCommand out = in; + Control out = in; const double dt = getDt(); const auto mode = current_operation_mode_; const auto current_status_cmd = getActualStatusAsCommand(); @@ -629,10 +629,10 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont prev_cmd.longitudinal.acceleration = std::max(prev_cmd.longitudinal.acceleration, current_status_cmd.longitudinal.acceleration); // consider reverse driving - prev_cmd.longitudinal.speed = - std::fabs(prev_cmd.longitudinal.speed) > std::fabs(current_status_cmd.longitudinal.speed) - ? prev_cmd.longitudinal.speed - : current_status_cmd.longitudinal.speed; + prev_cmd.longitudinal.velocity = std::fabs(prev_cmd.longitudinal.velocity) > + std::fabs(current_status_cmd.longitudinal.velocity) + ? prev_cmd.longitudinal.velocity + : current_status_cmd.longitudinal.velocity; filter_.setPrevCmd(prev_cmd); } filter_.filterAll(dt, current_steer_, out, is_filter_activated); @@ -663,42 +663,42 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont return out; } -AckermannControlCommand VehicleCmdGate::createStopControlCmd() const +Control VehicleCmdGate::createStopControlCmd() const { - AckermannControlCommand cmd; + Control cmd; const auto t = this->now(); cmd.stamp = t; cmd.lateral.stamp = t; cmd.longitudinal.stamp = t; cmd.lateral.steering_tire_angle = current_steer_; cmd.lateral.steering_tire_rotation_rate = 0.0; - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = stop_hold_acceleration_; return cmd; } -LongitudinalCommand VehicleCmdGate::createLongitudinalStopControlCmd() const +Longitudinal VehicleCmdGate::createLongitudinalStopControlCmd() const { - LongitudinalCommand cmd; + Longitudinal cmd; const auto t = this->now(); cmd.stamp = t; - cmd.speed = 0.0; + cmd.velocity = 0.0; cmd.acceleration = stop_hold_acceleration_; return cmd; } -AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd() const +Control VehicleCmdGate::createEmergencyStopControlCmd() const { - AckermannControlCommand cmd; + Control cmd; const auto t = this->now(); cmd.stamp = t; cmd.lateral.stamp = t; cmd.longitudinal.stamp = t; cmd.lateral.steering_tire_angle = prev_control_cmd_.lateral.steering_tire_angle; cmd.lateral.steering_tire_rotation_rate = prev_control_cmd_.lateral.steering_tire_rotation_rate; - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = emergency_acceleration_; return cmd; @@ -757,13 +757,13 @@ double VehicleCmdGate::getDt() return dt; } -AckermannControlCommand VehicleCmdGate::getActualStatusAsCommand() +Control VehicleCmdGate::getActualStatusAsCommand() { - AckermannControlCommand status; + Control status; status.stamp = status.lateral.stamp = status.longitudinal.stamp = this->now(); status.lateral.steering_tire_angle = current_steer_; status.lateral.steering_tire_rotation_rate = 0.0; - status.longitudinal.speed = current_kinematics_.twist.twist.linear.x; + status.longitudinal.velocity = current_kinematics_.twist.twist.linear.x; status.longitudinal.acceleration = current_acceleration_; return status; } @@ -828,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; @@ -906,7 +907,7 @@ void VehicleCmdGate::publishMarkers(const IsFilterActivated & filter_activated) filter_activated_flag_pub_->publish(filter_activated_flag); filter_activated_marker_raw_pub_->publish(createMarkerArray(filter_activated)); } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #include -RCLCPP_COMPONENTS_REGISTER_NODE(vehicle_cmd_gate::VehicleCmdGate) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::vehicle_cmd_gate::VehicleCmdGate) diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp new file mode 100644 index 0000000000000..42e28d633d16b --- /dev/null +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -0,0 +1,270 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 VEHICLE_CMD_GATE_HPP_ +#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 "vehicle_cmd_filter.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::vehicle_cmd_gate +{ + +using autoware_adapi_v1_msgs::msg::MrmState; +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_control_msgs::msg::Control; +using autoware_control_msgs::msg::Longitudinal; +using autoware_vehicle_cmd_gate::msg::IsFilterActivated; +using autoware_vehicle_msgs::msg::GearCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using std_srvs::srv::Trigger; +using tier4_control_msgs::msg::GateMode; +using tier4_debug_msgs::msg::BoolStamped; +using tier4_external_api_msgs::msg::Emergency; +using tier4_external_api_msgs::msg::Heartbeat; +using tier4_external_api_msgs::srv::SetEmergency; +using tier4_system_msgs::msg::MrmBehaviorStatus; +using tier4_vehicle_msgs::msg::VehicleEmergencyStamped; +using visualization_msgs::msg::MarkerArray; + +using diagnostic_msgs::msg::DiagnosticStatus; +using nav_msgs::msg::Odometry; + +using EngageMsg = autoware_vehicle_msgs::msg::Engage; +using EngageSrv = tier4_external_api_msgs::srv::Engage; + +using autoware::motion_utils::VehicleStopChecker; +struct Commands +{ + Control control; + TurnIndicatorsCommand turn_indicator; + HazardLightsCommand hazard_light; + GearCommand gear; + explicit Commands(const uint8_t & default_gear = GearCommand::PARK) + { + gear.command = default_gear; + } +}; + +class VehicleCmdGate : public rclcpp::Node +{ +public: + explicit VehicleCmdGate(const rclcpp::NodeOptions & node_options); + +private: + // Publisher + rclcpp::Publisher::SharedPtr vehicle_cmd_emergency_pub_; + rclcpp::Publisher::SharedPtr control_cmd_pub_; + rclcpp::Publisher::SharedPtr gear_cmd_pub_; + rclcpp::Publisher::SharedPtr turn_indicator_cmd_pub_; + rclcpp::Publisher::SharedPtr hazard_light_cmd_pub_; + rclcpp::Publisher::SharedPtr gate_mode_pub_; + rclcpp::Publisher::SharedPtr engage_pub_; + rclcpp::Publisher::SharedPtr operation_mode_pub_; + rclcpp::Publisher::SharedPtr is_filter_activated_pub_; + rclcpp::Publisher::SharedPtr filter_activated_marker_pub_; + rclcpp::Publisher::SharedPtr filter_activated_marker_raw_pub_; + rclcpp::Publisher::SharedPtr filter_activated_flag_pub_; + // Parameter callback + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + // Subscription + rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; + 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(MrmState::ConstSharedPtr msg); + + bool is_engaged_; + bool is_system_emergency_ = false; + bool is_external_emergency_stop_ = false; + bool is_filtered_marker_published_ = false; + double current_steer_ = 0; + GateMode current_gate_mode_; + MrmState current_mrm_state_; + Odometry current_kinematics_; + double current_acceleration_ = 0.0; + int filter_activated_count_ = 0; + + // Heartbeat + std::shared_ptr emergency_state_heartbeat_received_time_; + bool is_emergency_state_heartbeat_timeout_ = false; + std::shared_ptr external_emergency_stop_heartbeat_received_time_; + bool is_external_emergency_stop_heartbeat_timeout_ = false; + bool isHeartbeatTimeout( + const std::shared_ptr & heartbeat_received_time, const double timeout); + + // Check initialization + bool isDataReady(); + + // Subscriber for auto + Commands auto_commands_; + rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; + autoware::universe_utils::InterProcessPollingSubscriber + auto_turn_indicator_cmd_sub_{this, "input/auto/turn_indicators_cmd"}; + autoware::universe_utils::InterProcessPollingSubscriber + auto_hazard_light_cmd_sub_{this, "input/auto/hazard_lights_cmd"}; + autoware::universe_utils::InterProcessPollingSubscriber auto_gear_cmd_sub_{ + this, "input/auto/gear_cmd"}; + void onAutoCtrlCmd(Control::ConstSharedPtr msg); + + // Subscription for external + Commands remote_commands_; + rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; + autoware::universe_utils::InterProcessPollingSubscriber + remote_turn_indicator_cmd_sub_{this, "input/external/turn_indicators_cmd"}; + autoware::universe_utils::InterProcessPollingSubscriber + remote_hazard_light_cmd_sub_{this, "input/external/hazard_lights_cmd"}; + autoware::universe_utils::InterProcessPollingSubscriber remote_gear_cmd_sub_{ + this, "input/external/gear_cmd"}; + void onRemoteCtrlCmd(Control::ConstSharedPtr msg); + + // Subscription for emergency + Commands emergency_commands_; + rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; + autoware::universe_utils::InterProcessPollingSubscriber + emergency_hazard_light_cmd_sub_{this, "input/emergency/hazard_lights_cmd"}; + autoware::universe_utils::InterProcessPollingSubscriber emergency_gear_cmd_sub_{ + this, "input/emergency/gear_cmd"}; + void onEmergencyCtrlCmd(Control::ConstSharedPtr msg); + + // Parameter + bool use_emergency_handling_; + bool check_external_emergency_heartbeat_; + double system_emergency_heartbeat_timeout_; + double external_emergency_stop_heartbeat_timeout_; + double stop_hold_acceleration_; + double emergency_acceleration_; + double moderate_stop_service_acceleration_; + bool enable_cmd_limit_filter_; + int filter_activated_count_threshold_; + double filter_activated_velocity_threshold_; + + // Service + rclcpp::Service::SharedPtr srv_engage_; + rclcpp::Service::SharedPtr srv_external_emergency_; + rclcpp::Publisher::SharedPtr pub_external_emergency_; + void onEngageService( + const EngageSrv::Request::SharedPtr request, const EngageSrv::Response::SharedPtr response); + void onExternalEmergencyStopService( + const std::shared_ptr request_header, + const SetEmergency::Request::SharedPtr request, + const SetEmergency::Response::SharedPtr response); + + // TODO(Takagi, Isamu): deprecated + rclcpp::Subscription::SharedPtr engage_sub_; + rclcpp::Service::SharedPtr srv_external_emergency_stop_; + rclcpp::Service::SharedPtr srv_clear_external_emergency_stop_; + void onEngage(EngageMsg::ConstSharedPtr msg); + bool onSetExternalEmergencyStopService( + const std::shared_ptr req_header, const Trigger::Request::SharedPtr req, + const Trigger::Response::SharedPtr res); + bool onClearExternalEmergencyStopService( + const std::shared_ptr req_header, const Trigger::Request::SharedPtr req, + const Trigger::Response::SharedPtr res); + + // Timer / Event + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr timer_pub_status_; + + void onTimer(); + void publishControlCommands(const Commands & input_msg); + void publishEmergencyStopControlCommands(); + void publishStatus(); + + // Diagnostics Updater + diagnostic_updater::Updater updater_; + + void checkExternalEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat); + + // Algorithm + Control prev_control_cmd_; + Control createStopControlCmd() const; + Longitudinal createLongitudinalStopControlCmd() const; + Control createEmergencyStopControlCmd() const; + + std::shared_ptr prev_time_; + double getDt(); + Control getActualStatusAsCommand(); + + VehicleCmdFilter filter_; + Control filterControlCommand(const Control & msg); + + // filtering on transition + OperationModeState current_operation_mode_; + VehicleCmdFilter filter_on_transition_; + + // Pause interface for API + std::unique_ptr adapi_pause_; + std::unique_ptr moderate_stop_interface_; + + // stop checker + std::unique_ptr vehicle_stop_checker_; + double stop_check_duration_; + + // debug + MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); + void publishMarkers(const IsFilterActivated & filter_activated); + + std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; +}; + +} // namespace autoware::vehicle_cmd_gate +#endif // VEHICLE_CMD_GATE_HPP_ diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp similarity index 91% rename from control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp rename to control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 949ca46d27dea..46130fc8f9941 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -54,30 +54,29 @@ const std::vector lat_jerk_lim = {1.7, 1.3, 0.9, 0.6}; const std::vector actual_steer_diff_lim = {0.5, 0.4, 0.2, 0.1}; const double wheelbase = 2.89; -using vehicle_cmd_gate::VehicleCmdGate; +using autoware::vehicle_cmd_gate::VehicleCmdGate; using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_vehicle_msgs::msg::GearCommand; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_control_msgs::msg::Control; +using autoware_vehicle_msgs::msg::GearCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using nav_msgs::msg::Odometry; using tier4_control_msgs::msg::GateMode; using tier4_external_api_msgs::msg::Emergency; using tier4_external_api_msgs::msg::Heartbeat; -using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; +using EngageMsg = autoware_vehicle_msgs::msg::Engage; class PubSubNode : public rclcpp::Node { public: PubSubNode() : Node{"test_vehicle_cmd_gate_filter_pubsub"} { - sub_cmd_ = create_subscription( - "output/control_cmd", rclcpp::QoS{1}, - [this](const AckermannControlCommand::ConstSharedPtr msg) { + sub_cmd_ = create_subscription( + "output/control_cmd", rclcpp::QoS{1}, [this](const Control::ConstSharedPtr msg) { cmd_history_.push_back(msg); cmd_received_times_.push_back(now()); // check the effectiveness of the filter for last x elements in the queue @@ -97,8 +96,7 @@ class PubSubNode : public rclcpp::Node pub_operation_mode_ = create_publisher("input/operation_mode", qos); pub_mrm_state_ = create_publisher("input/mrm_state", qos); - pub_auto_control_cmd_ = - create_publisher("input/auto/control_cmd", qos); + pub_auto_control_cmd_ = create_publisher("input/auto/control_cmd", qos); pub_auto_turn_indicator_cmd_ = create_publisher("input/auto/turn_indicators_cmd", qos); pub_auto_hazard_light_cmd_ = @@ -106,7 +104,7 @@ class PubSubNode : public rclcpp::Node pub_auto_gear_cmd_ = create_publisher("input/auto/gear_cmd", qos); } - rclcpp::Subscription::SharedPtr sub_cmd_; + rclcpp::Subscription::SharedPtr sub_cmd_; rclcpp::Publisher::SharedPtr pub_external_emergency_stop_heartbeat_; rclcpp::Publisher::SharedPtr pub_engage_; @@ -116,13 +114,13 @@ class PubSubNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_steer_; rclcpp::Publisher::SharedPtr pub_operation_mode_; rclcpp::Publisher::SharedPtr pub_mrm_state_; - rclcpp::Publisher::SharedPtr pub_auto_control_cmd_; + rclcpp::Publisher::SharedPtr pub_auto_control_cmd_; rclcpp::Publisher::SharedPtr pub_auto_turn_indicator_cmd_; rclcpp::Publisher::SharedPtr pub_auto_hazard_light_cmd_; rclcpp::Publisher::SharedPtr pub_auto_gear_cmd_; - std::vector cmd_history_; - std::vector raw_cmd_history_; + std::vector cmd_history_; + std::vector raw_cmd_history_; std::vector cmd_received_times_; // publish except for the control_cmd @@ -152,7 +150,7 @@ class PubSubNode : public rclcpp::Node msg.twist.twist.linear.x = 0.0; if (!cmd_history_.empty()) { // ego moves as commanded. msg.twist.twist.linear.x = - cmd_history_.back()->longitudinal.speed; // ego moves as commanded. + cmd_history_.back()->longitudinal.velocity; // ego moves as commanded. } pub_odom_->publish(msg); } @@ -209,11 +207,11 @@ class PubSubNode : public rclcpp::Node } } - void publishControlCommand(AckermannControlCommand msg) + void publishControlCommand(Control msg) { msg.stamp = now(); pub_auto_control_cmd_->publish(msg); - raw_cmd_history_.push_back(std::make_shared(msg)); + raw_cmd_history_.push_back(std::make_shared(msg)); } void checkFilter(size_t last_x) @@ -249,7 +247,7 @@ class PubSubNode : public rclcpp::Node // TODO(Horibe): prev_lat_acc should use the previous velocity, but use the current velocity // since the current filtering logic uses the current velocity. // when it's fixed, should be like this: - // prev_lat_acc = calculate_lateral_acceleration(cmd_start->longitudinal.speed, + // prev_lat_acc = calculate_lateral_acceleration(cmd_start->longitudinal.velocity, // cmd_start->lateral.steering_tire_angle, wheelbase); prev_tire_angle = cmd_start->lateral.steering_tire_angle; } @@ -261,7 +259,7 @@ class PubSubNode : public rclcpp::Node ASSERT_GT(dt, 0.0) << "Invalid dt. Time must be strictly positive."; - lon_vel = cmd->longitudinal.speed; + lon_vel = cmd->longitudinal.velocity; const auto lon_acc = cmd->longitudinal.acceleration; const auto lon_jerk = (lon_acc - prev_lon_acc) / dt; @@ -341,7 +339,7 @@ class ControlCmdGenerator // generate ControlCommand with sin wave format. // TODO(Horibe): implement steering_rate and jerk command. - AckermannControlCommand calcSinWaveCommand(bool reset_clock = false) + Control calcSinWaveCommand(bool reset_clock = false) { if (reset_clock) { start_time_ = Clock::now(); @@ -355,9 +353,9 @@ class ControlCmdGenerator return amp * std::sin(2.0 * M_PI * freq * dt_s + bias); }; - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = sinWave(p_.steering.max, p_.steering.freq, p_.steering.bias); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = sinWave(p_.velocity.max, p_.velocity.freq, p_.velocity.bias) + p_.velocity.max; cmd.longitudinal.acceleration = sinWave(p_.acceleration.max, p_.acceleration.freq, p_.acceleration.bias); @@ -371,9 +369,9 @@ std::shared_ptr generateNode() auto node_options = rclcpp::NodeOptions{}; const auto vehicle_cmd_gate_dir = - ament_index_cpp::get_package_share_directory("vehicle_cmd_gate"); + ament_index_cpp::get_package_share_directory("autoware_vehicle_cmd_gate"); const auto vehicle_info_util_dir = - ament_index_cpp::get_package_share_directory("vehicle_info_util"); + ament_index_cpp::get_package_share_directory("autoware_vehicle_info_utils"); node_options.arguments( {"--ros-args", "--params-file", vehicle_cmd_gate_dir + "/config/vehicle_cmd_gate.param.yaml", diff --git a/control/vehicle_cmd_gate/test/src/test_main.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_main.cpp similarity index 100% rename from control/vehicle_cmd_gate/test/src/test_main.cpp rename to control/autoware_vehicle_cmd_gate/test/src/test_main.cpp diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp similarity index 90% rename from control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp rename to control/autoware_vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index b8200490dd1d5..528253671b38a 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/autoware_vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -24,17 +24,17 @@ #define ASSERT_LT_NEAR(x, y) ASSERT_LT(x, y + THRESHOLD) #define ASSERT_GT_NEAR(x, y) ASSERT_GT(x, y - THRESHOLD) -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using vehicle_cmd_gate::LimitArray; +using autoware::vehicle_cmd_gate::LimitArray; +using autoware_control_msgs::msg::Control; constexpr double NOMINAL_INTERVAL = 1.0; void setFilterParams( - vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a, + autoware::vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a, LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, LimitArray steer_lim, LimitArray steer_rate_lim, const double wheelbase) { - vehicle_cmd_gate::VehicleCmdFilterParam p; + autoware::vehicle_cmd_gate::VehicleCmdFilterParam p; p.vel_lim = v; p.wheel_base = wheelbase; p.reference_speed_points = speed_points; @@ -49,38 +49,37 @@ void setFilterParams( f.setParam(p); } -AckermannControlCommand genCmd(double s, double sr, double v, double a) +Control genCmd(double s, double sr, double v, double a) { - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = s; cmd.lateral.steering_tire_rotation_rate = sr; - cmd.longitudinal.speed = v; + cmd.longitudinal.velocity = v; cmd.longitudinal.acceleration = a; return cmd; } // calc from ego velocity -double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase, const double ego_v) +double calcLatAcc(const Control & cmd, const double wheelbase, const double ego_v) { return ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } // calc from command velocity -double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase) +double calcLatAcc(const Control & cmd, const double wheelbase) { - double v = cmd.longitudinal.speed; + double v = cmd.longitudinal.velocity; return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } // calc from command velocity double calcLatJerk( - const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, - const double wheelbase, const double dt) + const Control & cmd, const Control & prev_cmd, const double wheelbase, const double dt) { - const auto prev_v = prev_cmd.longitudinal.speed; + const auto prev_v = prev_cmd.longitudinal.velocity; const auto prev = prev_v * prev_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; - const auto curr_v = cmd.longitudinal.speed; + const auto curr_v = cmd.longitudinal.velocity; const auto curr = curr_v * curr_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; return (curr - prev) / dt; @@ -88,8 +87,8 @@ double calcLatJerk( // calc from ego velocity double calcLatJerk( - const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, - const double wheelbase, const double dt, const double ego_v) + const Control & cmd, const Control & prev_cmd, const double wheelbase, const double dt, + const double ego_v) { const auto prev = ego_v * ego_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; @@ -100,13 +99,13 @@ double calcLatJerk( void test_1d_limit( double ego_v, double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, - double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM, - const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) + double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM, const Control & prev_cmd, + const Control & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] - vehicle_cmd_gate::VehicleCmdFilter filter; + autoware::vehicle_cmd_gate::VehicleCmdFilter filter; filter.setCurrentSpeed(ego_v); setFilterParams( filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, {STEER_LIM}, @@ -119,11 +118,11 @@ void test_1d_limit( filter.limitLongitudinalWithVel(filtered_cmd); // check if the filtered value does not exceed the limit. - ASSERT_LT_NEAR(filtered_cmd.longitudinal.speed, V_LIM); + ASSERT_LT_NEAR(filtered_cmd.longitudinal.velocity, V_LIM); // check if the undesired filter is not applied. - if (std::abs(raw_cmd.longitudinal.speed) < V_LIM) { - ASSERT_NEAR(filtered_cmd.longitudinal.speed, raw_cmd.longitudinal.speed, THRESHOLD); + if (std::abs(raw_cmd.longitudinal.velocity) < V_LIM) { + ASSERT_NEAR(filtered_cmd.longitudinal.velocity, raw_cmd.longitudinal.velocity, THRESHOLD); } } @@ -145,14 +144,14 @@ void test_1d_limit( } // check if the filtered value does not exceed the limit. - const double v_max = prev_cmd.longitudinal.speed + A_LIM * DT; - const double v_min = prev_cmd.longitudinal.speed - A_LIM * DT; - ASSERT_LT_NEAR(filtered_cmd.longitudinal.speed, v_max); - ASSERT_GT_NEAR(filtered_cmd.longitudinal.speed, v_min); + const double v_max = prev_cmd.longitudinal.velocity + A_LIM * DT; + const double v_min = prev_cmd.longitudinal.velocity - A_LIM * DT; + ASSERT_LT_NEAR(filtered_cmd.longitudinal.velocity, v_max); + ASSERT_GT_NEAR(filtered_cmd.longitudinal.velocity, v_min); // check if the undesired filter is not applied. - if (v_min < raw_cmd.longitudinal.speed && raw_cmd.longitudinal.speed < v_max) { - ASSERT_NEAR(filtered_cmd.longitudinal.speed, raw_cmd.longitudinal.speed, THRESHOLD); + if (v_min < raw_cmd.longitudinal.velocity && raw_cmd.longitudinal.velocity < v_max) { + ASSERT_NEAR(filtered_cmd.longitudinal.velocity, raw_cmd.longitudinal.velocity, THRESHOLD); } } @@ -241,10 +240,10 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector steer_rate_lim_arr = {0.01, 1.0, 100.0}; const std::vector ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.0}; - const std::vector prev_cmd_arr = { + const std::vector prev_cmd_arr = { genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)}; - const std::vector raw_cmd_arr = { + const std::vector raw_cmd_arr = { genCmd(1.0, 1.0, 1.0, 1.0), genCmd(10.0, -1.0, -1.0, -1.0)}; for (const auto & v : v_arr) { @@ -276,9 +275,9 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) { constexpr double WHEELBASE = 2.8; - vehicle_cmd_gate::VehicleCmdFilter filter; + autoware::vehicle_cmd_gate::VehicleCmdFilter filter; - vehicle_cmd_gate::VehicleCmdFilterParam p; + autoware::vehicle_cmd_gate::VehicleCmdFilterParam p; p.wheel_base = WHEELBASE; p.vel_lim = 20.0; p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; @@ -294,10 +293,10 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) const auto DT = 0.033; const auto orig_cmd = []() { - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = 0.5; cmd.lateral.steering_tire_rotation_rate = 0.5; - cmd.longitudinal.speed = 30.0; + cmd.longitudinal.velocity = 30.0; cmd.longitudinal.acceleration = 10.0; cmd.longitudinal.jerk = 10.0; return cmd; @@ -353,7 +352,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) // vel lim { set_speed_and_reset_prev(0.0); - EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.speed, 20.0, ep); + EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.velocity, 20.0, ep); } // steer angle lim @@ -389,7 +388,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) const auto calcSteerRateFromAngle = [&](const auto & cmd) { return (cmd.steering_tire_angle - 0.0) / DT; }; - autoware_auto_control_msgs::msg::AckermannLateralCommand filtered; + autoware_control_msgs::msg::Lateral filtered; set_speed_and_reset_prev(0.0); filtered = _limitSteerRate(orig_cmd).lateral; @@ -521,7 +520,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; // p.lat_jerk_lim = std::vector{0.9, 0.7, 0.1}; const auto _calcLatJerk = [&](const auto & cmd, const double ego_v) { - return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT, ego_v); + return calcLatJerk(cmd, Control{}, WHEELBASE, DT, ego_v); }; { // since the lateral acceleration and jerk is 0 when the velocity is 0, the target value is 0 diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index 1fa87c2caf3a9..3895b3cc13465 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -25,13 +25,13 @@ Error acceleration calculations are made based on the velocity calculations abov ### Input topics -| Name | Type | Description | -| ---------------------------------------- | -------------------------------------------------------- | ------------------------------------------- | -| `/planning/scenario_planning/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Output trajectory from planning module. | -| `/control/command/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | Output control command from control module. | -| `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. | -| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | -| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | +| Name | Type | Description | +| ---------------------------------------- | ------------------------------------------ | ------------------------------------------- | +| `/planning/scenario_planning/trajectory` | autoware_planning_msgs::msg::Trajectory | Output trajectory from planning module. | +| `/control/command/control_cmd` | autoware_control_msgs::msg::Control | Output control command from control module. | +| `/vehicle/status/steering_status` | autoware_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. | +| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | +| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | ### Output topics 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 6bf8fb1c34f5e..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,18 +15,18 @@ #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 -#include -#include -#include +#include +#include +#include #include #include #include @@ -39,9 +39,9 @@ namespace control_performance_analysis { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Control; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::Error; using control_performance_analysis::msg::ErrorStamped; @@ -73,7 +73,7 @@ class ControlPerformanceAnalysisCore // Setters void setCurrentPose(const Pose & msg); void setCurrentWaypoints(const Trajectory & trajectory); - void setCurrentControlValue(const AckermannControlCommand & msg); + void setCurrentControlValue(const Control & msg); void setInterpolatedVars( const Pose & interpolated_pose, const double & interpolated_velocity, const double & interpolated_acceleration, const double & interpolated_steering_angle); @@ -100,10 +100,10 @@ class ControlPerformanceAnalysisCore Params p_; // Variables Received Outside - std::shared_ptr current_trajectory_ptr_; + std::shared_ptr current_trajectory_ptr_; std::shared_ptr current_vec_pose_ptr_; std::shared_ptr> odom_history_ptr_; // velocities at k-2, k-1, k, k+1 - std::shared_ptr current_control_ptr_; + std::shared_ptr current_control_ptr_; std::shared_ptr current_vec_steering_msg_ptr_; // State holder 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 2d5ab8cce002d..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,13 +19,13 @@ #include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" +#include #include #include -#include -#include -#include -#include +#include +#include +#include #include #include @@ -36,9 +36,9 @@ namespace control_performance_analysis { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Control; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::ErrorStamped; using geometry_msgs::msg::PoseStamped; @@ -52,9 +52,8 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node private: // Subscribers and Local Variable Assignment rclcpp::Subscription::SharedPtr sub_trajectory_; // subscribe to trajectory - rclcpp::Subscription::SharedPtr - sub_control_cmd_; // subscribe to steering control value - rclcpp::Subscription::SharedPtr sub_velocity_; // subscribe to velocity + rclcpp::Subscription::SharedPtr sub_control_cmd_; // subscribe to steering control value + rclcpp::Subscription::SharedPtr sub_velocity_; // subscribe to velocity rclcpp::Subscription::SharedPtr sub_vehicle_steering_; // Publishers @@ -68,26 +67,26 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node // Callback Methods void onTrajectory(const Trajectory::ConstSharedPtr msg); - void onControlRaw(const AckermannControlCommand::ConstSharedPtr control_msg); + void onControlRaw(const Control::ConstSharedPtr control_msg); void onVecSteeringMeasured(const SteeringReport::ConstSharedPtr meas_steer_msg); void onVelocity(const Odometry::ConstSharedPtr msg); // Parameters Params param_{}; // wheelbase, control period and feedback coefficients. // State holder - AckermannControlCommand::ConstSharedPtr last_control_cmd_; + Control::ConstSharedPtr last_control_cmd_; double d_control_cmd_{0}; // Subscriber Parameters Trajectory::ConstSharedPtr current_trajectory_ptr_; // ConstPtr to local traj. - AckermannControlCommand::ConstSharedPtr current_control_msg_ptr_; + Control::ConstSharedPtr current_control_msg_ptr_; SteeringReport::ConstSharedPtr current_vec_steering_msg_ptr_; Odometry::ConstSharedPtr current_odom_ptr_; PoseStamped::ConstSharedPtr current_pose_; // pose of the vehicle, x, y, heading // prev states Trajectory prev_traj; - AckermannControlCommand prev_cmd; + Control prev_cmd; SteeringReport prev_steering; // Algorithm diff --git a/control/control_performance_analysis/launch/control_performance_analysis.launch.xml b/control/control_performance_analysis/launch/control_performance_analysis.launch.xml index be2b30bdf1bf9..f2997f0e10b83 100644 --- a/control/control_performance_analysis/launch/control_performance_analysis.launch.xml +++ b/control/control_performance_analysis/launch/control_performance_analysis.launch.xml @@ -8,7 +8,7 @@ - + diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 7003b7d931fa8..878136d837678 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -24,12 +24,14 @@ builtin_interfaces - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + 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 @@ -39,9 +41,7 @@ tf2 tf2_eigen tf2_ros - tier4_autoware_utils tier4_debug_msgs - vehicle_info_util builtin_interfaces global_parameter_loader rosidl_default_runtime 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 f4ed125956bfa..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 @@ -75,9 +75,9 @@ void ControlPerformanceAnalysisCore::setCurrentPose(const Pose & msg) current_vec_pose_ptr_ = std::make_shared(msg); } -void ControlPerformanceAnalysisCore::setCurrentControlValue(const AckermannControlCommand & msg) +void ControlPerformanceAnalysisCore::setCurrentControlValue(const Control & msg) { - current_control_ptr_ = std::make_shared(msg); + current_control_ptr_ = std::make_shared(msg); } std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPointIdx_path_direction() @@ -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/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index 768aed321b4f0..47ce3ce07e83c 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -17,14 +17,14 @@ #include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" -#include +#include #include #include namespace { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::ErrorStamped; @@ -32,7 +32,7 @@ using control_performance_analysis::msg::ErrorStamped; namespace control_performance_analysis { -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfoUtils; ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( const rclcpp::NodeOptions & node_options) @@ -41,7 +41,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( using std::placeholders::_1; // Implement Reading Global and Local Variables. - const auto & vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); + const auto & vehicle_info = VehicleInfoUtils(*this).getVehicleInfo(); param_.wheelbase_ = vehicle_info.wheel_base_m; // Node Parameters. @@ -62,7 +62,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( "~/input/reference_trajectory", 1, std::bind(&ControlPerformanceAnalysisNode::onTrajectory, this, _1)); - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control_raw", 1, std::bind(&ControlPerformanceAnalysisNode::onControlRaw, this, _1)); sub_vehicle_steering_ = create_subscription( @@ -93,8 +93,7 @@ void ControlPerformanceAnalysisNode::onTrajectory(const Trajectory::ConstSharedP current_trajectory_ptr_ = msg; } -void ControlPerformanceAnalysisNode::onControlRaw( - const AckermannControlCommand::ConstSharedPtr control_msg) +void ControlPerformanceAnalysisNode::onControlRaw(const Control::ConstSharedPtr control_msg) { if (last_control_cmd_) { const rclcpp::Duration & duration = diff --git a/control/control_validator/CMakeLists.txt b/control/control_validator/CMakeLists.txt deleted file mode 100644 index fab942c4dc001..0000000000000 --- a/control/control_validator/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(control_validator) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(control_validator_helpers SHARED - src/utils.cpp - src/debug_marker.cpp -) - -# control validator -ament_auto_add_library(control_validator_component SHARED - include/control_validator/control_validator.hpp - src/control_validator.cpp -) -target_link_libraries(control_validator_component control_validator_helpers) -rclcpp_components_register_node(control_validator_component - PLUGIN "control_validator::ControlValidator" - EXECUTABLE control_validator_node -) - -rosidl_generate_interfaces( - ${PROJECT_NAME} - "msg/ControlValidatorStatus.msg" - DEPENDENCIES builtin_interfaces -) - -# to use a message defined in the same package -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(control_validator_component - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(control_validator_component "${cpp_typesupport_target}") -endif() - -# if(BUILD_TESTING) -# endif() - -ament_auto_package( - INSTALL_TO_SHARE - config - launch -) diff --git a/control/control_validator/README.md b/control/control_validator/README.md deleted file mode 100644 index 3d78721a0a040..0000000000000 --- a/control/control_validator/README.md +++ /dev/null @@ -1,58 +0,0 @@ -# Control Validator - -The `control_validator` is a module that checks the validity of the output of the control component. The status of the validation can be viewed in the `/diagnostics` topic. - -![control_validator](./image/control_validator.drawio.svg) - -## Supported features - -The following features are supported for the validation and can have thresholds set by parameters: - -- **Deviation check between reference trajectory and predicted trajectory** : invalid when the largest deviation between the predicted trajectory and reference trajectory is greater than the given threshold. - -![trajectory_deviation](./image/trajectory_deviation.drawio.svg) - -Other features are to be implemented. - -## Inputs/Outputs - -### Inputs - -The `control_validator` takes in the following inputs: - -| Name | Type | Description | -| ------------------------------ | ------------------------------------- | ------------------------------------------------------------------------------ | -| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | -| `~/input/reference_trajectory` | autoware_auto_control_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed | -| `~/input/predicted_trajectory` | autoware_auto_control_msgs/Trajectory | predicted trajectory which is outputted from control module | - -### Outputs - -It outputs the following: - -| Name | Type | Description | -| ---------------------------- | ---------------------------------------- | ------------------------------------------------------------------------- | -| `~/output/validation_status` | control_validator/ControlValidatorStatus | validator status to inform the reason why the trajectory is valid/invalid | -| `/diagnostics` | diagnostic_msgs/DiagnosticStatus | diagnostics to report errors | - -## Parameters - -The following parameters can be set for the `control_validator`: - -### System parameters - -| Name | Type | Description | Default value | -| :--------------------------- | :--- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `publish_diag` | bool | if true, diagnostics msg is published. | true | -| `diag_error_count_threshold` | int | the Diag will be set to ERROR when the number of consecutive invalid trajectory exceeds this threshold. (For example, threshold = 1 means, even if the trajectory is invalid, the Diag will not be ERROR if the next trajectory is valid.) | true | -| `display_on_terminal` | bool | show error msg on terminal | true | - -### Algorithm parameters - -#### Thresholds - -The input trajectory is detected as invalid if the index exceeds the following thresholds. - -| Name | Type | Description | Default value | -| :---------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | -| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0 | diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp deleted file mode 100644 index eba9bf700ba08..0000000000000 --- a/control/control_validator/include/control_validator/control_validator.hpp +++ /dev/null @@ -1,99 +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 CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ -#define CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ - -#include "control_validator/debug_marker.hpp" -#include "control_validator/msg/control_validator_status.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include -#include - -#include -#include -#include - -#include -#include - -namespace control_validator -{ -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using control_validator::msg::ControlValidatorStatus; -using diagnostic_updater::DiagnosticStatusWrapper; -using diagnostic_updater::Updater; -using nav_msgs::msg::Odometry; - -struct ValidationParams -{ - double max_distance_deviation_threshold; -}; - -class ControlValidator : public rclcpp::Node -{ -public: - explicit ControlValidator(const rclcpp::NodeOptions & options); - - void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); - void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); - - bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory); - -private: - void setupDiag(); - - void setupParameters(); - - bool isDataReady(); - - void validate(const Trajectory & trajectory); - - void publishPredictedTrajectory(); - void publishDebugInfo(); - void displayStatus(); - - void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); - - rclcpp::Subscription::SharedPtr sub_kinematics_; - rclcpp::Subscription::SharedPtr sub_reference_traj_; - rclcpp::Subscription::SharedPtr sub_predicted_traj_; - rclcpp::Publisher::SharedPtr pub_status_; - rclcpp::Publisher::SharedPtr pub_markers_; - - // system parameters - int diag_error_count_threshold_ = 0; - bool display_on_terminal_ = true; - - Updater diag_updater_{this}; - - ControlValidatorStatus validation_status_; - ValidationParams validation_params_; // for thresholds - - vehicle_info_util::VehicleInfo vehicle_info_; - - bool isAllValid(const ControlValidatorStatus & status); - - Trajectory::ConstSharedPtr current_reference_trajectory_; - Trajectory::ConstSharedPtr current_predicted_trajectory_; - - Odometry::ConstSharedPtr current_kinematics_; - - std::shared_ptr debug_pose_publisher_; -}; -} // namespace control_validator - -#endif // CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ diff --git a/control/control_validator/include/control_validator/debug_marker.hpp b/control/control_validator/include/control_validator/debug_marker.hpp deleted file mode 100644 index 794912e085949..0000000000000 --- a/control/control_validator/include/control_validator/debug_marker.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ -#define CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ - -#include - -#include -#include -#include - -#include -#include -#include -#include - -class ControlValidatorDebugMarkerPublisher -{ -public: - explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node); - - void pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, - int id = 0); - void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); - void pushVirtualWall(const geometry_msgs::msg::Pose & pose); - void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); - void publish(); - - void clearMarkers(); - -private: - rclcpp::Node * node_; - visualization_msgs::msg::MarkerArray marker_array_; - visualization_msgs::msg::MarkerArray marker_array_virtual_wall_; - rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr virtual_wall_pub_; - std::map marker_id_; - - int getMarkerId(const std::string & ns) - { - if (marker_id_.count(ns) == 0) { - marker_id_[ns] = 0.0; - } - return marker_id_[ns]++; - } -}; - -#endif // CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/control/control_validator/include/control_validator/utils.hpp b/control/control_validator/include/control_validator/utils.hpp deleted file mode 100644 index 8033ac9442960..0000000000000 --- a/control/control_validator/include/control_validator/utils.hpp +++ /dev/null @@ -1,55 +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 CONTROL_VALIDATOR__UTILS_HPP_ -#define CONTROL_VALIDATOR__UTILS_HPP_ - -#include -#include - -#include - -#include -#include -#include - -namespace control_validator -{ -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_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); - -void insertPointInPredictedTrajectory( - TrajectoryPoints & modified_trajectory, const geometry_msgs::msg::Pose & reference_pose, - const TrajectoryPoints & predicted_trajectory); - -TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory); - -bool removeFrontTrajectoryPoint( - const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, - const TrajectoryPoints & predicted_trajectory_points); - -Trajectory alignTrajectoryWithReferenceTrajectory( - const Trajectory & trajectory, const Trajectory & predicted_trajectory); - -double calcMaxLateralDistance( - const Trajectory & trajectory, const Trajectory & predicted_trajectory); -} // namespace control_validator - -#endif // CONTROL_VALIDATOR__UTILS_HPP_ diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml deleted file mode 100644 index faf254708ddbb..0000000000000 --- a/control/control_validator/package.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - control_validator - 0.1.0 - ros node for control_validator - Kyoichi Sugahara - Takamasa Horibe - Makoto Kurihara - Mamoru Sobue - Takayuki Murooka - - Apache License 2.0 - - Kyoichi Sugahara - Takamasa Horibe - Makoto Kurihara - - ament_cmake_auto - autoware_cmake - rosidl_default_generators - - autoware_auto_planning_msgs - diagnostic_updater - geometry_msgs - motion_utils - nav_msgs - rclcpp - rclcpp_components - tier4_autoware_utils - vehicle_info_util - visualization_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - rosidl_default_runtime - rosidl_interface_packages - - - ament_cmake - - diff --git a/control/control_validator/src/debug_marker.cpp b/control/control_validator/src/debug_marker.cpp deleted file mode 100644 index 1b92aee0bb416..0000000000000 --- a/control/control_validator/src/debug_marker.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "control_validator/debug_marker.hpp" - -#include -#include - -#include -#include -#include - -using visualization_msgs::msg::Marker; - -ControlValidatorDebugMarkerPublisher::ControlValidatorDebugMarkerPublisher(rclcpp::Node * node) -: node_(node) -{ - debug_viz_pub_ = - node_->create_publisher("~/debug/marker", 1); - - virtual_wall_pub_ = - node_->create_publisher("~/virtual_wall", 1); -} - -void ControlValidatorDebugMarkerPublisher::clearMarkers() -{ - marker_array_.markers.clear(); - marker_array_virtual_wall_.markers.clear(); -} - -void ControlValidatorDebugMarkerPublisher::pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) -{ - pushPoseMarker(p.pose, ns, id); -} - -void ControlValidatorDebugMarkerPublisher::pushPoseMarker( - const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) -{ - using tier4_autoware_utils::createMarkerColor; - - // append arrow marker - std_msgs::msg::ColorRGBA color; - if (id == 0) // Red - { - color = createMarkerColor(1.0, 0.0, 0.0, 0.999); - } - if (id == 1) // Green - { - color = createMarkerColor(0.0, 1.0, 0.0, 0.999); - } - if (id == 2) // Blue - { - color = createMarkerColor(0.0, 0.0, 1.0, 0.999); - } - Marker marker = tier4_autoware_utils::createDefaultMarker( - "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, - tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3), color); - marker.lifetime = rclcpp::Duration::from_seconds(0.2); - marker.pose = pose; - - marker_array_.markers.push_back(marker); -} - -void ControlValidatorDebugMarkerPublisher::pushWarningMsg( - const geometry_msgs::msg::Pose & pose, const std::string & msg) -{ - visualization_msgs::msg::Marker marker = tier4_autoware_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)); - marker.lifetime = rclcpp::Duration::from_seconds(0.2); - marker.pose = pose; - marker.text = msg; - marker_array_virtual_wall_.markers.push_back(marker); -} - -void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs::msg::Pose & pose) -{ - 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); -} - -void ControlValidatorDebugMarkerPublisher::publish() -{ - debug_viz_pub_->publish(marker_array_); - virtual_wall_pub_->publish(marker_array_virtual_wall_); -} diff --git a/control/control_validator/src/utils.cpp b/control/control_validator/src/utils.cpp deleted file mode 100644 index fab8b9f6f888b..0000000000000 --- a/control/control_validator/src/utils.cpp +++ /dev/null @@ -1,165 +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 "control_validator/utils.hpp" - -#include -#include -#include - -#include -#include -#include -#include - -namespace control_validator -{ - -void shiftPose(Pose & pose, double longitudinal) -{ - const auto yaw = tf2::getYaw(pose.orientation); - pose.position.x += std::cos(yaw) * longitudinal; - pose.position.y += std::sin(yaw) * longitudinal; -} - -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); - modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate); -} - -TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points) -{ - TrajectoryPoints reversed_trajectory_points; - reversed_trajectory_points.reserve(trajectory_points.size()); - std::reverse_copy( - trajectory_points.begin(), trajectory_points.end(), - std::back_inserter(reversed_trajectory_points)); - return reversed_trajectory_points; -} - -bool removeFrontTrajectoryPoint( - const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, - const TrajectoryPoints & predicted_trajectory_points) -{ - 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) { - modified_trajectory_points.erase(modified_trajectory_points.begin()); - - predicted_trajectory_point_removed = true; - } else { - break; - } - } - - return predicted_trajectory_point_removed; -} - -Trajectory alignTrajectoryWithReferenceTrajectory( - const Trajectory & trajectory, const Trajectory & predicted_trajectory) -{ - const auto last_seg_length = motion_utils::calcSignedArcLength( - trajectory.points, trajectory.points.size() - 2, trajectory.points.size() - 1); - - // If no overlapping between trajectory and predicted_trajectory, return empty trajectory - // predicted_trajectory: p1------------------pN - // trajectory: t1------------------tN - // OR - // predicted_trajectory: p1------------------pN - // trajectory: t1------------------tN - const bool & is_p_n_before_t1 = - motion_utils::calcLongitudinalOffsetToSegment( - trajectory.points, 0, predicted_trajectory.points.back().pose.position) < 0.0; - const bool & is_p1_behind_t_n = motion_utils::calcLongitudinalOffsetToSegment( - trajectory.points, trajectory.points.size() - 2, - predicted_trajectory.points.front().pose.position) - - last_seg_length > - 0.0; - const bool is_no_overlapping = (is_p_n_before_t1 || is_p1_behind_t_n); - - if (is_no_overlapping) { - return Trajectory(); - } - - auto modified_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); - auto predicted_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); - auto trajectory_points = convertToTrajectoryPointArray(trajectory); - - // If first point of predicted_trajectory is in front of start of trajectory, erase points which - // are in front of trajectory start point and insert pNew along the predicted_trajectory - // predicted_trajectory: p1-----p2-----p3----//------pN - // trajectory: t1--------//------tN - // ↓ - // predicted_trajectory: pNew--p3----//------pN - // trajectory: t1--------//------tN - auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( - trajectory_points, modified_trajectory_points, predicted_trajectory_points); - - if (predicted_trajectory_point_removed) { - insertPointInPredictedTrajectory( - modified_trajectory_points, trajectory_points.front().pose, predicted_trajectory_points); - } - - // If last point of predicted_trajectory is behind of end of trajectory, erase points which are - // behind trajectory last point and insert pNew along the predicted_trajectory - // predicted_trajectory: p1-----//------pN-2-----pN-1-----pN - // trajectory: t1-----//-----tN-1--tN - // ↓ - // predicted_trajectory: p1-----//------pN-2-pNew - // trajectory: t1-----//-----tN-1--tN - - auto reversed_predicted_trajectory_points = reverseTrajectoryPoints(predicted_trajectory_points); - auto reversed_trajectory_points = reverseTrajectoryPoints(trajectory_points); - auto reversed_modified_trajectory_points = reverseTrajectoryPoints(modified_trajectory_points); - - auto reversed_predicted_trajectory_point_removed = removeFrontTrajectoryPoint( - reversed_trajectory_points, reversed_modified_trajectory_points, - reversed_predicted_trajectory_points); - - if (reversed_predicted_trajectory_point_removed) { - insertPointInPredictedTrajectory( - reversed_modified_trajectory_points, reversed_trajectory_points.front().pose, - reversed_predicted_trajectory_points); - } - - return convertToTrajectory(reverseTrajectoryPoints(reversed_modified_trajectory_points)); -} - -double calcMaxLateralDistance( - const Trajectory & reference_trajectory, const Trajectory & predicted_trajectory) -{ - const auto alined_predicted_trajectory = - 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); - // 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)); - if (temp_dist > max_dist) { - max_dist = temp_dist; - } - } - return max_dist; -} - -} // namespace control_validator diff --git a/control/external_cmd_selector/CMakeLists.txt b/control/external_cmd_selector/CMakeLists.txt deleted file mode 100644 index 5c4860b7441f3..0000000000000 --- a/control/external_cmd_selector/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(external_cmd_selector) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(external_cmd_selector_node SHARED - src/external_cmd_selector/external_cmd_selector_node.cpp -) - -rclcpp_components_register_node(external_cmd_selector_node - PLUGIN "ExternalCmdSelector" - EXECUTABLE external_cmd_selector -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/control/external_cmd_selector/README.md b/control/external_cmd_selector/README.md deleted file mode 100644 index 93ded5713a25b..0000000000000 --- a/control/external_cmd_selector/README.md +++ /dev/null @@ -1,34 +0,0 @@ -# external_cmd_selector - -## Purpose - -`external_cmd_selector` is the package to publish `external_control_cmd`, `gear_cmd`, `hazard_lights_cmd`, `heartbeat` and `turn_indicators_cmd`, according to the current mode, which is `remote` or `local`. - -The current mode is set via service, `remote` is remotely operated, `local` is to use the values calculated by Autoware. - -## Input / Output - -### Input topics - -| Name | Type | Description | -| ---------------------------------------------- | ---- | ------------------------------------------------------- | -| `/api/external/set/command/local/control` | TBD | Local. Calculated control value. | -| `/api/external/set/command/local/heartbeat` | TBD | Local. Heartbeat. | -| `/api/external/set/command/local/shift` | TBD | Local. Gear shift like drive, rear and etc. | -| `/api/external/set/command/local/turn_signal` | TBD | Local. Turn signal like left turn, right turn and etc. | -| `/api/external/set/command/remote/control` | TBD | Remote. Calculated control value. | -| `/api/external/set/command/remote/heartbeat` | TBD | Remote. Heartbeat. | -| `/api/external/set/command/remote/shift` | TBD | Remote. Gear shift like drive, rear and etc. | -| `/api/external/set/command/remote/turn_signal` | TBD | Remote. Turn signal like left turn, right turn and etc. | - -### Output topics - -| Name | Type | Description | -| ------------------------------------------------------ | ------------------------------------------------------ | ----------------------------------------------- | -| `/control/external_cmd_selector/current_selector_mode` | TBD | Current selected mode, remote or local. | -| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Check if node is active or not. | -| `/external/selected/external_control_cmd` | TBD | Pass through control command with current mode. | -| `/external/selected/gear_cmd` | autoware_auto_vehicle_msgs::msg::GearCommand | Pass through gear command with current mode. | -| `/external/selected/hazard_lights_cmd` | autoware_auto_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | -| `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | -| `/external/selected/turn_indicators_cmd` | autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | diff --git a/control/external_cmd_selector/package.xml b/control/external_cmd_selector/package.xml deleted file mode 100644 index 4d8cc5a385494..0000000000000 --- a/control/external_cmd_selector/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - external_cmd_selector - 0.1.0 - The external_cmd_selector package - Taiki Tanaka - Tomoya Kimura - Shumpei Wakabayashi - Fumiya Watanabe - Takamasa Horibe - Satoshi Ota - Takayuki Murooka - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_auto_vehicle_msgs - diagnostic_updater - geometry_msgs - rclcpp - rclcpp_components - std_msgs - tier4_auto_msgs_converter - tier4_control_msgs - tier4_external_api_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/control/joy_controller/CMakeLists.txt b/control/joy_controller/CMakeLists.txt deleted file mode 100644 index f000c8c0909f4..0000000000000 --- a/control/joy_controller/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(joy_controller) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(joy_controller_node SHARED - src/joy_controller/joy_controller_node.cpp -) - -rclcpp_components_register_node(joy_controller_node - PLUGIN "joy_controller::AutowareJoyControllerNode" - EXECUTABLE joy_controller -) - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md deleted file mode 100644 index 4674ebdadb51d..0000000000000 --- a/control/joy_controller/README.md +++ /dev/null @@ -1,125 +0,0 @@ -# joy_controller - -## Role - -`joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle. - -## Usage - -### ROS 2 launch - -```bash -# With default config (ds4) -ros2 launch joy_controller joy_controller.launch.xml - -# Default config but select from the existing parameter files -ros2 launch joy_controller joy_controller_param_selection.launch.xml joy_type:=ds4 # or g29, p65, xbox - -# Override the param file -ros2 launch joy_controller joy_controller.launch.xml config_file:=/path/to/your/param.yaml -``` - -## Input / Output - -### Input topics - -| Name | Type | Description | -| ------------------ | ----------------------- | --------------------------------- | -| `~/input/joy` | sensor_msgs::msg::Joy | joy controller command | -| `~/input/odometry` | nav_msgs::msg::Odometry | ego vehicle odometry to get twist | - -### Output topics - -| Name | Type | Description | -| ----------------------------------- | -------------------------------------------------------- | ---------------------------------------- | -| `~/output/control_command` | autoware_auto_control_msgs::msg::AckermannControlCommand | lateral and longitudinal control command | -| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command | -| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command | -| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command | -| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) | -| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat | -| `~/output/vehicle_engage` | autoware_auto_vehicle_msgs::msg::Engage | vehicle engage | - -## Parameters - -| Parameter | Type | Description | -| ------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------ | -| `joy_type` | string | joy controller type (default: DS4) | -| `update_rate` | double | update rate to publish control commands | -| `accel_ratio` | double | ratio to calculate acceleration (commanded acceleration is ratio \* operation) | -| `brake_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | -| `steer_ratio` | double | ratio to calculate deceleration (commanded steer is ratio \* operation) | -| `steering_angle_velocity` | double | steering angle velocity for operation | -| `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | -| `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | -| `raw_control` | bool | skip input odometry if true | -| `velocity_gain` | double | ratio to calculate velocity by acceleration | -| `max_forward_velocity` | double | absolute max velocity to go forward | -| `max_backward_velocity` | double | absolute max velocity to go backward | -| `backward_accel_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | - -## P65 Joystick Key Map - -| Action | Button | -| -------------------- | --------------------- | -| Acceleration | R2 | -| Brake | L2 | -| Steering | Left Stick Left Right | -| Shift up | Cursor Up | -| Shift down | Cursor Down | -| Shift Drive | Cursor Left | -| Shift Reverse | Cursor Right | -| Turn Signal Left | L1 | -| Turn Signal Right | R1 | -| Clear Turn Signal | A | -| Gate Mode | B | -| Emergency Stop | Select | -| Clear Emergency Stop | Start | -| Autoware Engage | X | -| Autoware Disengage | Y | -| Vehicle Engage | PS | -| Vehicle Disengage | Right Trigger | - -## DS4 Joystick Key Map - -| Action | Button | -| -------------------- | -------------------------- | -| Acceleration | R2, ×, or Right Stick Up | -| Brake | L2, □, or Right Stick Down | -| Steering | Left Stick Left Right | -| Shift up | Cursor Up | -| Shift down | Cursor Down | -| Shift Drive | Cursor Left | -| Shift Reverse | Cursor Right | -| Turn Signal Left | L1 | -| Turn Signal Right | R1 | -| Clear Turn Signal | SHARE | -| Gate Mode | OPTIONS | -| Emergency Stop | PS | -| Clear Emergency Stop | PS | -| Autoware Engage | ○ | -| Autoware Disengage | ○ | -| Vehicle Engage | △ | -| Vehicle Disengage | △ | - -## XBOX Joystick Key Map - -| Action | Button | -| -------------------- | --------------------- | -| Acceleration | RT | -| Brake | LT | -| Steering | Left Stick Left Right | -| Shift up | Cursor Up | -| Shift down | Cursor Down | -| Shift Drive | Cursor Left | -| Shift Reverse | Cursor Right | -| Turn Signal Left | LB | -| Turn Signal Right | RB | -| Clear Turn Signal | A | -| Gate Mode | B | -| Emergency Stop | View | -| Clear Emergency Stop | Menu | -| Autoware Engage | X | -| Autoware Disengage | Y | -| Vehicle Engage | Left Stick Button | -| Vehicle Disengage | Right Stick Button | diff --git a/control/joy_controller/launch/joy_controller_param_selection.launch.xml b/control/joy_controller/launch/joy_controller_param_selection.launch.xml deleted file mode 100644 index d8e3d0bfe8b25..0000000000000 --- a/control/joy_controller/launch/joy_controller_param_selection.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/control/joy_controller/package.xml b/control/joy_controller/package.xml deleted file mode 100644 index 79ee9f27868f2..0000000000000 --- a/control/joy_controller/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - joy_controller - 0.1.0 - The joy_controller package - Taiki Tanaka - Tomoya Kimura - Shumpei Wakabayashi - Fumiya Watanabe - Takamasa Horibe - Satoshi Ota - Takayuki Murooka - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_vehicle_msgs - geometry_msgs - joy - nav_msgs - rclcpp - rclcpp_components - sensor_msgs - std_srvs - tier4_api_utils - tier4_control_msgs - tier4_external_api_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/control/lane_departure_checker/CMakeLists.txt b/control/lane_departure_checker/CMakeLists.txt deleted file mode 100644 index 874670c478240..0000000000000 --- a/control/lane_departure_checker/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(lane_departure_checker) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -include_directories( - include - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -ament_auto_add_library(lane_departure_checker SHARED - src/lane_departure_checker_node/lane_departure_checker.cpp - src/lane_departure_checker_node/lane_departure_checker_node.cpp -) - -rclcpp_components_register_node(lane_departure_checker - PLUGIN "lane_departure_checker::LaneDepartureCheckerNode" - EXECUTABLE lane_departure_checker_node -) - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md deleted file mode 100644 index 67b770aefb3d2..0000000000000 --- a/control/lane_departure_checker/README.md +++ /dev/null @@ -1,101 +0,0 @@ -# Lane Departure Checker - -The **Lane Departure Checker** checks if vehicle follows a trajectory. If it does not follow the trajectory, it reports its status via `diagnostic_updater`. - -## Features - -This package includes the following features: - -- **Lane Departure**: Check if ego vehicle is going to be out of lane boundaries based on output from control module (predicted trajectory). -- **Trajectory Deviation**: Check if ego vehicle's pose does not deviate from the trajectory. Checking lateral, longitudinal and yaw deviation. -- **Road Border Departure**: Check if ego vehicle's footprint, generated from the control's output, extends beyond the road border. - -## Inner-workings / Algorithms - -### How to extend footprint by covariance - -1. Calculate the standard deviation of error ellipse(covariance) in vehicle coordinate. - - 1.Transform covariance into vehicle coordinate. - - $$ - \begin{align} - \left( \begin{array}{cc} x_{vehicle}\\ y_{vehicle}\\ \end{array} \right) = R_{map2vehicle} \left( \begin{array}{cc} x_{map}\\ y_{map}\\ \end{array} \right) - \end{align} - $$ - - Calculate covariance in vehicle coordinate. - - $$ - \begin{align} - Cov_{vehicle} &= E \left[ - \left( \begin{array}{cc} x_{vehicle}\\ y_{vehicle}\\ \end{array} \right) (x_{vehicle}, y_{vehicle}) \right] \\ - &= E \left[ R\left( \begin{array}{cc} x_{map}\\ y_{map}\\ \end{array} \right) - (x_{map}, y_{map})R^t - \right] \\ - &= R E\left[ \left( \begin{array}{cc} x_{map}\\ y_{map}\\ \end{array} \right) - (x_{map}, y_{map}) - \right] R^t \\ - &= R Cov_{map} R^t - \end{align} - $$ - - 2.The longitudinal length we want to expand is correspond to marginal distribution of $x_{vehicle}$, which is represented in $Cov_{vehicle}(0,0)$. In the same way, the lateral length is represented in $Cov_{vehicle}(1,1)$. Wikipedia reference [here](https://en.wikipedia.org/wiki/Multivariate_normal_distribution#Marginal_distributions). - -2. Expand footprint based on the standard deviation multiplied with `footprint_margin_scale`. - -## Interface - -### Input - -- /localization/kinematic_state [`nav_msgs::msg::Odometry`] -- /map/vector_map [`autoware_auto_mapping_msgs::msg::HADMapBin`] -- /planning/mission_planning/route [`autoware_planning_msgs::msg::LaneletRoute`] -- /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs::msg::Trajectory`] -- /control/trajectory_follower/predicted_trajectory [`autoware_auto_planning_msgs::msg::Trajectory`] - -### Output - -- [`diagnostic_updater`] lane_departure : Update diagnostic level when ego vehicle is out of lane. -- [`diagnostic_updater`] trajectory_deviation : Update diagnostic level when ego vehicle deviates from trajectory. - -## Parameters - -### Node Parameters - -#### General Parameters - -| Name | Type | Description | Default value | -| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | -| will_out_of_lane_checker | bool | Enable checker whether ego vehicle footprint will depart from lane | True | -| out_of_lane_checker | bool | Enable checker whether ego vehicle footprint is out of lane | True | -| boundary_departure_checker | bool | Enable checker whether ego vehicle footprint wil depart from boundary specified by boundary_types_to_detect | False | -| update_rate | double | Frequency for publishing [Hz] | 10.0 | -| visualize_lanelet | bool | Flag for visualizing lanelet | False | - -#### Parameters For Lane Departure - -| Name | Type | Description | Default value | -| :------------------------ | :--- | :------------------------------------------------ | :------------ | -| include_right_lanes | bool | Flag for including right lanelet in borders | False | -| include_left_lanes | bool | Flag for including left lanelet in borders | False | -| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | -| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | - -#### Parameters For Road Border Departure - -| Name | Type | Description | Default value | -| :----------------------- | :------------------------- | :---------------------------------------------------------- | :------------ | -| boundary_types_to_detect | std::vector\ | line_string types to detect with boundary_departure_checker | [road_border] | - -### Core Parameters - -| Name | Type | Description | Default value | -| :------------------------- | :----- | :--------------------------------------------------------------------------------- | :------------ | -| footprint_margin_scale | double | Coefficient for expanding footprint margin. Multiplied by 1 standard deviation. | 1.0 | -| resample_interval | double | Minimum Euclidean distance between points when resample trajectory.[m] | 0.3 | -| max_deceleration | double | Maximum deceleration when calculating braking distance. | 2.8 | -| delay_time | double | Delay time which took to actuate brake when calculating braking distance. [second] | 1.3 | -| max_lateral_deviation | double | Maximum lateral deviation in vehicle coordinate. [m] | 2.0 | -| max_longitudinal_deviation | double | Maximum longitudinal deviation in vehicle coordinate. [m] | 2.0 | -| max_yaw_deviation_deg | double | Maximum ego yaw deviation from trajectory. [deg] | 60.0 | diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp deleted file mode 100644 index dd05ab226f6b7..0000000000000 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ /dev/null @@ -1,154 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ -#define LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ - -#include "lane_departure_checker/lane_departure_checker.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -namespace lane_departure_checker -{ -using autoware_auto_mapping_msgs::msg::HADMapBin; - -struct NodeParam -{ - bool will_out_of_lane_checker; - bool out_of_lane_checker; - bool boundary_departure_checker; - - double update_rate; - bool visualize_lanelet; - bool include_right_lanes; - bool include_left_lanes; - bool include_opposite_lanes; - bool include_conflicting_lanes; - std::vector boundary_types_to_detect; -}; - -class LaneDepartureCheckerNode : public rclcpp::Node -{ -public: - explicit LaneDepartureCheckerNode(const rclcpp::NodeOptions & options); - -private: - // Subscriber - rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; - rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; - - // Data Buffer - nav_msgs::msg::Odometry::ConstSharedPtr current_odom_; - lanelet::LaneletMapPtr lanelet_map_; - lanelet::ConstLanelets shoulder_lanelets_; - lanelet::traffic_rules::TrafficRulesPtr traffic_rules_; - lanelet::routing::RoutingGraphPtr routing_graph_; - LaneletRoute::ConstSharedPtr route_; - geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr cov_; - LaneletRoute::ConstSharedPtr last_route_; - lanelet::ConstLanelets route_lanelets_; - Trajectory::ConstSharedPtr reference_trajectory_; - Trajectory::ConstSharedPtr predicted_trajectory_; - - // Callback - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onLaneletMapBin(const HADMapBin::ConstSharedPtr msg); - void onRoute(const LaneletRoute::ConstSharedPtr msg); - void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); - void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); - - // Publisher - tier4_autoware_utils::DebugPublisher debug_publisher_{this, "~/debug"}; - tier4_autoware_utils::ProcessingTimePublisher processing_time_publisher_{this}; - - // Timer - rclcpp::TimerBase::SharedPtr timer_; - - bool isDataReady(); - bool isDataTimeout(); - bool isDataValid(); - void onTimer(); - - // Parameter - NodeParam node_param_; - Param param_; - double vehicle_length_m_; - - // Parameter callback - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - rcl_interfaces::msg::SetParametersResult onParameter( - const std::vector & parameters); - - // Core - Input input_{}; - Output output_{}; - std::unique_ptr lane_departure_checker_; - - // Diagnostic Updater - diagnostic_updater::Updater updater_{this}; - - void checkLaneDeparture(diagnostic_updater::DiagnosticStatusWrapper & stat); - void checkTrajectoryDeviation(diagnostic_updater::DiagnosticStatusWrapper & stat); - - // Visualization - visualization_msgs::msg::MarkerArray createMarkerArray() const; - - // Lanelet Neighbor Search - lanelet::ConstLanelets getAllSharedLineStringLanelets( - const lanelet::ConstLanelet & current_lane, const bool is_right, const bool is_left, - const bool is_opposite, const bool is_conflicting, const bool & invert_opposite); - - lanelet::ConstLanelets getAllRightSharedLinestringLanelets( - const lanelet::ConstLanelet & lane, const bool & include_opposite, - const bool & invert_opposite = false); - - lanelet::ConstLanelets getAllLeftSharedLinestringLanelets( - const lanelet::ConstLanelet & lane, const bool & include_opposite, - const bool & invert_opposite = false); - - boost::optional getLeftLanelet(const lanelet::ConstLanelet & lanelet); - - lanelet::Lanelets getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet); - boost::optional getRightLanelet( - const lanelet::ConstLanelet & lanelet) const; - - lanelet::Lanelets getRightOppositeLanelets(const lanelet::ConstLanelet & lanelet); -}; -} // namespace lane_departure_checker - -#endif // LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml deleted file mode 100644 index 1059e86cadc6d..0000000000000 --- a/control/lane_departure_checker/package.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - - lane_departure_checker - 0.1.0 - The lane_departure_checker package - Kyoichi Sugahara - Makoto Kurihara - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_planning_msgs - boost - diagnostic_updater - eigen - geometry_msgs - lanelet2_extension - motion_utils - nav_msgs - rclcpp - rclcpp_components - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - vehicle_info_util - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt deleted file mode 100644 index e4a3683ea1fdc..0000000000000 --- a/control/mpc_lateral_controller/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(mpc_lateral_controller) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(steering_offset_lib SHARED - src/steering_offset/steering_offset.cpp -) - -set(MPC_LAT_CON_LIB ${PROJECT_NAME}_lib) -ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED - src/mpc_lateral_controller.cpp - src/lowpass_filter.cpp - src/steering_predictor.cpp - src/mpc.cpp - src/mpc_trajectory.cpp - src/mpc_utils.cpp - src/qp_solver/qp_solver_osqp.cpp - src/qp_solver/qp_solver_unconstraint_fast.cpp - src/vehicle_model/vehicle_model_bicycle_dynamics.cpp - src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp - src/vehicle_model/vehicle_model_bicycle_kinematics.cpp - src/vehicle_model/vehicle_model_interface.cpp -) -target_link_libraries(${MPC_LAT_CON_LIB} steering_offset_lib) - -if(BUILD_TESTING) - set(TEST_LAT_SOURCES - test/test_mpc.cpp - test/test_mpc_utils.cpp - test/test_lowpass_filter.cpp - ) - set(TEST_LATERAL_CONTROLLER_EXE test_lateral_controller) - ament_add_ros_isolated_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES}) - target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${MPC_LAT_CON_LIB}) -endif() - -ament_auto_package(INSTALL_TO_SHARE - param -) diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md deleted file mode 100644 index 6bfba5818bd06..0000000000000 --- a/control/mpc_lateral_controller/README.md +++ /dev/null @@ -1,266 +0,0 @@ -# MPC Lateral Controller - -This is the design document for the lateral controller node -in the `trajectory_follower_node` package. - -## Purpose / Use cases - - - - -This node is used to general lateral control commands (steering angle and steering rate) -when following a path. - -## Design - - - - -The node uses an implementation of linear model predictive control (MPC) for accurate path tracking. -The MPC uses a model of the vehicle to simulate the trajectory resulting from the control command. -The optimization of the control command is formulated as a Quadratic Program (QP). - -Different vehicle models are implemented: - -- kinematics : bicycle kinematics model with steering 1st-order delay. -- kinematics_no_delay : bicycle kinematics model without steering delay. -- dynamics : bicycle dynamics model considering slip angle. - The kinematics model is being used by default. Please see the reference [1] for more details. - -For the optimization, a Quadratic Programming (QP) solver is used and two options are currently implemented: - - - -- unconstraint_fast : use least square method to solve unconstraint QP with eigen. -- [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) - algorithm (for more details see the related papers at - the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section): - -### Filtering - -Filtering is required for good noise reduction. -A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is employed for processing the yaw and lateral errors, which are used as inputs for the MPC, as well as for refining the output steering angle. -Other filtering methods can be considered as long as the noise reduction performances are good -enough. -The moving average filter for example is not suited and can yield worse results than without any -filtering. - -## Assumptions / Known limits - - - -The tracking is not accurate if the first point of the reference trajectory is at or in front of the current ego pose. - -## Inputs / Outputs / API - - - - -### Inputs - -Set the following from the [controller_node](../trajectory_follower_node/README.md) - -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. -- `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport`: current steering - -### Outputs - -Return LateralOutput which contains the following to the controller node - -- `autoware_auto_control_msgs/AckermannLateralCommand` -- LateralSyncData - - steer angle convergence - -### MPC class - -The `MPC` class (defined in `mpc.hpp`) provides the interface with the MPC algorithm. -Once a vehicle model, a QP solver, and the reference trajectory to follow have been set -(using `setVehicleModel()`, `setQPSolver()`, `setReferenceTrajectory()`), a lateral control command -can be calculated by providing the current steer, velocity, and pose to function `calculateMPC()`. - -### Parameter description - -The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the -AutonomouStuff Lexus RX 450h for under 40 km/h driving. - -#### System - -| Name | Type | Description | Default value | -| :------------------------ | :------ | :-------------------------------------------------------------------------- | :------------ | -| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | -| use_steer_prediction | boolean | flag for using steer prediction (do not use steer measurement) | false | -| admissible_position_error | double | stop vehicle when following position error is larger than this value [m] | 5.0 | -| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad] | 1.57 | - -#### Path Smoothing - -| Name | Type | Description | Default value | -| :-------------------------------- | :------ | :--------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_path_smoothing | boolean | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | false | -| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 25 | -| curvature_smoothing_num_traj | int | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 | -| curvature_smoothing_num_ref_steer | int | index distance of points used in curvature calculation for reference steering command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 | - -#### Trajectory Extending - -| Name | Type | Description | Default value | -| :------------------------------------ | :------ | :-------------------------------------------- | :------------ | -| extend_trajectory_for_end_yaw_control | boolean | trajectory extending flag for end yaw control | true | - -#### MPC Optimization - -| Name | Type | Description | Default value | -| :-------------------------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------- | :------------ | -| qp_solver_type | string | QP solver option. described below in detail. | "osqp" | -| mpc_prediction_horizon | int | total prediction step for MPC | 50 | -| mpc_prediction_dt | double | prediction period for one step [s] | 0.1 | -| mpc_weight_lat_error | double | weight for lateral error | 1.0 | -| mpc_weight_heading_error | double | weight for heading error | 0.0 | -| mpc_weight_heading_error_squared_vel | double | weight for heading error \* velocity | 0.3 | -| mpc_weight_steering_input | double | weight for steering error (steer command - reference steer) | 1.0 | -| mpc_weight_steering_input_squared_vel | double | weight for steering error (steer command - reference steer) \* velocity | 0.25 | -| mpc_weight_lat_jerk | double | weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.1 | -| mpc_weight_steer_rate | double | weight for steering rate [rad/s] | 0.0 | -| mpc_weight_steer_acc | double | weight for derivatives of the steering rate [rad/ss] | 0.000001 | -| mpc_low_curvature_weight_lat_error | double | [used in a low curvature trajectory] weight for lateral error | 0.1 | -| mpc_low_curvature_weight_heading_error | double | [used in a low curvature trajectory] weight for heading error | 0.0 | -| mpc_low_curvature_weight_heading_error_squared_vel | double | [used in a low curvature trajectory] weight for heading error \* velocity | 0.3 | -| mpc_low_curvature_weight_steering_input | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) | 1.0 | -| mpc_low_curvature_weight_steering_input_squared_vel | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) \* velocity | 0.25 | -| mpc_low_curvature_weight_lat_jerk | double | [used in a low curvature trajectory] weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | -| mpc_low_curvature_weight_steer_rate | double | [used in a low curvature trajectory] weight for steering rate [rad/s] | 0.0 | -| mpc_low_curvature_weight_steer_acc | double | [used in a low curvature trajectory] weight for derivatives of the steering rate [rad/ss] | 0.000001 | -| mpc_low_curvature_thresh_curvature | double | threshold of curvature to use "low_curvature" parameter | 0.0 | -| mpc_weight_terminal_lat_error | double | terminal lateral error weight in matrix Q to improve mpc stability | 1.0 | -| mpc_weight_terminal_heading_error | double | terminal heading error weight in matrix Q to improve mpc stability | 0.1 | -| mpc_zero_ff_steer_deg | double | threshold that feed-forward angle becomes zero | 0.5 | -| mpc_acceleration_limit | double | limit on the vehicle's acceleration | 2.0 | -| mpc_velocity_time_constant | double | time constant used for velocity smoothing | 0.3 | -| mpc_min_prediction_length | double | minimum prediction length | 5.0 | - -#### Vehicle Model - -| Name | Type | Description | Default value | -| :----------------------------------- | :------- | :--------------------------------------------------------------------------------- | :------------------- | -| vehicle_model_type | string | vehicle model type for mpc prediction | "kinematics" | -| input_delay | double | steering input delay time for delay compensation | 0.24 | -| vehicle_model_steer_tau | double | steering dynamics time constant (1d approximation) [s] | 0.3 | -| steer_rate_lim_dps_list_by_curvature | [double] | steering angle rate limit list depending on curvature [deg/s] | [40.0, 50.0, 60.0] | -| curvature_list_for_steer_rate_lim | [double] | curvature list for steering angle rate limit interpolation in ascending order [/m] | [0.001, 0.002, 0.01] | -| steer_rate_lim_dps_list_by_velocity | [double] | steering angle rate limit list depending on velocity [deg/s] | [60.0, 50.0, 40.0] | -| velocity_list_for_steer_rate_lim | [double] | velocity list for steering angle rate limit interpolation in ascending order [m/s] | [10.0, 15.0, 20.0] | -| acceleration_limit | double | acceleration limit for trajectory velocity modification [m/ss] | 2.0 | -| velocity_time_constant | double | velocity dynamics time constant for trajectory velocity modification [s] | 0.3 | - -#### Lowpass Filter for Noise Reduction - -| Name | Type | Description | Default value | -| :------------------------ | :----- | :------------------------------------------------------------------ | :------------ | -| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 | -| error_deriv_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for error derivative [Hz] | 5.0 | - -#### Stop State - -| Name | Type | Description | Default value | -| :------------------------------------------- | :------ | :---------------------------------------------------------------------------------------------- | :------------ | -| stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.001 | -| stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.001 | -| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | -| keep_steer_control_until_converged | boolean | keep steer control until steer is converged | true | -| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | -| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | -| mpc_converged_threshold_rps | double | threshold value to be sure output of the optimization is converged, it is used in stopped state | 0.01 | - -(\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state. - -#### Steer Offset - -Defined in the `steering_offset` namespace. This logic is designed as simple as possible, with minimum design parameters. - -| Name | Type | Description | Default value | -| :---------------------------------- | :------ | :----------------------------------------------------------------------------------------------- | :------------ | -| enable_auto_steering_offset_removal | boolean | Estimate the steering offset and apply compensation | true | -| update_vel_threshold | double | If the velocity is smaller than this value, the data is not used for the offset estimation | 5.56 | -| update_steer_threshold | double | If the steering angle is larger than this value, the data is not used for the offset estimation. | 0.035 | -| average_num | int | The average of this number of data is used as a steering offset. | 1000 | -| steering_offset_limit | double | The angle limit to be applied to the offset compensation. | 0.02 | - -##### For dynamics model (WIP) - -| Name | Type | Description | Default value | -| :------------ | :----- | :------------------------------------------ | :------------ | -| cg_to_front_m | double | distance from baselink to the front axle[m] | 1.228 | -| cg_to_rear_m | double | distance from baselink to the rear axle [m] | 1.5618 | -| mass_fl | double | mass applied to front left tire [kg] | 600 | -| mass_fr | double | mass applied to front right tire [kg] | 600 | -| mass_rl | double | mass applied to rear left tire [kg] | 600 | -| mass_rr | double | mass applied to rear right tire [kg] | 600 | -| cf | double | front cornering power [N/rad] | 155494.663 | -| cr | double | rear cornering power [N/rad] | 155494.663 | - -### How to tune MPC parameters - -#### Set kinematics information - -First, it's important to set the appropriate parameters for vehicle kinematics. This includes parameters like `wheelbase`, which represents the distance between the front and rear wheels, and `max_steering_angle`, which indicates the maximum tire steering angle. These parameters should be set in the `vehicle_info.param.yaml`. - -#### Set dynamics information - -Next, you need to set the proper parameters for the dynamics model. These include the time constant `steering_tau` and time delay `steering_delay` for steering dynamics, and the maximum acceleration `mpc_acceleration_limit` and the time constant `mpc_velocity_time_constant` for velocity dynamics. - -#### Confirmation of the input information - -It's also important to make sure the input information is accurate. Information such as the velocity of the center of the rear wheel [m/s] and the steering angle of the tire [rad] is required. Please note that there have been frequent reports of performance degradation due to errors in input information. For instance, there are cases where the velocity of the vehicle is offset due to an unexpected difference in tire radius, or the tire angle cannot be accurately measured due to a deviation in the steering gear ratio or midpoint. It is suggested to compare information from multiple sensors (e.g., integrated vehicle speed and GNSS position, steering angle and IMU angular velocity), and ensure the input information for MPC is appropriate. - -#### MPC weight tuning - -Then, tune the weights of the MPC. One simple approach of tuning is to keep the weight for the lateral deviation (`weight_lat_error`) constant, and vary the input weight (`weight_steering_input`) while observing the trade-off between steering oscillation and control accuracy. - -Here, `weight_lat_error` acts to suppress the lateral error in path following, while `weight_steering_input` works to adjust the steering angle to a standard value determined by the path's curvature. When `weight_lat_error` is large, the steering moves significantly to improve accuracy, which can cause oscillations. On the other hand, when `weight_steering_input` is large, the steering doesn't respond much to tracking errors, providing stable driving but potentially reducing tracking accuracy. - -The steps are as follows: - -1. Set `weight_lat_error` = 0.1, `weight_steering_input` = 1.0 and other weights to 0. -2. If the vehicle oscillates when driving, set `weight_steering_input` larger. -3. If the tracking accuracy is low, set `weight_steering_input` smaller. - -If you want to adjust the effect only in the high-speed range, you can use `weight_steering_input_squared_vel`. This parameter corresponds to the steering weight in the high-speed range. - -#### Descriptions for weights - -- `weight_lat_error`: Reduce lateral tracking error. This acts like P gain in PID. -- `weight_heading_error`: Make a drive straight. This acts like D gain in PID. -- `weight_heading_error_squared_vel_coeff` : Make a drive straight in high speed range. -- `weight_steering_input`: Reduce oscillation of tracking. -- `weight_steering_input_squared_vel_coeff`: Reduce oscillation of tracking in high speed range. -- `weight_lat_jerk`: Reduce lateral jerk. -- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability. -- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability. - -#### Other tips for tuning - -Here are some tips for adjusting other parameters: - -- In theory, increasing terminal weights, `weight_terminal_lat_error` and `weight_terminal_heading_error`, can enhance the tracking stability. This method sometimes proves effective. -- A larger `prediction_horizon` and a smaller `prediction_sampling_time` are efficient for tracking performance. However, these come at the cost of higher computational costs. -- If you want to modify the weight according to the trajectory curvature (for instance, when you're driving on a sharp curve and want a larger weight), use `mpc_low_curvature_thresh_curvature` and adjust `mpc_low_curvature_weight_**` weights. -- If you want to adjust the steering rate limit based on the vehicle speed and trajectory curvature, you can modify the values of `steer_rate_lim_dps_list_by_curvature`, `curvature_list_for_steer_rate_lim`, `steer_rate_lim_dps_list_by_velocity`, `velocity_list_for_steer_rate_lim`. By doing this, you can enforce the steering rate limit during high-speed driving or relax it while curving. -- In case your target curvature appears jagged, adjusting `curvature_smoothing` becomes critically important for accurate curvature calculations. A larger value yields a smooth curvature calculation which reduces noise but can cause delay in feedforward computation and potentially degrade performance. -- Adjusting the `steering_lpf_cutoff_hz` value can also be effective to forcefully reduce computational noise. This refers to the cutoff frequency in the second order Butterworth filter installed in the final layer. The smaller the cutoff frequency, the stronger the noise reduction, but it also induce operation delay. -- If the vehicle consistently deviates laterally from the trajectory, it's most often due to the offset of the steering sensor or self-position estimation. It's preferable to eliminate these biases before inputting into MPC, but it's also possible to remove this bias within MPC. To utilize this, set `enable_auto_steering_offset_removal` to true and activate the steering offset remover. The steering offset estimation logic works when driving at high speeds with the steering close to the center, applying offset removal. -- If the onset of steering in curves is late, it's often due to incorrect delay time and time constant in the steering model. Please recheck the values of `input_delay` and `vehicle_model_steer_tau`. Additionally, as a part of its debug information, MPC outputs the current steering angle assumed by the MPC model, so please check if that steering angle matches the actual one. - -## References / External links - - - -- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", - Robotics Institute, Carnegie Mellon University, February 2009. - -## Related issues - - diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp deleted file mode 100644 index 227bd0594312b..0000000000000 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright 2018-2021 The Autoware Foundation -// -// 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 MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ -#define MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ - -#include -#include -#include -#include - -namespace autoware::motion::control::mpc_lateral_controller -{ -/** - * @brief 2nd-order Butterworth Filter - * reference : S. Butterworth, "On the Theory of Filter Amplifier", Experimental wireless, 1930. - */ -class Butterworth2dFilter -{ -private: - double m_y1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_y2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_u1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_u2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_a1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_a2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_b0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_b1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_b2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - -public: - /** - * @brief constructor with initialization - * @param [in] dt sampling time - * @param [in] f_cutoff_hz cutoff frequency [Hz] - */ - explicit Butterworth2dFilter(double dt = 0.01, double f_cutoff_hz = 5.0); - - /** - * @brief destructor - */ - ~Butterworth2dFilter(); - - /** - * @brief constructor - * @param [in] dt sampling time - * @param [in] f_cutoff_hz cutoff frequency [Hz] - */ - void initialize(const double & dt, const double & f_cutoff_hz); - - /** - * @brief filtering (call this function at each sampling time with input) - * @param [in] u scalar input for filter - * @return filtered scalar value - */ - double filter(const double & u); - - /** - * @brief filtering for time-series data - * @param [in] t time-series data for input vector - * @param [out] u object vector - */ - void filt_vector(const std::vector & t, std::vector & u) const; - - /** - * @brief filtering for time-series data from both forward-backward direction for zero phase delay - * @param [in] t time-series data for input vector - * @param [out] u object vector - */ - void filtfilt_vector( - const std::vector & t, - std::vector & u) const; // filtering forward and backward direction - - /** - * @brief get filter coefficients - * @param [out] coeffs coefficients of filter [a0, a1, a2, b0, b1, b2]. - */ - void getCoefficients(std::vector & coeffs) const; -}; - -/** - * @brief Move Average Filter - */ -namespace MoveAverageFilter -{ -/** - * @brief filtering vector - * @param [in] num index distance for moving average filter - * @param [out] u object vector - */ -bool filt_vector(const int num, std::vector & u); -} // namespace MoveAverageFilter -} // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp deleted file mode 100644 index e2f66e95ab0e3..0000000000000 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2018-2021 The Autoware Foundation -// -// 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. - -/* - * Representation - * e : lateral error - * th : heading angle error - * steer : steering angle - * steer_d: desired steering angle (input) - * v : velocity - * W : wheelbase length - * tau : time constant for steering dynamics - * - * State & Input - * x = [e, th, steer]^T - * u = steer_d - * - * Nonlinear model - * dx1/dt = v * sin(x2) - * dx2/dt = v * tan(x3) / W - * dx3/dt = -(x3 - u) / tau - * - * Linearized model around reference point (v = v_r, th = th_r, steer = steer_r) - * [0, vr, 0] [ 0] [ 0] - * dx/dt = [0, 0, vr/W/cos(steer_r)^2] * x + [ 0] * u + [-vr*steer_r/W/cos(steer_r)^2] - * [0, 0, 1/tau] [1/tau] [ 0] - * - */ - -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ - -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" - -#include -#include - -#include - -namespace autoware::motion::control::mpc_lateral_controller -{ - -/** - * Vehicle model class of bicycle kinematics - * @brief calculate model-related values - */ -class KinematicsBicycleModel : public VehicleModelInterface -{ -public: - /** - * @brief constructor with parameter initialization - * @param [in] wheelbase wheelbase length [m] - * @param [in] steer_lim steering angle limit [rad] - * @param [in] steer_tau steering time constant for 1d-model [s] - */ - KinematicsBicycleModel(const double wheelbase, const double steer_lim, const double steer_tau); - - /** - * @brief destructor - */ - ~KinematicsBicycleModel() = default; - - /** - * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk - * @param [out] a_d coefficient matrix - * @param [out] b_d coefficient matrix - * @param [out] c_d coefficient matrix - * @param [out] w_d coefficient matrix - * @param [in] dt Discretization time [s] - */ - void calculateDiscreteMatrix( - Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, - const double dt) override; - - /** - * @brief calculate reference input - * @param [out] u_ref input - */ - void calculateReferenceInput(Eigen::MatrixXd & u_ref) override; - - std::string modelName() override { return "kinematics"; }; - - MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( - const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, - const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const MPCTrajectory & reference_trajectory, const double dt) const override; - - MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( - const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, - const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const MPCTrajectory & reference_trajectory, const double dt) const override; - -private: - double m_steer_lim; //!< @brief steering angle limit [rad] - double m_steer_tau; //!< @brief steering time constant for 1d-model [s] -}; -} // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp deleted file mode 100644 index 68806ff5a00dc..0000000000000 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp +++ /dev/null @@ -1,152 +0,0 @@ -// Copyright 2018-2021 The Autoware Foundation -// -// 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 MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ - -#include "mpc_lateral_controller/mpc_trajectory.hpp" - -#include - -#include - -namespace autoware::motion::control::mpc_lateral_controller -{ - -/** - * Vehicle model class - * @brief calculate model-related values - */ -class VehicleModelInterface -{ -protected: - const int m_dim_x; //!< @brief dimension of state x - const int m_dim_u; //!< @brief dimension of input u - const int m_dim_y; //!< @brief dimension of output y - double m_velocity; //!< @brief vehicle velocity [m/s] - double m_curvature; //!< @brief curvature on the linearized point on path - double m_wheelbase; //!< @brief wheelbase of the vehicle [m] - -public: - /** - * @brief constructor - * @param [in] dim_x dimension of state x - * @param [in] dim_u dimension of input u - * @param [in] dim_y dimension of output y - * @param [in] wheelbase wheelbase of the vehicle [m] - */ - VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheelbase); - - /** - * @brief destructor - */ - virtual ~VehicleModelInterface() = default; - - /** - * @brief get state x dimension - * @return state dimension - */ - int getDimX() const; - - /** - * @brief get input u dimension - * @return input dimension - */ - int getDimU() const; - - /** - * @brief get output y dimension - * @return output dimension - */ - int getDimY() const; - - /** - * @brief get wheelbase of the vehicle - * @return wheelbase value [m] - */ - double getWheelbase() const; - - /** - * @brief set velocity - * @param [in] velocity vehicle velocity - */ - void setVelocity(const double velocity); - - /** - * @brief set curvature - * @param [in] curvature curvature on the linearized point on path - */ - void setCurvature(const double curvature); - - /** - * @brief calculate discrete model matrix of x_k+1 = a_d * xk + b_d * uk + w_d, yk = c_d * xk - * @param [out] a_d coefficient matrix - * @param [out] b_d coefficient matrix - * @param [out] c_d coefficient matrix - * @param [out] w_d coefficient matrix - * @param [in] dt Discretization time [s] - */ - virtual void calculateDiscreteMatrix( - Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, - const double dt) = 0; - - /** - * @brief calculate reference input - * @param [out] u_ref input - */ - virtual void calculateReferenceInput(Eigen::MatrixXd & u_ref) = 0; - - /** - * @brief returns model name e.g. kinematics, dynamics - */ - virtual std::string modelName() = 0; - - /** - * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in world - * coordinate - * @param a_d The MPC A matrix used for optimization. - * @param b_d The MPC B matrix used for optimization. - * @param c_d The MPC C matrix used for optimization. - * @param w_d The MPC W matrix used for optimization. - * @param x0 initial state vector. - * @param Uex The optimized input vector. - * @param reference_trajectory The resampled reference trajectory. - * @param dt delta time used in the optimization - * @return The predicted trajectory. - */ - virtual MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( - const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, - const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const MPCTrajectory & reference_trajectory, const double dt) const = 0; - - /** - * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in Frenet - * Coordinate - * @param a_d The MPC A matrix used for optimization. - * @param b_d The MPC B matrix used for optimization. - * @param c_d The MPC C matrix used for optimization. - * @param w_d The MPC W matrix used for optimization. - * @param x0 initial state vector. - * @param Uex The optimized input vector. - * @param reference_trajectory The resampled reference trajectory. - * @param dt delta time used in the optimization - * @return The predicted trajectory. - */ - virtual MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( - const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, - const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const MPCTrajectory & reference_trajectory, const double dt) const = 0; -}; -} // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/package.xml b/control/mpc_lateral_controller/package.xml deleted file mode 100644 index da03c4481e782..0000000000000 --- a/control/mpc_lateral_controller/package.xml +++ /dev/null @@ -1,48 +0,0 @@ - - - - mpc_lateral_controller - 1.0.0 - MPC-based lateral controller - - Takamasa Horibe - Takayuki Murooka - - Apache 2.0 - - Takamasa Horibe - Maxime CLEMENT - Takayuki Murooka - - ament_cmake_auto - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs - diagnostic_msgs - diagnostic_updater - eigen - geometry_msgs - interpolation - motion_utils - osqp_interface - rclcpp - rclcpp_components - std_msgs - tf2 - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - trajectory_follower_base - vehicle_info_util - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp deleted file mode 100644 index cd52dd4f79314..0000000000000 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ /dev/null @@ -1,152 +0,0 @@ -// Copyright 2018-2021 The Autoware Foundation -// -// 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 "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" - -#include - -namespace autoware::motion::control::mpc_lateral_controller -{ -KinematicsBicycleModel::KinematicsBicycleModel( - const double wheelbase, const double steer_lim, const double steer_tau) -: VehicleModelInterface(/* dim_x */ 3, /* dim_u */ 1, /* dim_y */ 2, wheelbase) -{ - m_steer_lim = steer_lim; - m_steer_tau = steer_tau; -} - -void KinematicsBicycleModel::calculateDiscreteMatrix( - Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d, - const double dt) -{ - auto sign = [](double x) { return (x > 0.0) - (x < 0.0); }; - - /* Linearize delta around delta_r (reference delta) */ - double delta_r = atan(m_wheelbase * m_curvature); - if (std::abs(delta_r) >= m_steer_lim) { - delta_r = m_steer_lim * static_cast(sign(delta_r)); - } - double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r)); - double velocity = m_velocity; - if (std::abs(m_velocity) < 1e-04) { - velocity = 1e-04 * (m_velocity >= 0 ? 1 : -1); - } - - a_d << 0.0, velocity, 0.0, 0.0, 0.0, velocity / m_wheelbase * cos_delta_r_squared_inv, 0.0, 0.0, - -1.0 / m_steer_tau; - - b_d << 0.0, 0.0, 1.0 / m_steer_tau; - - c_d << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0; - - w_d << 0.0, - -velocity * m_curvature + - velocity / m_wheelbase * (tan(delta_r) - delta_r * cos_delta_r_squared_inv), - 0.0; - - // bilinear discretization for ZOH system - // no discretization is needed for Cd - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x); - const Eigen::MatrixXd i_dt2a_inv = (I - dt * 0.5 * a_d).inverse(); - a_d = i_dt2a_inv * (I + dt * 0.5 * a_d); - b_d = i_dt2a_inv * b_d * dt; - w_d = i_dt2a_inv * w_d * dt; -} - -void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) -{ - u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); -} - -MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate( - [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d, - [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d, - const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const MPCTrajectory & reference_trajectory, const double dt) const -{ - // Calculate predicted state in world coordinate since there is modeling errors in Frenet - // Relative coordinate x = [lat_err, yaw_err, steer] - // World coordinate x = [x, y, yaw, steer] - - const auto & t = reference_trajectory; - - // create initial state in the world coordinate - Eigen::VectorXd state_w = [&]() { - Eigen::VectorXd state = Eigen::VectorXd::Zero(4); - const auto lateral_error_0 = x0(0); - const auto yaw_error_0 = x0(1); - state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x - state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y - state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw - state(3, 0) = x0(2); // steering - return state; - }(); - - // update state in the world coordinate - const auto updateState = [&]( - const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, - const double dt, const double velocity) { - const auto yaw = state_w(2); - const auto steer = state_w(3); - const auto desired_steer = input(0); - - Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4); - dstate(0) = velocity * std::cos(yaw); - dstate(1) = velocity * std::sin(yaw); - dstate(2) = velocity * std::tan(steer) / m_wheelbase; - dstate(3) = -(steer - desired_steer) / m_steer_tau; - - // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation - // in Eigen. - const Eigen::VectorXd next_state = state_w + dstate * dt; - return next_state; - }; - - MPCTrajectory mpc_predicted_trajectory; - const auto DIM_U = getDimU(); - - for (size_t i = 0; i < reference_trajectory.size(); ++i) { - state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); - mpc_predicted_trajectory.push_back( - state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), - t.relative_time.at(i)); - } - return mpc_predicted_trajectory; -} - -MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate( - const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, - [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, - const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const -{ - // Relative coordinate x = [lat_err, yaw_err, steer] - - Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; - MPCTrajectory mpc_predicted_trajectory; - const auto DIM_X = getDimX(); - const auto & t = reference_trajectory; - - for (size_t i = 0; i < reference_trajectory.size(); ++i) { - const auto lateral_error = Xex(i * DIM_X); // model dependent - const auto yaw_error = Xex(i * DIM_X + 1); // model dependent - const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; - const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; - const auto yaw = t.yaw.at(i) + yaw_error; - mpc_predicted_trajectory.push_back( - x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); - } - return mpc_predicted_trajectory; -} -} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp deleted file mode 100644 index f54a5e325bead..0000000000000 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2018-2021 The Autoware Foundation -// -// 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 "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" - -namespace autoware::motion::control::mpc_lateral_controller -{ -VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheelbase) -: m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase) -{ -} -int VehicleModelInterface::getDimX() const -{ - return m_dim_x; -} -int VehicleModelInterface::getDimU() const -{ - return m_dim_u; -} -int VehicleModelInterface::getDimY() const -{ - return m_dim_y; -} -double VehicleModelInterface::getWheelbase() const -{ - return m_wheelbase; -} -void VehicleModelInterface::setVelocity(const double velocity) -{ - m_velocity = velocity; -} -void VehicleModelInterface::setCurvature(const double curvature) -{ - m_curvature = curvature; -} -} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/obstacle_collision_checker/README.md b/control/obstacle_collision_checker/README.md index 5478db1f5a135..25b57fe6f4653 100644 --- a/control/obstacle_collision_checker/README.md +++ b/control/obstacle_collision_checker/README.md @@ -57,13 +57,13 @@ If any collision is found on predicted path, this module sets `ERROR` level as d ### Input -| Name | Type | Description | -| ---------------------------------------------- | ---------------------------------------------- | ------------------------------------------------------------------ | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| ---------------------------------------------- | ----------------------------------------- | ------------------------------------------------------------------ | +| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output 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 4ab26c31f9910..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,10 +15,10 @@ #ifndef OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ #define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ -#include -#include +#include +#include -#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 { @@ -52,15 +52,15 @@ struct Input geometry_msgs::msg::Twist::ConstSharedPtr current_twist; sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud; geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory; }; struct Output { std::map processing_time_map; bool will_collide; - autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory; + autoware_planning_msgs::msg::Trajectory resampled_trajectory; std::vector vehicle_footprints; std::vector vehicle_passing_areas; }; @@ -75,18 +75,18 @@ class ObstacleCollisionChecker private: Param param_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; //! This function assumes the input trajectory is sampled dense enough - static autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval); + static autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval); - static autoware_auto_planning_msgs::msg::Trajectory cutTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length); + static autoware_planning_msgs::msg::Trajectory cutTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double length); static std::vector createVehicleFootprints( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); static std::vector createVehiclePassingAreas( const std::vector & vehicle_footprints); 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 12b1e51a1cf3c..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,15 +17,15 @@ #include "obstacle_collision_checker/obstacle_collision_checker.hpp" +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include +#include #include #include #include @@ -48,12 +48,12 @@ 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 + rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; rclcpp::Subscription::SharedPtr sub_odom_; @@ -62,18 +62,18 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_; geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; // Callback void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); - void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); 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/launch/obstacle_collision_checker.launch.xml b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml index 078993d064064..f82384534e040 100755 --- a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml +++ b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index 46b0b18191e81..45b142fe601fe 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -18,7 +18,9 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs + autoware_universe_utils + autoware_vehicle_info_utils boost diagnostic_updater eigen @@ -32,8 +34,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils - vehicle_info_util 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 8b89913ef35c8..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 @@ -54,7 +54,7 @@ pcl::PointCloud getTransformedPointCloud( pcl::PointCloud filterPointCloudByTrajectory( const pcl::PointCloud & pointcloud, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double radius) + const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius) { pcl::PointCloud filtered_pointcloud; for (const auto & point : pointcloud.points) { @@ -83,14 +83,14 @@ double calcBrakingDistance( namespace obstacle_collision_checker { ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) -: vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) +: vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { } 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; @@ -121,18 +121,19 @@ Output ObstacleCollisionChecker::update(const Input & input) return output; } -autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval) +autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval) { - autoware_auto_planning_msgs::msg::Trajectory resampled; + autoware_planning_msgs::msg::Trajectory resampled; resampled.header = trajectory.header; resampled.points.push_back(trajectory.points.front()); 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); @@ -143,10 +144,10 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleT return resampled; } -autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length) +autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double length) { - autoware_auto_planning_msgs::msg::Trajectory cut; + autoware_planning_msgs::msg::Trajectory cut; cut.header = trajectory.header; double total_length = 0.0; @@ -154,8 +155,8 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec 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; @@ -169,7 +170,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec if (remain_distance <= points_distance) { const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized(); - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.pose.position.x = p_interpolated.x(); p.pose.position.y = p_interpolated.y(); p.pose.position.z = p_interpolated.z(); @@ -187,8 +188,8 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec } std::vector ObstacleCollisionChecker::createVehicleFootprints( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { // Create vehicle footprint in base_link coordinate const auto local_vehicle_footprint = vehicle_info.createFootprint(param.footprint_margin); @@ -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 bf3da0fe32627..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 #include @@ -70,24 +70,25 @@ 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(), std::bind(&ObstacleCollisionCheckerNode::onObstaclePointcloud, this, _1)); - sub_reference_trajectory_ = create_subscription( + sub_reference_trajectory_ = create_subscription( "input/reference_trajectory", 1, std::bind(&ObstacleCollisionCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = create_subscription( + sub_predicted_trajectory_ = create_subscription( "input/predicted_trajectory", 1, std::bind(&ObstacleCollisionCheckerNode::onPredictedTrajectory, this, _1)); sub_odom_ = create_subscription( "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"); @@ -109,13 +110,13 @@ void ObstacleCollisionCheckerNode::onObstaclePointcloud( } void ObstacleCollisionCheckerNode::onReferenceTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { reference_trajectory_ = msg; } void ObstacleCollisionCheckerNode::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { predicted_trajectory_ = msg; } @@ -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/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp index e851cca85c11f..705bff754d3d9 100644 --- a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp @@ -18,9 +18,9 @@ TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory) { pcl::PointCloud pcl; - autoware_auto_planning_msgs::msg::Trajectory trajectory; + autoware_planning_msgs::msg::Trajectory trajectory; pcl::PointXYZ pcl_point; - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + autoware_planning_msgs::msg::TrajectoryPoint traj_point; pcl_point.y = 0.0; traj_point.pose.position.y = 0.99; for (double x = 0.0; x < 10.0; x += 1.0) { diff --git a/control/operation_mode_transition_manager/CMakeLists.txt b/control/operation_mode_transition_manager/CMakeLists.txt deleted file mode 100644 index 9c8ca29fdc277..0000000000000 --- a/control/operation_mode_transition_manager/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(operation_mode_transition_manager) - -find_package(autoware_cmake REQUIRED) -autoware_package() -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -ament_auto_add_library(operation_mode_transition_manager_node SHARED - src/compatibility.cpp - src/data.cpp - src/node.cpp - src/state.cpp -) - -rclcpp_components_register_node(operation_mode_transition_manager_node - PLUGIN "operation_mode_transition_manager::OperationModeTransitionManager" - EXECUTABLE operation_mode_transition_manager_exe -) - -rosidl_generate_interfaces( - ${PROJECT_NAME} - "msg/OperationModeTransitionManagerDebug.msg" - DEPENDENCIES builtin_interfaces -) - -# to use same package defined message -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(operation_mode_transition_manager_node - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(operation_mode_transition_manager_node "${cpp_typesupport_target}") -endif() - - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md deleted file mode 100644 index ea0688e88d9f1..0000000000000 --- a/control/operation_mode_transition_manager/README.md +++ /dev/null @@ -1,138 +0,0 @@ -# operation_mode_transition_manager - -## Purpose / Use cases - -This module is responsible for managing the different modes of operation for the Autoware system. The possible modes are: - -- `Autonomous`: the vehicle is fully controlled by the autonomous driving system -- `Local`: the vehicle is controlled by a physically connected control system such as a joy stick -- `Remote`: the vehicle is controlled by a remote controller -- `Stop`: the vehicle is stopped and there is no active control system. - -There is also an `In Transition` state that occurs during each mode transitions. During this state, the transition to the new operator is not yet complete, and the previous operator is still responsible for controlling the system until the transition is complete. Some actions may be restricted during the `In Transition` state, such as sudden braking or steering. (This is restricted by the `vehicle_cmd_gate`). - -### Features - -- Transit mode between `Autonomous`, `Local`, `Remote` and `Stop` based on the indication command. -- Check whether the each transition is available (safe or not). -- Limit some sudden motion control in `In Transition` mode (this is done with `vehicle_cmd_gate` feature). -- Check whether the transition is completed. - -- Transition between the `Autonomous`, `Local`, `Remote`, and `Stop` modes based on the indicated command. -- Determine whether each transition is safe to execute. -- Restrict certain sudden motion controls during the `In Transition` mode (using the `vehicle_cmd_gate` feature). -- Verify that the transition is complete. - -## Design - -A rough design of the relationship between `operation_mode_transition_manager`` and the other nodes is shown below. - -![transition_rough_structure](image/transition_rough_structure.drawio.svg) - -A more detailed structure is below. - -![transition_detailed_structure](image/transition_detailed_structure.drawio.svg) - -Here we see that `operation_mode_transition_manager` has multiple state transitions as follows - -- **AUTOWARE ENABLED <---> DISABLED** - - **ENABLED**: the vehicle is controlled by Autoware. - - **DISABLED**: the vehicle is out of Autoware control, expecting the e.g. manual driving. -- **AUTOWARE ENABLED <---> AUTO/LOCAL/REMOTE/NONE** - - **AUTO**: the vehicle is controlled by Autoware, with the autonomous control command calculated by the planning/control component. - - **LOCAL**: the vehicle is controlled by Autoware, with the locally connected operator, e.g. joystick controller. - - **REMOTE**: the vehicle is controlled by Autoware, with the remotely connected operator. - - **NONE**: the vehicle is not controlled by any operator. -- **IN TRANSITION <---> COMPLETED** - - **IN TRANSITION**: the mode listed above is in the transition process, expecting the former operator to have a responsibility to confirm the transition is completed. - - **COMPLETED**: the mode transition is completed. - -## Inputs / Outputs / API - -### Inputs - -For the mode transition: - -- /system/operation_mode/change_autoware_control [`tier4_system_msgs/srv/ChangeAutowareControl`]: change operation mode to Autonomous -- /system/operation_mode/change_operation_mode [`tier4_system_msgs/srv/ChangeOperationMode`]: change operation mode - -For the transition availability/completion check: - -- /control/command/control_cmd [`autoware_auto_control_msgs/msg/AckermannControlCommand`]: vehicle control signal -- /localization/kinematic_state [`nav_msgs/msg/Odometry`]: ego vehicle state -- /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs/msg/Trajectory`]: planning trajectory -- /vehicle/status/control_mode [`autoware_auto_vehicle_msgs/msg/ControlModeReport`]: vehicle control mode (autonomous/manual) -- /control/vehicle_cmd_gate/operation_mode [`autoware_adapi_v1_msgs/msg/OperationModeState`]: the operation mode in the `vehicle_cmd_gate`. (To be removed) - -For the backward compatibility (to be removed): - -- /api/autoware/get/engage [`autoware_auto_vehicle_msgs/msg/Engage`] -- /control/current_gate_mode [`tier4_control_msgs/msg/GateMode`] -- /control/external_cmd_selector/current_selector_mode [`tier4_control_msgs/msg/ExternalCommandSelectorMode`] - -### Outputs - -- /system/operation_mode/state [`autoware_adapi_v1_msgs/msg/OperationModeState`]: to inform the current operation mode -- /control/operation_mode_transition_manager/debug_info [`operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug`]: detailed information about the operation mode transition - -- /control/gate_mode_cmd [`tier4_control_msgs/msg/GateMode`]: to change the `vehicle_cmd_gate` state to use its features (to be removed) -- /autoware/engage [`autoware_auto_vehicle_msgs/msg/Engage`]: - -- /control/control_mode_request [`autoware_auto_vehicle_msgs/srv/ControlModeCommand`]: to change the vehicle control mode (autonomous/manual) -- /control/external_cmd_selector/select_external_command [`tier4_control_msgs/srv/ExternalCommandSelect`]: - -## Parameters - -{{ json_to_markdown("control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json") }} - -| Name | Type | Description | Default value | -| :--------------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | -| `frequency_hz` | `double` | running hz | 10.0 | -| `enable_engage_on_driving` | `bool` | Set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted. | 0.1 | -| `check_engage_condition` | `bool` | If false, autonomous transition is always available | 0.1 | -| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 | -| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 | - -For `engage_acceptable_limits` related parameters: - -| Name | Type | Description | Default value | -| :---------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `allow_autonomous_in_stopped` | `bool` | If true, autonomous transition is available when the vehicle is stopped even if other checks fail. | true | -| `dist_threshold` | `double` | the distance between the trajectory and ego vehicle must be within this distance for `Autonomous` transition. | 1.5 | -| `yaw_threshold` | `double` | the yaw angle between trajectory and ego vehicle must be within this threshold for `Autonomous` transition. | 0.524 | -| `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold for `Autonomous` transition. | 10.0 | -| `speed_lower_threshold` | `double` | the velocity deviation between the control command and ego vehicle must be within this threshold for `Autonomous` transition. | -10.0 | -| `acc_threshold` | `double` | the control command acceleration must be less than this threshold for `Autonomous` transition. | 1.5 | -| `lateral_acc_threshold` | `double` | the control command lateral acceleration must be less than this threshold for `Autonomous` transition. | 1.0 | -| `lateral_acc_diff_threshold` | `double` | the lateral acceleration deviation between the control command must be less than this threshold for `Autonomous` transition. | 0.5 | - -For `stable_check` related parameters: - -| Name | Type | Description | Default value | -| :---------------------- | :------- | :-------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `duration` | `double` | the stable condition must be satisfied for this duration to complete the transition. | 0.1 | -| `dist_threshold` | `double` | the distance between the trajectory and ego vehicle must be within this distance to complete `Autonomous` transition. | 1.5 | -| `yaw_threshold` | `double` | the yaw angle between trajectory and ego vehicle must be within this threshold to complete `Autonomous` transition. | 0.262 | -| `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | -| `speed_lower_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | - -## Engage check behavior on each parameter setting - -This matrix describes the scenarios in which the vehicle can be engaged based on the combinations of parameter settings: - -| `enable_engage_on_driving` | `check_engage_condition` | `allow_autonomous_in_stopped` | Scenarios where engage is permitted | -| :------------------------: | :----------------------: | :---------------------------: | :---------------------------------------------------------------- | -| x | x | x | Only when the vehicle is stationary. | -| x | x | o | Only when the vehicle is stationary. | -| x | o | x | When the vehicle is stationary and all engage conditions are met. | -| x | o | o | Only when the vehicle is stationary. | -| o | x | x | At any time (Caution: Not recommended). | -| o | x | o | At any time (Caution: Not recommended). | -| o | o | x | When all engage conditions are met, regardless of vehicle status. | -| o | o | o | When all engage conditions are met or the vehicle is stationary. | - -## Future extensions / Unimplemented parts - -- Need to remove backward compatibility interfaces. -- This node should be merged to the `vehicle_cmd_gate` due to its strong connection. diff --git a/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml b/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml deleted file mode 100644 index 569d743ba2c4f..0000000000000 --- a/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml deleted file mode 100644 index dbe9a21c1186a..0000000000000 --- a/control/operation_mode_transition_manager/package.xml +++ /dev/null @@ -1,40 +0,0 @@ - - operation_mode_transition_manager - 0.1.0 - The vehicle_cmd_gate package - Takamasa Horibe - Tomoya Kimura - Apache License 2.0 - - Takamasa Horibe - - autoware_cmake - rosidl_default_generators - - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_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 - vehicle_info_util - - ament_cmake_gtest - ament_lint_auto - autoware_lint_common - - rosidl_default_runtime - rosidl_interface_packages - - - ament_cmake - - diff --git a/control/operation_mode_transition_manager/src/node.cpp b/control/operation_mode_transition_manager/src/node.cpp deleted file mode 100644 index 9c268102d7b67..0000000000000 --- a/control/operation_mode_transition_manager/src/node.cpp +++ /dev/null @@ -1,292 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// 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 "node.hpp" - -#include - -namespace operation_mode_transition_manager -{ - -OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::NodeOptions & options) -: Node("operation_mode_transition_manager", options), compatibility_(this) -{ - sub_control_mode_report_ = create_subscription( - "control_mode_report", 1, - [this](const ControlModeReport::SharedPtr msg) { control_mode_report_ = *msg; }); - - sub_gate_operation_mode_ = create_subscription( - "gate_operation_mode", 1, - [this](const OperationModeState::SharedPtr msg) { gate_operation_mode_ = *msg; }); - - cli_control_mode_ = create_client("control_mode_request"); - pub_debug_info_ = create_publisher("~/debug_info", 1); - - // component interface - { - const auto node = component_interface_utils::NodeAdaptor(this); - node.init_srv( - srv_autoware_control_, this, &OperationModeTransitionManager::onChangeAutowareControl); - node.init_srv( - srv_operation_mode_, this, &OperationModeTransitionManager::onChangeOperationMode); - node.init_pub(pub_operation_mode_); - } - - // timer - { - const auto period_ns = rclcpp::Rate(declare_parameter("frequency_hz")).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&OperationModeTransitionManager::onTimer, this)); - } - - // initialize state - current_mode_ = OperationMode::STOP; - transition_ = nullptr; - gate_operation_mode_.mode = OperationModeState::UNKNOWN; - gate_operation_mode_.is_in_transition = false; - control_mode_report_.mode = ControlModeReport::NO_COMMAND; - transition_timeout_ = declare_parameter("transition_timeout"); - { - // check `transition_timeout` value - const auto stable_duration = declare_parameter("stable_check.duration"); - const double TIMEOUT_MARGIN = 0.5; - if (transition_timeout_ < stable_duration + TIMEOUT_MARGIN) { - transition_timeout_ = stable_duration + TIMEOUT_MARGIN; - RCLCPP_WARN( - get_logger(), "`transition_timeout` must be somewhat larger than `stable_check.duration`"); - RCLCPP_WARN_STREAM(get_logger(), "transition_timeout is set to " << transition_timeout_); - } - } - - // modes - modes_[OperationMode::STOP] = std::make_unique(); - modes_[OperationMode::AUTONOMOUS] = std::make_unique(this); - modes_[OperationMode::LOCAL] = std::make_unique(); - modes_[OperationMode::REMOTE] = std::make_unique(); -} - -void OperationModeTransitionManager::onChangeAutowareControl( - const ChangeAutowareControlAPI::Service::Request::SharedPtr request, - const ChangeAutowareControlAPI::Service::Response::SharedPtr response) -{ - if (request->autoware_control) { - // Treat as a transition to the current operation mode. - changeOperationMode(std::nullopt); - } else { - // Allow mode transition to complete without canceling. - compatibility_transition_ = std::nullopt; - transition_.reset(); - changeControlMode(ControlModeCommand::Request::MANUAL); - } - response->status.success = true; -} - -void OperationModeTransitionManager::onChangeOperationMode( - const ChangeOperationModeAPI::Service::Request::SharedPtr request, - const ChangeOperationModeAPI::Service::Response::SharedPtr response) -{ - const auto mode = toEnum(*request); - if (!mode) { - throw component_interface_utils::ParameterError("The operation mode is invalid."); - } - changeOperationMode(mode.value()); - response->status.success = true; -} - -void OperationModeTransitionManager::changeControlMode(ControlModeCommandType mode) -{ - const auto callback = [this](rclcpp::Client::SharedFuture future) { - if (!future.get()->success) { - RCLCPP_WARN(get_logger(), "Autonomous mode change was rejected."); - if (transition_) { - transition_->is_engage_failed = true; - } - } - }; - - const auto request = std::make_shared(); - request->stamp = now(); - request->mode = mode; - cli_control_mode_->async_send_request(request, callback); -} - -void OperationModeTransitionManager::changeOperationMode(std::optional request_mode) -{ - // NOTE: If request_mode is nullopt, indicates to enable autoware control - - const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; - const bool request_control = request_mode ? false : true; - - if (current_mode_ == request_mode) { - throw component_interface_utils::NoEffectWarning("The mode is the same as the current."); - } - - if (current_control && request_control) { - throw component_interface_utils::NoEffectWarning("Autoware already controls the vehicle."); - } - - // TODO(Takagi, Isamu): Consider mode change request during transition. - if (transition_ && request_mode != OperationMode::STOP) { - throw component_interface_utils::ServiceException( - ServiceResponse::ERROR_IN_TRANSITION, "The mode transition is in progress."); - } - - // Enter transition mode if the vehicle is being or will be controlled by Autoware. - if (current_control || request_control) { - if (!available_mode_change_[request_mode.value_or(current_mode_)]) { - throw component_interface_utils::ServiceException( - ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change condition is not satisfied."); - } - if (request_control) { - transition_ = std::make_unique(now(), request_control, std::nullopt); - } else { - transition_ = std::make_unique(now(), request_control, current_mode_); - } - } - compatibility_transition_ = now(); - current_mode_ = request_mode.value_or(current_mode_); -} - -void OperationModeTransitionManager::cancelTransition() -{ - const auto & previous = transition_->previous; - if (previous) { - compatibility_transition_ = now(); - current_mode_ = previous.value(); - } else { - compatibility_transition_ = std::nullopt; - changeControlMode(ControlModeCommand::Request::MANUAL); - } - transition_.reset(); -} - -void OperationModeTransitionManager::processTransition() -{ - const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; - - // Check timeout. - if (transition_timeout_ < (now() - transition_->time).seconds()) { - return cancelTransition(); - } - - // Check engage failure. - if (transition_->is_engage_failed) { - return cancelTransition(); - } - - // Check override. - if (current_control) { - transition_->is_engage_completed = true; - } else { - if (transition_->is_engage_completed) { - return cancelTransition(); - } - } - - // Check reflection of mode change to the compatible interface. - if (current_mode_ != compatibility_.get_mode()) { - return; - } - - // Check completion when engaged, otherwise engage after the gate reflects transition. - if (current_control) { - if (modes_.at(current_mode_)->isModeChangeCompleted()) { - return transition_.reset(); - } - } else { - if (transition_->is_engage_requested && gate_operation_mode_.is_in_transition) { - transition_->is_engage_requested = false; - return changeControlMode(ControlModeCommand::Request::AUTONOMOUS); - } - } -} - -void OperationModeTransitionManager::onTimer() -{ - for (const auto & [type, mode] : modes_) { - mode->update(current_mode_ == type && transition_); - } - - for (const auto & [type, mode] : modes_) { - available_mode_change_[type] = mode->isModeChangeAvailable(); - } - - // Check sync timeout to the compatible interface. - if (compatibility_transition_) { - if (compatibility_timeout_ < (now() - compatibility_transition_.value()).seconds()) { - compatibility_transition_ = std::nullopt; - } - } - - // Reflects the mode when changed by the compatible interface. - if (compatibility_transition_) { - compatibility_.set_mode(current_mode_); - } else { - current_mode_ = compatibility_.get_mode().value_or(current_mode_); - } - - // Reset sync timeout when it is completed. - if (compatibility_transition_) { - if (current_mode_ == compatibility_.get_mode()) { - compatibility_transition_ = std::nullopt; - } - } - - if (transition_) { - processTransition(); - } - - publishData(); -} - -void OperationModeTransitionManager::publishData() -{ - const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; - const auto time = now(); - - OperationModeStateAPI::Message state; - state.mode = toMsg(current_mode_); - state.is_autoware_control_enabled = current_control; - state.is_in_transition = transition_ ? true : false; - state.is_stop_mode_available = available_mode_change_[OperationMode::STOP]; - state.is_autonomous_mode_available = available_mode_change_[OperationMode::AUTONOMOUS]; - state.is_local_mode_available = available_mode_change_[OperationMode::LOCAL]; - state.is_remote_mode_available = available_mode_change_[OperationMode::REMOTE]; - - if (prev_state_ != state) { - prev_state_ = state; - state.stamp = time; - pub_operation_mode_->publish(state); - } - - const auto status_str = [&]() { - if (!current_control) return "DISENGAGE (autoware mode = " + toString(current_mode_) + ")"; - if (transition_) - return toString(current_mode_) + " (in transition from " + toString(transition_->previous) + - ")"; - return toString(current_mode_); - }(); - - ModeChangeBase::DebugInfo debug_info = modes_.at(OperationMode::AUTONOMOUS)->getDebugInfo(); - debug_info.stamp = time; - debug_info.status = status_str; - debug_info.in_autoware_control = current_control; - debug_info.in_transition = transition_ ? true : false; - pub_debug_info_->publish(debug_info); -} - -} // namespace operation_mode_transition_manager - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(operation_mode_transition_manager::OperationModeTransitionManager) diff --git a/control/operation_mode_transition_manager/src/node.hpp b/control/operation_mode_transition_manager/src/node.hpp deleted file mode 100644 index c2d761fcf138c..0000000000000 --- a/control/operation_mode_transition_manager/src/node.hpp +++ /dev/null @@ -1,81 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// 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 NODE_HPP_ -#define NODE_HPP_ - -#include "compatibility.hpp" -#include "state.hpp" - -#include -#include -#include - -#include -#include -#include - -namespace operation_mode_transition_manager -{ - -class OperationModeTransitionManager : public rclcpp::Node -{ -public: - explicit OperationModeTransitionManager(const rclcpp::NodeOptions & options); - -private: - using ChangeAutowareControlAPI = system_interface::ChangeAutowareControl; - using ChangeOperationModeAPI = system_interface::ChangeOperationMode; - using OperationModeStateAPI = system_interface::OperationModeState; - component_interface_utils::Service::SharedPtr srv_autoware_control_; - component_interface_utils::Service::SharedPtr srv_operation_mode_; - component_interface_utils::Publisher::SharedPtr pub_operation_mode_; - void onChangeAutowareControl( - const ChangeAutowareControlAPI::Service::Request::SharedPtr request, - const ChangeAutowareControlAPI::Service::Response::SharedPtr response); - void onChangeOperationMode( - const ChangeOperationModeAPI::Service::Request::SharedPtr request, - const ChangeOperationModeAPI::Service::Response::SharedPtr response); - - using ControlModeCommandType = ControlModeCommand::Request::_mode_type; - rclcpp::Subscription::SharedPtr sub_control_mode_report_; - rclcpp::Subscription::SharedPtr sub_gate_operation_mode_; - rclcpp::Client::SharedPtr cli_control_mode_; - rclcpp::Publisher::SharedPtr pub_debug_info_; - rclcpp::TimerBase::SharedPtr timer_; - void onTimer(); - void publishData(); - void changeControlMode(ControlModeCommandType mode); - void changeOperationMode(std::optional mode); - void cancelTransition(); - void processTransition(); - - double transition_timeout_; - OperationMode current_mode_; - std::unique_ptr transition_; - std::unordered_map> modes_; - std::unordered_map available_mode_change_; - OperationModeState gate_operation_mode_; - ControlModeReport control_mode_report_; - - std::optional prev_state_; - - static constexpr double compatibility_timeout_ = 1.0; - Compatibility compatibility_; - std::optional compatibility_transition_; -}; - -} // namespace operation_mode_transition_manager - -#endif // NODE_HPP_ diff --git a/control/operation_mode_transition_manager/src/util.hpp b/control/operation_mode_transition_manager/src/util.hpp deleted file mode 100644 index e3fa20aaafd74..0000000000000 --- a/control/operation_mode_transition_manager/src/util.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// 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 UTIL_HPP_ -#define UTIL_HPP_ - -#include "operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" - -using DebugInfo = operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; - -void setAllOk(DebugInfo & debug_info) -{ - debug_info.is_all_ok = true; - debug_info.engage_allowed_for_stopped_vehicle = true; - debug_info.large_acceleration_ok = true; - debug_info.large_lateral_acceleration_diff_ok = true; - debug_info.large_lateral_acceleration_ok = true; - debug_info.lateral_deviation_ok = true; - debug_info.speed_lower_deviation_ok = true; - debug_info.speed_upper_deviation_ok = true; - debug_info.stop_ok = true; - debug_info.trajectory_available_ok = true; - debug_info.yaw_deviation_ok = true; -} - -#endif // UTIL_HPP_ diff --git a/control/pid_longitudinal_controller/CMakeLists.txt b/control/pid_longitudinal_controller/CMakeLists.txt deleted file mode 100644 index 3e7a992b15e00..0000000000000 --- a/control/pid_longitudinal_controller/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(pid_longitudinal_controller) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -set(PID_LON_CON_LIB ${PROJECT_NAME}_lib) -ament_auto_add_library(${PID_LON_CON_LIB} SHARED - src/pid_longitudinal_controller.cpp - src/pid.cpp - src/smooth_stop.cpp - src/longitudinal_controller_utils.cpp -) - -if(BUILD_TESTING) - set(TEST_LON_SOURCES - test/test_pid.cpp - test/test_smooth_stop.cpp - test/test_longitudinal_controller_utils.cpp - ) - set(TEST_LONGITUDINAL_CONTROLLER_EXE test_longitudinal_controller) - ament_add_ros_isolated_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES}) - target_link_libraries(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${PID_LON_CON_LIB}) -endif() - -ament_auto_package( - INSTALL_TO_SHARE - param -) diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md deleted file mode 100644 index 57eab2d87f18e..0000000000000 --- a/control/pid_longitudinal_controller/README.md +++ /dev/null @@ -1,266 +0,0 @@ -# PID Longitudinal Controller - -## Purpose / Use cases - -The longitudinal_controller computes the target acceleration to achieve the target velocity set at each point of the target trajectory using a feed-forward/back control. - -It also contains a slope force correction that takes into account road slope information, and a delay compensation function. -It is assumed that the target acceleration calculated here will be properly realized by the vehicle interface. - -Note that the use of this module is not mandatory for Autoware if the vehicle supports the "target speed" interface. - -## Design / Inner-workings / Algorithms - -### States - -This module has four state transitions as shown below in order to handle special processing in a specific situation. - -- **DRIVE** - - Executes target velocity tracking by PID control. - - It also applies the delay compensation and slope compensation. -- **STOPPING** - - Controls the motion just before stopping. - - Special sequence is performed to achieve accurate and smooth stopping. -- **STOPPED** - - Performs operations in the stopped state (e.g. brake hold) -- **EMERGENCY**. - - Enters an emergency state when certain conditions are met (e.g., when the vehicle has crossed a certain distance of a stop line). - - The recovery condition (whether or not to keep emergency state until the vehicle completely stops) or the deceleration in the emergency state are defined by parameters. - -The state transition diagram is shown below. - -![LongitudinalControllerStateTransition](./media/LongitudinalControllerStateTransition.drawio.svg) - -### Logics - -#### Control Block Diagram - -![LongitudinalControllerDiagram](./media/LongitudinalControllerDiagram.drawio.svg) - -#### FeedForward (FF) - -The reference acceleration set in the trajectory and slope compensation terms are output as a feedforward. Under ideal conditions with no modeling error, this FF term alone should be sufficient for velocity tracking. - -Tracking errors causing modeling or discretization errors are removed by the feedback control (now using PID). - -##### Brake keeping - -From the viewpoint of ride comfort, stopping with 0 acceleration is important because it reduces the impact of braking. However, if the target acceleration when stopping is 0, the vehicle may cross over the stop line or accelerate a little in front of the stop line due to vehicle model error or gradient estimation error. - -For reliable stopping, the target acceleration calculated by the FeedForward system is limited to a negative acceleration when stopping. - -![BrakeKeepingDiagram](./media/BrakeKeeping.drawio.svg) - -#### Slope compensation - -Based on the slope information, a compensation term is added to the target acceleration. - -There are two sources of the slope information, which can be switched by a parameter. - -- Pitch of the estimated ego-pose (default) - - Calculates the current slope from the pitch angle of the estimated ego-pose - - Pros: Easily available - - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. -- Z coordinate on the trajectory - - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory - - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained - - Pros: Can be used in combination with delay compensation (not yet implemented) - - Cons: z-coordinates of high-precision map is needed. - - Cons: Does not support free space planning (for now) - -**Notation:** This function works correctly only in a vehicle system that does not have acceleration feedback in the low-level control system. - -This compensation adds gravity correction to the target acceleration, resulting in an output value that is no longer equal to the target acceleration that the autonomous driving system desires. Therefore, it conflicts with the role of the acceleration feedback in the low-level controller. -For instance, if the vehicle is attempting to start with an acceleration of `1.0 m/s^2` and a gravity correction of `-1.0 m/s^2` is applied, the output value will be `0`. If this output value is mistakenly treated as the target acceleration, the vehicle will not start. - -A suitable example of a vehicle system for the slope compensation function is one in which the output acceleration from the longitudinal_controller is converted into target accel/brake pedal input without any feedbacks. In this case, the output acceleration is just used as a feedforward term to calculate the target pedal, and hence the issue mentioned above does not arise. - -Note: The angle of the slope is defined as positive for an uphill slope, while the pitch angle of the ego pose is defined as negative when facing upward. They have an opposite definition. - -![slope_definition](./media/slope_definition.drawio.svg) - -#### PID control - -For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system. - -This PID control calculates the target acceleration from the deviation between the current ego-velocity and the target velocity. - -This PID logic has a maximum value for the output of each term. This is to prevent the following: - -- Large integral terms may cause unintended behavior by users. -- Unintended noise may cause the output of the derivative term to be very large. - -Note: by default, the integral term in the control system is not accumulated when the vehicle is stationary. This precautionary measure aims to prevent unintended accumulation of the integral term in scenarios where Autoware assumes the vehicle is engaged, but an external system has immobilized the vehicle to initiate startup procedures. - -However, certain situations may arise, such as when the vehicle encounters a depression in the road surface during startup or if the slope compensation is inaccurately estimated (lower than necessary), leading to a failure to initiate motion. To address these scenarios, it is possible to activate error integration even when the vehicle is at rest by setting the `enable_integration_at_low_speed` parameter to true. - -When `enable_integration_at_low_speed` is set to true, the PID controller will initiate integration of the acceleration error after a specified duration defined by the `time_threshold_before_pid_integration` parameter has elapsed without the vehicle surpassing a minimum velocity set by the `current_vel_threshold_pid_integration` parameter. - -The presence of the `time_threshold_before_pid_integration` parameter is important for practical PID tuning. Integrating the error when the vehicle is stationary or at low speed can complicate PID tuning. This parameter effectively introduces a delay before the integral part becomes active, preventing it from kicking in immediately. This delay allows for more controlled and effective tuning of the PID controller. - -At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance. -This may be replaced by a higher performance controller (adaptive control or robust control) in future development. - -#### Time delay compensation - -At high speeds, the delay of actuator systems such as gas pedals and brakes has a significant impact on driving accuracy. -Depending on the actuating principle of the vehicle, the mechanism that physically controls the gas pedal and brake typically has a delay of about a hundred millisecond. - -In this controller, the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem. - -### Slope compensation - -Based on the slope information, a compensation term is added to the target acceleration. - -There are two sources of the slope information, which can be switched by a parameter. - -- Pitch of the estimated ego-pose (default) - - Calculates the current slope from the pitch angle of the estimated ego-pose - - Pros: Easily available - - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. -- Z coordinate on the trajectory - - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory - - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained - - Pros: Can be used in combination with delay compensation (not yet implemented) - - Cons: z-coordinates of high-precision map is needed. - - Cons: Does not support free space planning (for now) - -## Assumptions / Known limits - -1. Smoothed target velocity and its acceleration shall be set in the trajectory - 1. The velocity command is not smoothed inside the controller (only noise may be removed). - 2. For step-like target signal, tracking is performed as fast as possible. -2. The vehicle velocity must be an appropriate value - 1. The ego-velocity must be a signed-value corresponding to the forward/backward direction - 2. The ego-velocity should be given with appropriate noise processing. - 3. If there is a large amount of noise in the ego-velocity, the tracking performance will be significantly reduced. -3. The output of this controller must be achieved by later modules (e.g. vehicle interface). - 1. If the vehicle interface does not have the target velocity or acceleration interface (e.g., the vehicle only has a gas pedal and brake interface), an appropriate conversion must be done after this controller. - -## Inputs / Outputs / API - -### Input - -Set the following from the [controller_node](../trajectory_follower_node/README.md) - -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. -- `nav_msgs/Odometry`: current odometry - -### Output - -Return LongitudinalOutput which contains the following to the controller node - -- `autoware_auto_control_msgs/LongitudinalCommand`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. -- LongitudinalSyncData - - velocity convergence(currently not used) - -### PIDController class - -The `PIDController` class is straightforward to use. -First, gains and limits must be set (using `setGains()` and `setLimits()`) for the proportional (P), integral (I), and derivative (D) components. -Then, the velocity can be calculated by providing the current error and time step duration to the `calculate()` function. - -## Parameter description - -The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the -AutonomouStuff Lexus RX 450h for under 40 km/h driving. - -| Name | Type | Description | Default value | -| :------------------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| delay_compensation_time | double | delay for longitudinal control [s] | 0.17 | -| enable_smooth_stop | bool | flag to enable transition to STOPPING | true | -| enable_overshoot_emergency | bool | flag to enable transition to EMERGENCY when the ego is over the stop line with a certain distance. See `emergency_state_overshoot_stop_dist`. | true | -| enable_large_tracking_error_emergency | bool | flag to enable transition to EMERGENCY when the closest trajectory point search is failed due to a large deviation between trajectory and ego pose. | true | -| enable_slope_compensation | bool | flag to modify output acceleration for slope compensation. The source of the slope angle can be selected from ego-pose or trajectory angle. See `use_trajectory_for_pitch_calculation`. | true | -| enable_brake_keeping_before_stop | bool | flag to keep a certain acceleration during DRIVE state before the ego stops. See [Brake keeping](#brake-keeping). | false | -| enable_keep_stopped_until_steer_convergence | bool | flag to keep stopped condition until until the steer converges. | true | -| max_acc | double | max value of output acceleration [m/s^2] | 3.0 | -| min_acc | double | min value of output acceleration [m/s^2] | -5.0 | -| max_jerk | double | max value of jerk of output acceleration [m/s^3] | 2.0 | -| min_jerk | double | min value of jerk of output acceleration [m/s^3] | -5.0 | -| use_trajectory_for_pitch_calculation | bool | If true, the slope is estimated from trajectory z-level. Otherwise the pitch angle of the ego pose is used. | false | -| lpf_pitch_gain | double | gain of low-pass filter for pitch estimation | 0.95 | -| max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 | -| min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 | - -### State transition - -| Name | Type | Description | Default value | -| :---------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| drive_state_stop_dist | double | The state will transit to DRIVE when the distance to the stop point is longer than `drive_state_stop_dist` + `drive_state_offset_stop_dist` [m] | 0.5 | -| drive_state_offset_stop_dist | double | The state will transit to DRIVE when the distance to the stop point is longer than `drive_state_stop_dist` + `drive_state_offset_stop_dist` [m] | 1.0 | -| stopping_state_stop_dist | double | The state will transit to STOPPING when the distance to the stop point is shorter than `stopping_state_stop_dist` [m] | 0.5 | -| stopped_state_entry_vel | double | threshold of the ego velocity in transition to the STOPPED state [m/s] | 0.01 | -| stopped_state_entry_acc | double | threshold of the ego acceleration in transition to the STOPPED state [m/s^2] | 0.1 | -| emergency_state_overshoot_stop_dist | double | If `enable_overshoot_emergency` is true and the ego is `emergency_state_overshoot_stop_dist`-meter ahead of the stop point, the state will transit to EMERGENCY. [m] | 1.5 | -| emergency_state_traj_trans_dev | double | If the ego's position is `emergency_state_traj_tran_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m] | 3.0 | -| emergency_state_traj_rot_dev | double | If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad] | 0.784 | - -### DRIVE Parameter - -| Name | Type | Description | Default value | -| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| kp | double | p gain for longitudinal control | 1.0 | -| ki | double | i gain for longitudinal control | 0.1 | -| kd | double | d gain for longitudinal control | 0.0 | -| max_out | double | max value of PID's output acceleration during DRIVE state [m/s^2] | 1.0 | -| min_out | double | min value of PID's output acceleration during DRIVE state [m/s^2] | -1.0 | -| max_p_effort | double | max value of acceleration with p gain | 1.0 | -| min_p_effort | double | min value of acceleration with p gain | -1.0 | -| max_i_effort | double | max value of acceleration with i gain | 0.3 | -| min_i_effort | double | min value of acceleration with i gain | -0.3 | -| 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] | -| 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 | - -### STOPPING Parameter (smooth stop) - -Smooth stop is enabled if `enable_smooth_stop` is true. -In smooth stop, strong acceleration (`strong_acc`) will be output first to decrease the ego velocity. -Then weak acceleration (`weak_acc`) will be output to stop smoothly by decreasing the ego jerk. -If the ego does not stop in a certain time or some-meter over the stop point, weak acceleration to stop right (`weak_stop_acc`) now will be output. -If the ego is still running, strong acceleration (`strong_stop_acc`) to stop right now will be output. - -| Name | Type | Description | Default value | -| :--------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | -| smooth_stop_max_strong_acc | double | max strong acceleration [m/s^2] | -0.5 | -| smooth_stop_min_strong_acc | double | min strong acceleration [m/s^2] | -0.8 | -| smooth_stop_weak_acc | double | weak acceleration [m/s^2] | -0.3 | -| smooth_stop_weak_stop_acc | double | weak acceleration to stop right now [m/s^2] | -0.8 | -| smooth_stop_strong_stop_acc | double | strong acceleration to be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m/s^2] | -3.4 | -| smooth_stop_max_fast_vel | double | max fast vel to judge the ego is running fast [m/s]. If the ego is running fast, strong acceleration will be output. | 0.5 | -| smooth_stop_min_running_vel | double | min ego velocity to judge if the ego is running or not [m/s] | 0.01 | -| smooth_stop_min_running_acc | double | min ego acceleration to judge if the ego is running or not [m/s^2] | 0.01 | -| smooth_stop_weak_stop_time | double | max time to output weak acceleration [s]. After this, strong acceleration will be output. | 0.8 | -| smooth_stop_weak_stop_dist | double | Weak acceleration will be output when the ego is `smooth_stop_weak_stop_dist`-meter before the stop point. [m] | -0.3 | -| smooth_stop_strong_stop_dist | double | Strong acceleration will be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m] | -0.5 | - -### STOPPED Parameter - -The `STOPPED` state assumes that the vehicle is completely stopped with the brakes fully applied. -Therefore, `stopped_acc` should be set to a value that allows the vehicle to apply the strongest possible brake. -If `stopped_acc` is not sufficiently low, there is a possibility of sliding down on steep slopes. - -| Name | Type | Description | Default value | -| :----------- | :----- | :------------------------------------------- | :------------ | -| stopped_vel | double | target velocity in STOPPED state [m/s] | 0.0 | -| stopped_acc | double | target acceleration in STOPPED state [m/s^2] | -3.4 | -| stopped_jerk | double | target jerk in STOPPED state [m/s^3] | -5.0 | - -### EMERGENCY Parameter - -| Name | Type | Description | Default value | -| :------------- | :----- | :------------------------------------------------ | :------------ | -| emergency_vel | double | target velocity in EMERGENCY state [m/s] | 0.0 | -| emergency_acc | double | target acceleration in an EMERGENCY state [m/s^2] | -5.0 | -| emergency_jerk | double | target jerk in an EMERGENCY state [m/s^3] | -3.0 | - -## References / External links - -## Future extensions / Unimplemented parts - -## Related issues diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp deleted file mode 100644 index 06fe1793c8123..0000000000000 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2018-2021 The Autoware Foundation -// -// 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 PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ - -#include -#include -#include -#include - -namespace autoware::motion::control::pid_longitudinal_controller -{ - -/** - * @brief Simple filter with gain on the first derivative of the value - */ -class LowpassFilter1d -{ -private: - double m_x; //!< @brief current filtered value - double m_gain; //!< @brief gain value of first-order low-pass filter - -public: - /** - * @brief constructor with initial value and first-order gain - * @param [in] x initial value - * @param [in] gain first-order gain - */ - LowpassFilter1d(const double x, const double gain) : m_x(x), m_gain(gain) {} - - /** - * @brief set the current value of the filter - * @param [in] x new value - */ - void reset(const double x) { m_x = x; } - - /** - * @brief get the current value of the filter - */ - double getValue() const { return m_x; } - - /** - * @brief filter a new value - * @param [in] u new value - */ - double filter(const double u) - { - const double ret = m_gain * m_x + (1.0 - m_gain) * u; - m_x = ret; - return ret; - } -}; -} // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp deleted file mode 100644 index 8b981c0485ed9..0000000000000 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright 2018-2021 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 PID_LONGITUDINAL_CONTROLLER__PID_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__PID_HPP_ - -#include - -namespace autoware::motion::control::pid_longitudinal_controller -{ - -/// @brief implementation of a PID controller -class PIDController -{ -public: - PIDController(); - - /** - * @brief calculate the output of this PID - * @param [in] error previous error - * @param [in] dt time step [s] - * @param [in] is_integrated if true, will use the integral component for calculation - * @param [out] pid_contributions values of the proportional, integral, and derivative components - * @return PID output - * @throw std::runtime_error if gains or limits have not been set - */ - double calculate( - const double error, const double dt, const bool is_integrated, - std::vector & pid_contributions); - /** - * @brief set the coefficients for the P (proportional) I (integral) D (derivative) terms - * @param [in] kp proportional coefficient - * @param [in] ki integral coefficient - * @param [in] kd derivative coefficient - */ - void setGains(const double kp, const double ki, const double kd); - /** - * @brief set limits on the total, proportional, integral, and derivative components - * @param [in] max_ret maximum return value of this PID - * @param [in] min_ret minimum return value of this PID - * @param [in] max_ret_p maximum value of the proportional component - * @param [in] min_ret_p minimum value of the proportional component - * @param [in] max_ret_i maximum value of the integral component - * @param [in] min_ret_i minimum value of the integral component - * @param [in] max_ret_d maximum value of the derivative component - * @param [in] min_ret_d minimum value of the derivative component - */ - void setLimits( - const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, - const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d); - /** - * @brief reset this PID to its initial state - */ - void reset(); - -private: - // PID parameters - struct Params - { - double kp; - double ki; - double kd; - double max_ret_p; - double min_ret_p; - double max_ret_i; - double min_ret_i; - double max_ret_d; - double min_ret_d; - double max_ret; - double min_ret; - }; - Params m_params; - - // state variables - double m_error_integral; - double m_prev_error; - bool m_is_first_time; - bool m_is_gains_set; - bool m_is_limits_set; -}; -} // namespace autoware::motion::control::pid_longitudinal_controller - -#endif // PID_LONGITUDINAL_CONTROLLER__PID_HPP_ diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml deleted file mode 100644 index 959aca689816a..0000000000000 --- a/control/pid_longitudinal_controller/package.xml +++ /dev/null @@ -1,49 +0,0 @@ - - - - pid_longitudinal_controller - 1.0.0 - PID-based longitudinal controller - - Takamasa Horibe - Takayuki Murooka - Mamoru Sobue - - Apache 2.0 - - Takamasa Horibe - Maxime CLEMENT - Takayuki Murooka - - ament_cmake_auto - autoware_cmake - - autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs - diagnostic_msgs - diagnostic_updater - eigen - geometry_msgs - interpolation - motion_utils - rclcpp - rclcpp_components - std_msgs - tf2 - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - trajectory_follower_base - vehicle_info_util - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/control/pid_longitudinal_controller/src/pid.cpp b/control/pid_longitudinal_controller/src/pid.cpp deleted file mode 100644 index 530b5cd15e754..0000000000000 --- a/control/pid_longitudinal_controller/src/pid.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright 2018-2021 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 "pid_longitudinal_controller/pid.hpp" - -#include -#include -#include -#include -#include - -namespace autoware::motion::control::pid_longitudinal_controller -{ -PIDController::PIDController() -: m_error_integral(0.0), - m_prev_error(0.0), - m_is_first_time(true), - m_is_gains_set(false), - m_is_limits_set(false) -{ -} - -double PIDController::calculate( - const double error, const double dt, const bool enable_integration, - std::vector & pid_contributions) -{ - if (!m_is_gains_set || !m_is_limits_set) { - throw std::runtime_error("Trying to calculate uninitialized PID"); - } - - const auto & p = m_params; - - double ret_p = p.kp * error; - ret_p = std::min(std::max(ret_p, p.min_ret_p), p.max_ret_p); - - if (enable_integration) { - m_error_integral += error * dt; - m_error_integral = std::min(std::max(m_error_integral, p.min_ret_i / p.ki), p.max_ret_i / p.ki); - } - const double ret_i = p.ki * m_error_integral; - - double error_differential; - if (m_is_first_time) { - error_differential = 0; - m_is_first_time = false; - } else { - error_differential = (error - m_prev_error) / dt; - } - double ret_d = p.kd * error_differential; - ret_d = std::min(std::max(ret_d, p.min_ret_d), p.max_ret_d); - - m_prev_error = error; - - pid_contributions.resize(3); - pid_contributions.at(0) = ret_p; - pid_contributions.at(1) = ret_i; - pid_contributions.at(2) = ret_d; - - double ret = ret_p + ret_i + ret_d; - ret = std::min(std::max(ret, p.min_ret), p.max_ret); - - return ret; -} - -void PIDController::setGains(const double kp, const double ki, const double kd) -{ - m_params.kp = kp; - m_params.ki = ki; - m_params.kd = kd; - m_is_gains_set = true; -} - -void PIDController::setLimits( - const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, - const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d) -{ - m_params.max_ret = max_ret; - m_params.min_ret = min_ret; - m_params.max_ret_p = max_ret_p; - m_params.min_ret_p = min_ret_p; - m_params.max_ret_d = max_ret_d; - m_params.min_ret_d = min_ret_d; - m_params.max_ret_i = max_ret_i; - m_params.min_ret_i = min_ret_i; - m_is_limits_set = true; -} - -void PIDController::reset() -{ - m_error_integral = 0.0; - m_prev_error = 0.0; - m_is_first_time = true; -} -} // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/predicted_path_checker/README.md b/control/predicted_path_checker/README.md index 24e4b91ef441b..a968a285928f6 100644 --- a/control/predicted_path_checker/README.md +++ b/control/predicted_path_checker/README.md @@ -55,14 +55,14 @@ make the vehicle stop. ## Inputs -| Name | Type | Description | -| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------------- | -| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | -| `~/input/predicted_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment | -| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity | -| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration | -| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle | +| Name | Type | Description | +| ------------------------------------- | ------------------------------------------------ | --------------------------------------------------- | +| `~/input/reference_trajectory` | `autoware_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/predicted_trajectory` | `autoware_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment | +| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity | +| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration | +| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle | ## Outputs 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 1d7791955b576..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 #include @@ -42,15 +42,15 @@ namespace autoware::motion::control::predicted_path_checker { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +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; @@ -120,7 +120,7 @@ class CollisionChecker // Variables std::shared_ptr debug_ptr_; rclcpp::Node * node_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::vector predicted_object_history_{}; }; } // namespace autoware::motion::control::predicted_path_checker 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 3ce5728521141..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,20 +15,20 @@ #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 -#include -#include +#include +#include #include #include #include @@ -47,14 +47,14 @@ namespace autoware::motion::control::predicted_path_checker { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObjects; +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,11 +88,11 @@ 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 + rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_accel_; @@ -106,8 +106,8 @@ class PredictedPathCheckerNode : public rclcpp::Node geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; control_interface::IsStopped::Message::ConstSharedPtr is_stopped_ptr_{nullptr}; // Core @@ -116,14 +116,14 @@ class PredictedPathCheckerNode : public rclcpp::Node // Variables State current_state_{State::DRIVE}; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; bool is_calling_set_stop_{false}; bool is_stopped_by_node_{false}; // Callback void onDynamicObjects(PredictedObjects::ConstSharedPtr msg); - void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); - void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); void onIsStopped(const control_interface::IsStopped::Message::ConstSharedPtr msg); 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 75e90e624a17e..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,13 +15,13 @@ #ifndef PREDICTED_PATH_CHECKER__UTILS_HPP_ #define PREDICTED_PATH_CHECKER__UTILS_HPP_ +#include +#include +#include #include -#include -#include -#include -#include -#include +#include +#include #include #include #include @@ -39,16 +39,16 @@ namespace utils { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +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; +using autoware_planning_msgs::msg::TrajectoryPoint; 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 vehicle_info_util::VehicleInfo; using PointArray = std::vector; using TrajectoryPoints = std::vector; @@ -57,7 +57,7 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width); TrajectoryPoint calcInterpolatedPoint( const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, @@ -65,7 +65,7 @@ TrajectoryPoint calcInterpolatedPoint( std::pair findStopPoint( TrajectoryPoints & predicted_trajectory_array, const size_t collision_idx, - const double stop_margin, vehicle_info_util::VehicleInfo & vehicle_info); + const double stop_margin, autoware::vehicle_info_utils::VehicleInfo & vehicle_info); bool isInBrakeDistance( const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, @@ -85,14 +85,14 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( const double & base_to_width); Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertObjToPolygon(const PredictedObject & obj); -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape); void getCurrentObjectPose( PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, diff --git a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml index a35c11b80c1f7..6af1372a5bb4a 100755 --- a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml +++ b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index 35f971907696a..153d11107a8cb 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/predicted_path_checker/package.xml @@ -12,15 +12,17 @@ ament_cmake autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_motion_utils + autoware_perception_msgs + autoware_planning_msgs + autoware_universe_utils + autoware_vehicle_info_utils boost component_interface_specs component_interface_utils diagnostic_updater eigen geometry_msgs - motion_utils nav_msgs rclcpp rclcpp_components @@ -30,11 +32,9 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_control_msgs tier4_external_api_msgs tier4_planning_msgs - vehicle_info_util ament_cmake_ros ament_lint_auto 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 5e8d96805b832..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 @@ -27,7 +27,7 @@ CollisionChecker::CollisionChecker( rclcpp::Node * node, std::shared_ptr debug_ptr) : debug_ptr_(std::move(debug_ptr)), node_(node), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo()) + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo()) { } @@ -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 bba069442bee7..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 @@ -38,7 +38,7 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); adaptor.init_cli(cli_set_stop_, group_cli_); adaptor.init_sub(sub_stop_state_, this, &PredictedPathCheckerNode::onIsStopped); - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); // Node Parameter node_param_.update_rate = declare_parameter("update_rate", 10.0); @@ -67,15 +67,15 @@ 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(), std::bind(&PredictedPathCheckerNode::onDynamicObjects, this, _1)); - sub_reference_trajectory_ = create_subscription( + sub_reference_trajectory_ = create_subscription( "~/input/reference_trajectory", 1, std::bind(&PredictedPathCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = create_subscription( + sub_predicted_trajectory_ = create_subscription( "~/input/predicted_trajectory", 1, std::bind(&PredictedPathCheckerNode::onPredictedTrajectory, this, _1)); sub_odom_ = create_subscription( @@ -109,13 +109,13 @@ void PredictedPathCheckerNode::onDynamicObjects(const PredictedObjects::ConstSha } void PredictedPathCheckerNode::onReferenceTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { reference_trajectory_ = msg; } void PredictedPathCheckerNode::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { predicted_trajectory_ = msg; } @@ -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 9cb095e908d41..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,15 +21,15 @@ 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( const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) { Polygon2d polygon; @@ -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) { @@ -146,7 +148,7 @@ TrajectoryPoint calcInterpolatedPoint( std::pair findStopPoint( TrajectoryPoints & trajectory_array, const size_t collision_idx, const double stop_margin, - vehicle_info_util::VehicleInfo & vehicle_info) + autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { // It returns the stop point and segment of the point on trajectory. @@ -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; @@ -298,7 +300,7 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( } Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; @@ -320,7 +322,7 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; tf2::Transform tf_map2obj; @@ -350,15 +352,15 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & Polygon2d convertObjToPolygon(const PredictedObject & obj) { Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + 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_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } 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_auto_perception_msgs::msg::Shape::POLYGON) { + } 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 { @@ -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); @@ -377,13 +379,13 @@ bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & ob return base_pose_vec.dot(obstacle_vec) >= 0; } -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.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/control/pure_pursuit/CMakeLists.txt b/control/pure_pursuit/CMakeLists.txt deleted file mode 100644 index b1c73d5397f07..0000000000000 --- a/control/pure_pursuit/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(pure_pursuit) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) - -include_directories( - SYSTEM - ${EIGEN3_INCLUDE_DIRS} -) - -# pure_pursuit_core -ament_auto_add_library(pure_pursuit_core SHARED - src/pure_pursuit_core/planning_utils.cpp - src/pure_pursuit_core/pure_pursuit.cpp - src/pure_pursuit_core/interpolate.cpp -) - -# pure_pursuit -ament_auto_add_library(pure_pursuit_lateral_controller SHARED - src/pure_pursuit/pure_pursuit_lateral_controller.cpp - src/pure_pursuit/pure_pursuit_viz.cpp -) - -target_link_libraries(pure_pursuit_lateral_controller - pure_pursuit_core -) - -rclcpp_components_register_node(pure_pursuit_lateral_controller - PLUGIN "pure_pursuit::PurePursuitLateralController" - EXECUTABLE pure_pursuit_lateral_controller_exe -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/control/pure_pursuit/README.md b/control/pure_pursuit/README.md deleted file mode 100644 index 48e2a13ef664d..0000000000000 --- a/control/pure_pursuit/README.md +++ /dev/null @@ -1,19 +0,0 @@ -# Pure Pursuit Controller - -The Pure Pursuit Controller module calculates the steering angle for tracking a desired trajectory using the pure pursuit algorithm. This is used as a lateral controller plugin in the `trajectory_follower_node`. - -## Inputs - -Set the following from the [controller_node](../trajectory_follower_node/README.md) - -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. -- `nav_msgs/Odometry`: current ego pose and velocity information - -## Outputs - -Return LateralOutput which contains the following to the controller node - -- `autoware_auto_control_msgs/AckermannLateralCommand`: target steering angle -- LateralSyncData - - steer angle convergence -- `autoware_auto_planning_msgs/Trajectory`: predicted path for ego vehicle diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp deleted file mode 100644 index 99b1a82ab2dce..0000000000000 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/* - * Copyright 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_HPP_ -#define PURE_PURSUIT__PURE_PURSUIT_HPP_ - -#include - -#include - -#include -#include -#include - -#define EIGEN_MPL2_ONLY -#include -#include - -namespace pure_pursuit -{ -class PurePursuit -{ -public: - PurePursuit() : lookahead_distance_(0.0), closest_thr_dist_(3.0), closest_thr_ang_(M_PI / 4) {} - ~PurePursuit() = default; - - rclcpp::Logger logger = rclcpp::get_logger("pure_pursuit"); - // setter - void setCurrentPose(const geometry_msgs::msg::Pose & msg); - void setWaypoints(const std::vector & msg); - void setLookaheadDistance(double ld) { lookahead_distance_ = ld; } - void setClosestThreshold(double closest_thr_dist, double closest_thr_ang) - { - closest_thr_dist_ = closest_thr_dist; - closest_thr_ang_ = closest_thr_ang; - } - - // getter - geometry_msgs::msg::Point getLocationOfNextWaypoint() const { return loc_next_wp_; } - geometry_msgs::msg::Point getLocationOfNextTarget() const { return loc_next_tgt_; } - - bool isDataReady(); - std::pair run(); // calculate curvature - -private: - // variables for debug - geometry_msgs::msg::Point loc_next_wp_; - geometry_msgs::msg::Point loc_next_tgt_; - - // variables got from outside - double lookahead_distance_, closest_thr_dist_, closest_thr_ang_; - std::shared_ptr> curr_wps_ptr_; - std::shared_ptr curr_pose_ptr_; - - // functions - int32_t findNextPointIdx(int32_t search_start_idx); - std::pair lerpNextTarget(int32_t next_wp_idx); -}; - -} // namespace pure_pursuit - -#endif // PURE_PURSUIT__PURE_PURSUIT_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp deleted file mode 100644 index ca0d77140b195..0000000000000 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright 2020-2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. -// -// 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. -/* - * Copyright 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ -#define PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ - -#include "pure_pursuit/pure_pursuit.hpp" -#include "pure_pursuit/pure_pursuit_viz.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "trajectory_follower_base/lateral_controller_base.hpp" - -#include -#include -#include - -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" - -#include // To be replaced by std::optional in C++17 - -#include -#include - -using autoware::motion::control::trajectory_follower::InputData; -using autoware::motion::control::trajectory_follower::LateralControllerBase; -using autoware::motion::control::trajectory_follower::LateralOutput; -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; - -namespace pure_pursuit -{ - -struct PpOutput -{ - double curvature; - double velocity; -}; - -struct Param -{ - // Global Parameters - double wheel_base; - double max_steering_angle; // [rad] - - // Algorithm Parameters - double ld_velocity_ratio; - double ld_lateral_error_ratio; - double ld_curvature_ratio; - double min_lookahead_distance; - double max_lookahead_distance; - double reverse_min_lookahead_distance; // min_lookahead_distance in reverse gear - double converged_steer_rad_; - double prediction_ds; - double prediction_distance_length; // Total distance of prediction trajectory - double resampling_ds; - double curvature_calculation_distance; - double long_ld_lateral_error_threshold; - bool enable_path_smoothing; - int path_filter_moving_ave_num; -}; - -struct DebugData -{ - geometry_msgs::msg::Point next_target; -}; - -class PurePursuitLateralController : public LateralControllerBase -{ -public: - /// \param node Reference to the node used only for the component and parameter initialization. - explicit PurePursuitLateralController(rclcpp::Node & node); - -private: - rclcpp::Clock::SharedPtr clock_; - rclcpp::Logger logger_; - std::vector output_tp_array_; - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; - autoware_auto_planning_msgs::msg::Trajectory trajectory_; - nav_msgs::msg::Odometry current_odometry_; - autoware_auto_vehicle_msgs::msg::SteeringReport current_steering_; - boost::optional prev_cmd_; - - // Debug Publisher - rclcpp::Publisher::SharedPtr pub_debug_marker_; - rclcpp::Publisher::SharedPtr pub_debug_values_; - // Predicted Trajectory publish - rclcpp::Publisher::SharedPtr - pub_predicted_trajectory_; - - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - - void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - - void setResampledTrajectory(); - - // TF - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - geometry_msgs::msg::Pose current_pose_; - - void publishDebugMarker() const; - - /** - * @brief compute control command for path follow with a constant control period - */ - bool isReady([[maybe_unused]] const InputData & input_data) override; - LateralOutput run(const InputData & input_data) override; - - AckermannLateralCommand generateCtrlCmdMsg(const double target_curvature); - - // Parameter - Param param_{}; - - // Algorithm - std::unique_ptr pure_pursuit_; - - boost::optional calcTargetCurvature( - bool is_control_output, geometry_msgs::msg::Pose pose); - - /** - * @brief It takes current pose, control command, and delta distance. Then it calculates next pose - * of vehicle. - */ - - TrajectoryPoint calcNextPose( - const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const; - - boost::optional generatePredictedTrajectory(); - - AckermannLateralCommand generateOutputControlCmd(); - - bool calcIsSteerConverged(const AckermannLateralCommand & cmd); - - double calcLookaheadDistance( - const double lateral_error, const double curvature, const double velocity, const double min_ld, - const bool is_control_cmd); - - double calcCurvature(const size_t closest_idx); - - void averageFilterTrajectory(autoware_auto_planning_msgs::msg::Trajectory & u); - - // Debug - mutable DebugData debug_data_; -}; - -} // namespace pure_pursuit - -#endif // PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp deleted file mode 100644 index 6d9fca4b852cf..0000000000000 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp +++ /dev/null @@ -1,128 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/* - * Copyright 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_ -#define PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_ - -#include "pure_pursuit/pure_pursuit.hpp" -#include "pure_pursuit/pure_pursuit_viz.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -#include // To be replaced by std::optional in C++17 - -#include -#include - -#include -#include - -namespace pure_pursuit -{ -struct Param -{ - // Global Parameters - double wheel_base; - - // Node Parameters - double ctrl_period; - - // Algorithm Parameters - double lookahead_distance_ratio; - double min_lookahead_distance; - double reverse_min_lookahead_distance; // min_lookahead_distance in reverse gear -}; - -struct DebugData -{ - geometry_msgs::msg::Point next_target; -}; - -class PurePursuitNode : public rclcpp::Node -{ -public: - explicit PurePursuitNode(const rclcpp::NodeOptions & node_options); - -private: - // Subscriber - tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; - rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Subscription::SharedPtr sub_current_odometry_; - - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; - nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; - - bool isDataReady(); - - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - - // TF - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - - // Publisher - rclcpp::Publisher::SharedPtr - pub_ctrl_cmd_; - - void publishCommand(const double target_curvature); - - // Debug Publisher - rclcpp::Publisher::SharedPtr pub_debug_marker_; - - void publishDebugMarker() const; - - // Timer - rclcpp::TimerBase::SharedPtr timer_; - void onTimer(); - - // Parameter - Param param_; - - // Algorithm - std::unique_ptr pure_pursuit_; - - boost::optional calcTargetCurvature(); - boost::optional calcTargetPoint() const; - - // Debug - mutable DebugData debug_data_; -}; - -} // namespace pure_pursuit - -#endif // PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_viz.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_viz.hpp deleted file mode 100644 index adc803538703f..0000000000000 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_viz.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/* - * Copyright 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_ -#define PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_ - -#include -#include -#include - -#include -namespace pure_pursuit -{ -visualization_msgs::msg::Marker createNextTargetMarker( - const geometry_msgs::msg::Point & next_target); - -visualization_msgs::msg::Marker createTrajectoryCircleMarker( - const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose); -} // namespace pure_pursuit - -#endif // PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/util/marker_helper.hpp b/control/pure_pursuit/include/pure_pursuit/util/marker_helper.hpp deleted file mode 100644 index 56943b4072fb3..0000000000000 --- a/control/pure_pursuit/include/pure_pursuit/util/marker_helper.hpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ -#define PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ - -#include - -#include - -#include - -namespace pure_pursuit -{ -inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) -{ - geometry_msgs::msg::Point point; - - point.x = x; - point.y = y; - point.z = z; - - return point; -} - -inline geometry_msgs::msg::Quaternion createMarkerOrientation( - double x, double y, double z, double w) -{ - geometry_msgs::msg::Quaternion quaternion; - - quaternion.x = x; - quaternion.y = y; - quaternion.z = z; - quaternion.w = w; - - return quaternion; -} - -inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z) -{ - geometry_msgs::msg::Vector3 scale; - - scale.x = x; - scale.y = y; - scale.z = z; - - return scale; -} - -inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a) -{ - std_msgs::msg::ColorRGBA color; - - color.r = r; - color.g = g; - color.b = b; - color.a = a; - - return color; -} - -inline visualization_msgs::msg::Marker createDefaultMarker( - const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type, - const geometry_msgs::msg::Vector3 & scale, const std_msgs::msg::ColorRGBA & color) -{ - visualization_msgs::msg::Marker marker; - - marker.header.frame_id = frame_id; - // ToDo - // marker.header.stamp = rclcpp::Node::now(); - marker.ns = ns; - marker.id = id; - marker.type = type; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration::from_seconds(0); - - marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); - marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); - marker.scale = scale; - marker.color = color; - marker.frame_locked = true; - - return marker; -} - -inline void appendMarkerArray( - const visualization_msgs::msg::MarkerArray & additional_marker_array, - visualization_msgs::msg::MarkerArray * marker_array) -{ - for (const auto & marker : additional_marker_array.markers) { - marker_array->markers.push_back(marker); - } -} -} // namespace pure_pursuit - -#endif // PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml deleted file mode 100644 index 30356b856fbac..0000000000000 --- a/control/pure_pursuit/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - pure_pursuit - 0.1.0 - The pure_pursuit package - Takamasa Horibe - Apache License 2.0 - - Berkay Karaman - Takamasa Horibe - - ament_cmake_auto - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_planning_msgs - boost - geometry_msgs - libboost-dev - motion_utils - nav_msgs - rclcpp - rclcpp_components - sensor_msgs - std_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - trajectory_follower_base - vehicle_info_util - visualization_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp deleted file mode 100644 index f0c49b07e675a..0000000000000 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ /dev/null @@ -1,491 +0,0 @@ -// Copyright 2020-2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. -// -// 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. - -/* - * Copyright 2015-2019 Autoware Foundation. 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 "pure_pursuit/pure_pursuit_lateral_controller.hpp" - -#include "pure_pursuit/pure_pursuit_viz.hpp" -#include "pure_pursuit/util/planning_utils.hpp" -#include "pure_pursuit/util/tf_utils.hpp" - -#include - -#include -#include -#include - -namespace -{ -enum TYPE { - VEL_LD = 0, - CURVATURE_LD = 1, - LATERAL_ERROR_LD = 2, - TOTAL_LD = 3, - CURVATURE = 4, - LATERAL_ERROR = 5, - VELOCITY = 6, - SIZE // this is the number of enum elements -}; -} // namespace - -namespace pure_pursuit -{ -PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) -: clock_(node.get_clock()), - logger_(node.get_logger().get_child("lateral_controller")), - tf_buffer_(clock_), - tf_listener_(tf_buffer_) -{ - pure_pursuit_ = std::make_unique(); - - // Vehicle Parameters - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - param_.wheel_base = vehicle_info.wheel_base_m; - param_.max_steering_angle = vehicle_info.max_steer_angle_rad; - - // Algorithm Parameters - param_.ld_velocity_ratio = node.declare_parameter("ld_velocity_ratio"); - param_.ld_lateral_error_ratio = node.declare_parameter("ld_lateral_error_ratio"); - param_.ld_curvature_ratio = node.declare_parameter("ld_curvature_ratio"); - param_.long_ld_lateral_error_threshold = - node.declare_parameter("long_ld_lateral_error_threshold"); - param_.min_lookahead_distance = node.declare_parameter("min_lookahead_distance"); - param_.max_lookahead_distance = node.declare_parameter("max_lookahead_distance"); - param_.reverse_min_lookahead_distance = - node.declare_parameter("reverse_min_lookahead_distance"); - param_.converged_steer_rad_ = node.declare_parameter("converged_steer_rad"); - param_.prediction_ds = node.declare_parameter("prediction_ds"); - param_.prediction_distance_length = node.declare_parameter("prediction_distance_length"); - param_.resampling_ds = node.declare_parameter("resampling_ds"); - param_.curvature_calculation_distance = - node.declare_parameter("curvature_calculation_distance"); - param_.enable_path_smoothing = node.declare_parameter("enable_path_smoothing"); - param_.path_filter_moving_ave_num = node.declare_parameter("path_filter_moving_ave_num"); - - // Debug Publishers - pub_debug_marker_ = - node.create_publisher("~/debug/markers", 0); - pub_debug_values_ = node.create_publisher( - "~/debug/ld_outputs", rclcpp::QoS{1}); - - // Publish predicted trajectory - pub_predicted_trajectory_ = node.create_publisher( - "~/output/predicted_trajectory", 1); -} - -double PurePursuitLateralController::calcLookaheadDistance( - const double lateral_error, const double curvature, const double velocity, const double min_ld, - const bool is_control_cmd) -{ - const double vel_ld = abs(param_.ld_velocity_ratio * velocity); - const double curvature_ld = -abs(param_.ld_curvature_ratio * curvature); - double lateral_error_ld = 0.0; - - if (abs(lateral_error) >= param_.long_ld_lateral_error_threshold) { - // If lateral error is higher than threshold, we should make ld larger to prevent entering the - // road with high heading error. - lateral_error_ld = abs(param_.ld_lateral_error_ratio * lateral_error); - } - - const double total_ld = - std::clamp(vel_ld + curvature_ld + lateral_error_ld, min_ld, param_.max_lookahead_distance); - - auto pubDebugValues = [&]() { - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; - debug_msg.data.resize(TYPE::SIZE); - debug_msg.data.at(TYPE::VEL_LD) = static_cast(vel_ld); - debug_msg.data.at(TYPE::CURVATURE_LD) = static_cast(curvature_ld); - debug_msg.data.at(TYPE::LATERAL_ERROR_LD) = static_cast(lateral_error_ld); - debug_msg.data.at(TYPE::TOTAL_LD) = static_cast(total_ld); - debug_msg.data.at(TYPE::VELOCITY) = static_cast(velocity); - debug_msg.data.at(TYPE::CURVATURE) = static_cast(curvature); - debug_msg.data.at(TYPE::LATERAL_ERROR) = static_cast(lateral_error); - debug_msg.stamp = clock_->now(); - pub_debug_values_->publish(debug_msg); - }; - - if (is_control_cmd) { - pubDebugValues(); - } - - return total_ld; -} - -TrajectoryPoint PurePursuitLateralController::calcNextPose( - const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const -{ - geometry_msgs::msg::Transform transform; - transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); - transform.rotation = - planning_utils::getQuaternionFromYaw(((tan(cmd.steering_tire_angle) * ds) / param_.wheel_base)); - TrajectoryPoint output_p; - - tf2::Transform tf_pose; - tf2::Transform tf_offset; - tf2::fromMsg(transform, tf_offset); - tf2::fromMsg(point.pose, tf_pose); - tf2::toMsg(tf_pose * tf_offset, output_p.pose); - return output_p; -} - -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); - 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_->points.back() = trajectory_.points.back(); - trajectory_resampled_->header = trajectory_.header; - output_tp_array_ = motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_); -} - -double PurePursuitLateralController::calcCurvature(const size_t closest_idx) -{ - // Calculate current curvature - const size_t idx_dist = static_cast( - std::max(static_cast((param_.curvature_calculation_distance) / param_.resampling_ds), 1)); - - // Find the points in trajectory to calculate curvature - size_t next_idx = trajectory_resampled_->points.size() - 1; - size_t prev_idx = 0; - - if (static_cast(closest_idx) >= idx_dist) { - prev_idx = closest_idx - idx_dist; - } else { - // return zero curvature when backward distance is not long enough in the trajectory - return 0.0; - } - - if (trajectory_resampled_->points.size() - 1 >= closest_idx + idx_dist) { - next_idx = closest_idx + idx_dist; - } else { - // return zero curvature when forward distance is not long enough in the trajectory - return 0.0; - } - // TODO(k.sugahara): shift the center point of the curvature calculation to allow sufficient - // distance, because if sufficient distance cannot be obtained in front or behind, the curvature - // will be zero in the current implementation. - - // Calculate curvature assuming the trajectory points interval is constant - 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))); - } catch (std::exception const & e) { - // ...code that handles the error... - RCLCPP_WARN(rclcpp::get_logger("pure_pursuit"), "%s", e.what()); - current_curvature = 0.0; - } - return current_curvature; -} - -void PurePursuitLateralController::averageFilterTrajectory( - autoware_auto_planning_msgs::msg::Trajectory & u) -{ - if (static_cast(u.points.size()) <= 2 * param_.path_filter_moving_ave_num) { - RCLCPP_ERROR(logger_, "Cannot smooth path! Trajectory size is too low!"); - return; - } - - autoware_auto_planning_msgs::msg::Trajectory filtered_trajectory(u); - - for (int64_t i = 0; i < static_cast(u.points.size()); ++i) { - TrajectoryPoint tmp{}; - int64_t num_tmp = param_.path_filter_moving_ave_num; - int64_t count = 0; - double yaw = 0.0; - if (i - num_tmp < 0) { - num_tmp = i; - } - if (i + num_tmp > static_cast(u.points.size()) - 1) { - num_tmp = static_cast(u.points.size()) - i - 1; - } - for (int64_t j = -num_tmp; j <= num_tmp; ++j) { - const auto & p = u.points.at(static_cast(i + j)); - - tmp.pose.position.x += p.pose.position.x; - tmp.pose.position.y += p.pose.position.y; - tmp.pose.position.z += p.pose.position.z; - tmp.longitudinal_velocity_mps += p.longitudinal_velocity_mps; - tmp.acceleration_mps2 += p.acceleration_mps2; - tmp.front_wheel_angle_rad += p.front_wheel_angle_rad; - tmp.heading_rate_rps += p.heading_rate_rps; - yaw += tf2::getYaw(p.pose.orientation); - tmp.lateral_velocity_mps += p.lateral_velocity_mps; - tmp.rear_wheel_angle_rad += p.rear_wheel_angle_rad; - ++count; - } - auto & p = filtered_trajectory.points.at(static_cast(i)); - - p.pose.position.x = tmp.pose.position.x / count; - p.pose.position.y = tmp.pose.position.y / count; - p.pose.position.z = tmp.pose.position.z / count; - p.longitudinal_velocity_mps = tmp.longitudinal_velocity_mps / count; - p.acceleration_mps2 = tmp.acceleration_mps2 / count; - p.front_wheel_angle_rad = tmp.front_wheel_angle_rad / count; - p.heading_rate_rps = tmp.heading_rate_rps / count; - p.lateral_velocity_mps = tmp.lateral_velocity_mps / count; - p.rear_wheel_angle_rad = tmp.rear_wheel_angle_rad / count; - p.pose.orientation = pure_pursuit::planning_utils::getQuaternionFromYaw(yaw / count); - } - trajectory_resampled_ = std::make_shared(filtered_trajectory); -} - -boost::optional PurePursuitLateralController::generatePredictedTrajectory() -{ - const auto closest_idx_result = - motion_utils::findNearestIndex(output_tp_array_, current_odometry_.pose.pose, 3.0, M_PI_4); - - if (!closest_idx_result) { - return boost::none; - } - - const double remaining_distance = planning_utils::calcArcLengthFromWayPoint( - *trajectory_resampled_, *closest_idx_result, trajectory_resampled_->points.size() - 1); - - const auto num_of_iteration = std::max( - static_cast(std::ceil( - std::min(remaining_distance, param_.prediction_distance_length) / param_.prediction_ds)), - 1); - Trajectory predicted_trajectory; - - // Iterative prediction: - for (int i = 0; i < num_of_iteration; i++) { - if (i == 0) { - // For first point, use the odometry for velocity, and use the current_pose for prediction. - - TrajectoryPoint p; - p.pose = current_odometry_.pose.pose; - p.longitudinal_velocity_mps = current_odometry_.twist.twist.linear.x; - predicted_trajectory.points.push_back(p); - - const auto pp_output = calcTargetCurvature(true, predicted_trajectory.points.at(i).pose); - AckermannLateralCommand tmp_msg; - - if (pp_output) { - tmp_msg = generateCtrlCmdMsg(pp_output->curvature); - predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; - } else { - RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); - tmp_msg = generateCtrlCmdMsg(0.0); - } - TrajectoryPoint p2; - p2 = calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg); - predicted_trajectory.points.push_back(p2); - - } else { - const auto pp_output = calcTargetCurvature(false, predicted_trajectory.points.at(i).pose); - AckermannLateralCommand tmp_msg; - - if (pp_output) { - tmp_msg = generateCtrlCmdMsg(pp_output->curvature); - predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; - } else { - RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); - tmp_msg = generateCtrlCmdMsg(0.0); - } - predicted_trajectory.points.push_back( - calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg)); - } - } - - // for last point - predicted_trajectory.points.back().longitudinal_velocity_mps = 0.0; - predicted_trajectory.header.frame_id = trajectory_resampled_->header.frame_id; - predicted_trajectory.header.stamp = trajectory_resampled_->header.stamp; - - return predicted_trajectory; -} - -bool PurePursuitLateralController::isReady([[maybe_unused]] const InputData & input_data) -{ - return true; -} - -LateralOutput PurePursuitLateralController::run(const InputData & input_data) -{ - current_pose_ = input_data.current_odometry.pose.pose; - trajectory_ = input_data.current_trajectory; - current_odometry_ = input_data.current_odometry; - current_steering_ = input_data.current_steering; - - setResampledTrajectory(); - if (param_.enable_path_smoothing) { - averageFilterTrajectory(*trajectory_resampled_); - } - const auto cmd_msg = generateOutputControlCmd(); - - LateralOutput output; - output.control_cmd = cmd_msg; - output.sync_data.is_steer_converged = calcIsSteerConverged(cmd_msg); - - // calculate predicted trajectory with iterative calculation - const auto predicted_trajectory = generatePredictedTrajectory(); - if (!predicted_trajectory) { - RCLCPP_ERROR(logger_, "Failed to generate predicted trajectory."); - } else { - pub_predicted_trajectory_->publish(*predicted_trajectory); - } - - return output; -} - -bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCommand & cmd) -{ - return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) < - static_cast(param_.converged_steer_rad_); -} - -AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() -{ - // Generate the control command - const auto pp_output = calcTargetCurvature(true, current_odometry_.pose.pose); - AckermannLateralCommand output_cmd; - - if (pp_output) { - output_cmd = generateCtrlCmdMsg(pp_output->curvature); - prev_cmd_ = boost::optional(output_cmd); - publishDebugMarker(); - } else { - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 5000, "failed to solve pure_pursuit for control command calculation"); - if (prev_cmd_) { - output_cmd = *prev_cmd_; - } else { - output_cmd = generateCtrlCmdMsg(0.0); - } - } - return output_cmd; -} - -AckermannLateralCommand PurePursuitLateralController::generateCtrlCmdMsg( - const double target_curvature) -{ - const double tmp_steering = - planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); - AckermannLateralCommand cmd; - cmd.stamp = clock_->now(); - cmd.steering_tire_angle = static_cast( - std::min(std::max(tmp_steering, -param_.max_steering_angle), param_.max_steering_angle)); - - // pub_ctrl_cmd_->publish(cmd); - return cmd; -} - -void PurePursuitLateralController::publishDebugMarker() const -{ - visualization_msgs::msg::MarkerArray marker_array; - - marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target)); - marker_array.markers.push_back( - createTrajectoryCircleMarker(debug_data_.next_target, current_odometry_.pose.pose)); -} - -boost::optional PurePursuitLateralController::calcTargetCurvature( - bool is_control_output, geometry_msgs::msg::Pose pose) -{ - // Ignore invalid trajectory - if (trajectory_resampled_->points.size() < 3) { - RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "received path size is < 3, ignored"); - return {}; - } - - // Calculate target point for velocity/acceleration - - const auto closest_idx_result = - motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4); - if (!closest_idx_result) { - RCLCPP_ERROR(logger_, "cannot find closest waypoint"); - return {}; - } - - const double target_vel = - trajectory_resampled_->points.at(*closest_idx_result).longitudinal_velocity_mps; - - // calculate the lateral error - - const double lateral_error = - motion_utils::calcLateralOffset(trajectory_resampled_->points, pose.position); - - // calculate the current curvature - - const double current_curvature = calcCurvature(*closest_idx_result); - - // Calculate lookahead distance - - const bool is_reverse = (target_vel < 0); - const double min_lookahead_distance = - is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance; - double lookahead_distance = min_lookahead_distance; - if (is_control_output) { - lookahead_distance = calcLookaheadDistance( - lateral_error, current_curvature, current_odometry_.twist.twist.linear.x, - min_lookahead_distance, is_control_output); - } else { - lookahead_distance = calcLookaheadDistance( - lateral_error, current_curvature, target_vel, min_lookahead_distance, is_control_output); - } - - // Set PurePursuit data - pure_pursuit_->setCurrentPose(pose); - pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_resampled_)); - pure_pursuit_->setLookaheadDistance(lookahead_distance); - - // Run PurePursuit - const auto pure_pursuit_result = pure_pursuit_->run(); - if (!pure_pursuit_result.first) { - return {}; - } - - const auto kappa = pure_pursuit_result.second; - - // Set debug data - if (is_control_output) { - debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget(); - } - PpOutput output{}; - output.curvature = kappa; - if (!is_control_output) { - output.velocity = current_odometry_.twist.twist.linear.x; - } else { - output.velocity = target_vel; - } - - return output; -} -} // namespace pure_pursuit diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp deleted file mode 100644 index 254b83bccbc34..0000000000000 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp +++ /dev/null @@ -1,230 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Copyright 2015-2019 Autoware Foundation. 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 "pure_pursuit/pure_pursuit_node.hpp" - -#include "pure_pursuit/pure_pursuit_viz.hpp" -#include "pure_pursuit/util/planning_utils.hpp" -#include "pure_pursuit/util/tf_utils.hpp" - -#include - -#include -#include -#include - -namespace -{ -double calcLookaheadDistance( - const double velocity, const double lookahead_distance_ratio, const double min_lookahead_distance) -{ - const double lookahead_distance = lookahead_distance_ratio * std::abs(velocity); - return std::max(lookahead_distance, min_lookahead_distance); -} - -} // namespace - -namespace pure_pursuit -{ -PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) -: Node("pure_pursuit", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) -{ - pure_pursuit_ = std::make_unique(); - - // Vehicle Parameters - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); - param_.wheel_base = vehicle_info.wheel_base_m; - - // Node Parameters - param_.ctrl_period = this->declare_parameter("control_period"); - - // Algorithm Parameters - param_.lookahead_distance_ratio = this->declare_parameter("lookahead_distance_ratio"); - param_.min_lookahead_distance = this->declare_parameter("min_lookahead_distance"); - param_.reverse_min_lookahead_distance = - this->declare_parameter("reverse_min_lookahead_distance"); - - // Subscribers - using std::placeholders::_1; - sub_trajectory_ = this->create_subscription( - "input/reference_trajectory", 1, std::bind(&PurePursuitNode::onTrajectory, this, _1)); - sub_current_odometry_ = this->create_subscription( - "input/current_odometry", 1, std::bind(&PurePursuitNode::onCurrentOdometry, this, _1)); - - // Publishers - pub_ctrl_cmd_ = this->create_publisher( - "output/control_raw", 1); - - // Debug Publishers - pub_debug_marker_ = - this->create_publisher("~/debug/markers", 0); - - // Timer - { - const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(param_.ctrl_period)); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&PurePursuitNode::onTimer, this)); - } - - // Wait for first current pose - tf_utils::waitForTransform(tf_buffer_, "map", "base_link"); -} - -bool PurePursuitNode::isDataReady() -{ - if (!current_odometry_) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_odometry..."); - return false; - } - - if (!trajectory_) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for trajectory..."); - return false; - } - - if (!current_pose_) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_pose..."); - return false; - } - - return true; -} - -void PurePursuitNode::onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - current_odometry_ = msg; -} - -void PurePursuitNode::onTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) -{ - trajectory_ = msg; -} - -void PurePursuitNode::onTimer() -{ - current_pose_ = self_pose_listener_.getCurrentPose(); - - if (!isDataReady()) { - return; - } - - const auto target_curvature = calcTargetCurvature(); - - if (target_curvature) { - publishCommand(*target_curvature); - publishDebugMarker(); - } else { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "failed to solve pure_pursuit"); - publishCommand({0.0}); - } -} - -void PurePursuitNode::publishCommand(const double target_curvature) -{ - autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; - cmd.stamp = get_clock()->now(); - cmd.steering_tire_angle = - planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); - pub_ctrl_cmd_->publish(cmd); -} - -void PurePursuitNode::publishDebugMarker() const -{ - visualization_msgs::msg::MarkerArray marker_array; - - marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target)); - marker_array.markers.push_back( - createTrajectoryCircleMarker(debug_data_.next_target, current_pose_->pose)); - - pub_debug_marker_->publish(marker_array); -} - -boost::optional PurePursuitNode::calcTargetCurvature() -{ - // Ignore invalid trajectory - if (trajectory_->points.size() < 3) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "received path size is < 3, ignored"); - return {}; - } - - // Calculate target point for velocity/acceleration - const auto target_point = calcTargetPoint(); - if (!target_point) { - return {}; - } - - const double target_vel = target_point->longitudinal_velocity_mps; - - // Calculate lookahead distance - const bool is_reverse = (target_vel < 0); - const double min_lookahead_distance = - is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance; - const double lookahead_distance = calcLookaheadDistance( - current_odometry_->twist.twist.linear.x, param_.lookahead_distance_ratio, - min_lookahead_distance); - - // Set PurePursuit data - pure_pursuit_->setCurrentPose(current_pose_->pose); - pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_)); - pure_pursuit_->setLookaheadDistance(lookahead_distance); - - // Run PurePursuit - const auto pure_pursuit_result = pure_pursuit_->run(); - if (!pure_pursuit_result.first) { - return {}; - } - - const auto kappa = pure_pursuit_result.second; - - // Set debug data - debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget(); - - return kappa; -} - -boost::optional -PurePursuitNode::calcTargetPoint() const -{ - const auto closest_idx_result = planning_utils::findClosestIdxWithDistAngThr( - planning_utils::extractPoses(*trajectory_), current_pose_->pose, 3.0, M_PI_4); - - if (!closest_idx_result.first) { - RCLCPP_ERROR(get_logger(), "cannot find closest waypoint"); - return {}; - } - - return trajectory_->points.at(closest_idx_result.second); -} -} // namespace pure_pursuit - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(pure_pursuit::PurePursuitNode) diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_viz.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_viz.cpp deleted file mode 100644 index 1778bc6fbe79f..0000000000000 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_viz.cpp +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Copyright 2015-2019 Autoware Foundation. 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 "pure_pursuit/pure_pursuit_viz.hpp" - -#include "pure_pursuit/util/marker_helper.hpp" -#include "pure_pursuit/util/planning_utils.hpp" - -#include - -namespace -{ -std::vector generateTrajectoryCircle( - const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose) -{ - constexpr double theta_range = M_PI / 10; - constexpr double step_rad = 0.005; - - const double radius = pure_pursuit::planning_utils::calcRadius(target, current_pose); - - std::vector trajectory_circle; - for (double theta = -theta_range; theta <= theta_range; theta += step_rad) { - geometry_msgs::msg::Point p; - p.x = radius * sin(theta); - p.y = radius * (1 - cos(theta)); - p.z = target.z; - - trajectory_circle.push_back( - pure_pursuit::planning_utils::transformToAbsoluteCoordinate2D(p, current_pose)); - } - - return trajectory_circle; -} - -} // namespace - -namespace pure_pursuit -{ -visualization_msgs::msg::Marker createNextTargetMarker( - const geometry_msgs::msg::Point & next_target) -{ - auto marker = createDefaultMarker( - "map", "next_target", 0, visualization_msgs::msg::Marker::SPHERE, - createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(0.0, 1.0, 0.0, 1.0)); - - marker.pose.position = next_target; - - return marker; -} - -visualization_msgs::msg::Marker createTrajectoryCircleMarker( - const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose) -{ - auto marker = createDefaultMarker( - "map", "trajectory_circle", 0, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0, 0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); - - const auto trajectory_circle = generateTrajectoryCircle(target, current_pose); - for (auto p : trajectory_circle) { - marker.points.push_back(p); - marker.colors.push_back(marker.color); - } - - return marker; -} -} // namespace pure_pursuit diff --git a/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp b/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp deleted file mode 100644 index 0ee96e970782c..0000000000000 --- a/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp +++ /dev/null @@ -1,212 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/* - * Copyright 2015-2019 Autoware Foundation. 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 "pure_pursuit/pure_pursuit.hpp" - -#include "pure_pursuit/util/planning_utils.hpp" - -#include -#include -#include -#include - -namespace pure_pursuit -{ -bool PurePursuit::isDataReady() -{ - if (!curr_wps_ptr_) { - return false; - } - if (!curr_pose_ptr_) { - return false; - } - return true; -} - -std::pair PurePursuit::run() -{ - if (!isDataReady()) { - return std::make_pair(false, std::numeric_limits::quiet_NaN()); - } - - auto closest_pair = planning_utils::findClosestIdxWithDistAngThr( - *curr_wps_ptr_, *curr_pose_ptr_, closest_thr_dist_, closest_thr_ang_); - - if (!closest_pair.first) { - RCLCPP_WARN( - logger, "cannot find, curr_bool: %d, closest_idx: %d", closest_pair.first, - closest_pair.second); - return std::make_pair(false, std::numeric_limits::quiet_NaN()); - } - - int32_t next_wp_idx = findNextPointIdx(closest_pair.second); - if (next_wp_idx == -1) { - RCLCPP_WARN(logger, "lost next waypoint"); - return std::make_pair(false, std::numeric_limits::quiet_NaN()); - } - - loc_next_wp_ = curr_wps_ptr_->at(next_wp_idx).position; - - geometry_msgs::msg::Point next_tgt_pos; - // if next waypoint is first - if (next_wp_idx == 0) { - next_tgt_pos = curr_wps_ptr_->at(next_wp_idx).position; - } else { - // linear interpolation - std::pair lerp_pair = lerpNextTarget(next_wp_idx); - - if (!lerp_pair.first) { - RCLCPP_WARN(logger, "lost target! "); - return std::make_pair(false, std::numeric_limits::quiet_NaN()); - } - - next_tgt_pos = lerp_pair.second; - } - loc_next_tgt_ = next_tgt_pos; - - double kappa = planning_utils::calcCurvature(next_tgt_pos, *curr_pose_ptr_); - - return std::make_pair(true, kappa); -} - -// linear interpolation of next target -std::pair PurePursuit::lerpNextTarget(int32_t next_wp_idx) -{ - constexpr double ERROR2 = 1e-5; // 0.00001 - const geometry_msgs::msg::Point & vec_end = curr_wps_ptr_->at(next_wp_idx).position; - const geometry_msgs::msg::Point & vec_start = curr_wps_ptr_->at(next_wp_idx - 1).position; - const geometry_msgs::msg::Pose & curr_pose = *curr_pose_ptr_; - - Eigen::Vector3d vec_a( - (vec_end.x - vec_start.x), (vec_end.y - vec_start.y), (vec_end.z - vec_start.z)); - - if (vec_a.norm() < ERROR2) { - RCLCPP_ERROR(logger, "waypoint interval is almost 0"); - return std::make_pair(false, geometry_msgs::msg::Point()); - } - - const double lateral_error = - planning_utils::calcLateralError2D(vec_start, vec_end, curr_pose.position); - - if (fabs(lateral_error) > lookahead_distance_) { - RCLCPP_ERROR(logger, "lateral error is larger than lookahead distance"); - RCLCPP_ERROR( - logger, "lateral error: %lf, lookahead distance: %lf", lateral_error, lookahead_distance_); - return std::make_pair(false, geometry_msgs::msg::Point()); - } - - /* calculate the position of the foot of a perpendicular line */ - Eigen::Vector2d uva2d(vec_a.x(), vec_a.y()); - uva2d.normalize(); - Eigen::Rotation2Dd rot = - (lateral_error > 0) ? Eigen::Rotation2Dd(-M_PI / 2.0) : Eigen::Rotation2Dd(M_PI / 2.0); - Eigen::Vector2d uva2d_rot = rot * uva2d; - - geometry_msgs::msg::Point h; - h.x = curr_pose.position.x + fabs(lateral_error) * uva2d_rot.x(); - h.y = curr_pose.position.y + fabs(lateral_error) * uva2d_rot.y(); - h.z = curr_pose.position.z; - - // if there is a intersection - if (fabs(fabs(lateral_error) - lookahead_distance_) < ERROR2) { - return std::make_pair(true, h); - } else { - // if there are two intersection - // get intersection in front of vehicle - const double s = sqrt(pow(lookahead_distance_, 2) - pow(lateral_error, 2)); - geometry_msgs::msg::Point res; - res.x = h.x + s * uva2d.x(); - res.y = h.y + s * uva2d.y(); - res.z = curr_pose.position.z; - return std::make_pair(true, res); - } -} - -int32_t PurePursuit::findNextPointIdx(int32_t search_start_idx) -{ - // if waypoints are not given, do nothing. - if (curr_wps_ptr_->empty() || search_start_idx == -1) { - return -1; - } - - // look for the next waypoint. - for (int32_t i = search_start_idx; i < (int32_t)curr_wps_ptr_->size(); i++) { - // if search waypoint is the last - if (i == ((int32_t)curr_wps_ptr_->size() - 1)) { - return i; - } - - // if waypoint direction is forward - const auto gld = planning_utils::getLaneDirection(*curr_wps_ptr_, 0.05); - if (gld == 0) { - // if waypoint is not in front of ego, skip - auto ret = planning_utils::transformToRelativeCoordinate2D( - curr_wps_ptr_->at(i).position, *curr_pose_ptr_); - if (ret.x < 0) { - continue; - } - } else if (gld == 1) { - // waypoint direction is backward - - // if waypoint is in front of ego, skip - auto ret = planning_utils::transformToRelativeCoordinate2D( - curr_wps_ptr_->at(i).position, *curr_pose_ptr_); - if (ret.x > 0) { - continue; - } - } else { - return -1; - } - - const geometry_msgs::msg::Point & curr_motion_point = curr_wps_ptr_->at(i).position; - const geometry_msgs::msg::Point & curr_pose_point = curr_pose_ptr_->position; - // if there exists an effective waypoint - const double ds = planning_utils::calcDistSquared2D(curr_motion_point, curr_pose_point); - if (ds > std::pow(lookahead_distance_, 2)) { - return i; - } - } - - // if this program reaches here , it means we lost the waypoint! - return -1; -} - -void PurePursuit::setCurrentPose(const geometry_msgs::msg::Pose & msg) -{ - curr_pose_ptr_ = std::make_shared(); - *curr_pose_ptr_ = msg; -} - -void PurePursuit::setWaypoints(const std::vector & msg) -{ - curr_wps_ptr_ = std::make_shared>(); - *curr_wps_ptr_ = msg; -} - -} // namespace pure_pursuit diff --git a/control/shift_decider/CMakeLists.txt b/control/shift_decider/CMakeLists.txt deleted file mode 100644 index ca6f4af74f8ca..0000000000000 --- a/control/shift_decider/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(shift_decider) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(shift_decider_node SHARED - src/shift_decider.cpp -) - -rclcpp_components_register_node(shift_decider_node - PLUGIN "ShiftDecider" - EXECUTABLE shift_decider -) - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/control/shift_decider/README.md b/control/shift_decider/README.md deleted file mode 100644 index 820432e4c9e1a..0000000000000 --- a/control/shift_decider/README.md +++ /dev/null @@ -1,56 +0,0 @@ -# Shift Decider - -## Purpose - -`shift_decider` is a module to decide shift from ackermann control command. - -## Inner-workings / Algorithms - -### Flow chart - -```plantuml -@startuml -skinparam monochrome true - -title update current shift -start -if (absolute target velocity is less than threshold) then (yes) - :set previous shift; -else(no) -if (target velocity is positive) then (yes) - :set shift DRIVE; -else - :set shift REVERSE; -endif -endif - :publish current shift; -note right - publish shift for constant interval -end note -stop -@enduml -``` - -### Algorithms - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| --------------------- | ---------------------------------------------------------- | ---------------------------- | -| `~/input/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Control command for vehicle. | - -### Output - -| Name | Type | Description | -| ------------------ | ---------------------------------------------- | ---------------------------------- | -| `~output/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Gear for drive forward / backward. | - -## Parameters - -none. - -## Assumptions / Known limits - -TBD. diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp deleted file mode 100644 index a6b9938e404f6..0000000000000 --- a/control/shift_decider/include/shift_decider/shift_decider.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef SHIFT_DECIDER__SHIFT_DECIDER_HPP_ -#define SHIFT_DECIDER__SHIFT_DECIDER_HPP_ - -#include - -#include -#include -#include -#include - -#include - -class ShiftDecider : public rclcpp::Node -{ -public: - explicit ShiftDecider(const rclcpp::NodeOptions & node_options); - -private: - void onTimer(); - void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg); - void onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg); - void onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg); - void updateCurrentShiftCmd(); - void initTimer(double period_s); - - rclcpp::Publisher::SharedPtr pub_shift_cmd_; - rclcpp::Subscription::SharedPtr - sub_control_cmd_; - rclcpp::Subscription::SharedPtr - sub_autoware_state_; - rclcpp::Subscription::SharedPtr sub_current_gear_; - - rclcpp::TimerBase::SharedPtr timer_; - - autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr control_cmd_; - autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_; - autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_; - autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_; - uint8_t prev_shift_command = autoware_auto_vehicle_msgs::msg::GearCommand::PARK; - - bool park_on_goal_; -}; - -#endif // SHIFT_DECIDER__SHIFT_DECIDER_HPP_ diff --git a/control/shift_decider/launch/shift_decider.launch.xml b/control/shift_decider/launch/shift_decider.launch.xml deleted file mode 100644 index 73ed434ade7f6..0000000000000 --- a/control/shift_decider/launch/shift_decider.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml deleted file mode 100644 index b24dbab1786e1..0000000000000 --- a/control/shift_decider/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - shift_decider - 0.1.0 - The shift_decider package - Takamasa Horibe - Apache License 2.0 - - Takamasa Horibe - - ament_cmake - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs - rclcpp - rclcpp_components - - ament_cmake_cppcheck - ament_cmake_cpplint - ament_lint_auto - - - ament_cmake - - diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp deleted file mode 100644 index 0e47cc7378f27..0000000000000 --- a/control/shift_decider/src/shift_decider.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "shift_decider/shift_decider.hpp" - -#include - -#include -#include -#include -#include - -ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) -: Node("shift_decider", node_options) -{ - using std::placeholders::_1; - - static constexpr std::size_t queue_size = 1; - rclcpp::QoS durable_qos(queue_size); - durable_qos.transient_local(); - - park_on_goal_ = declare_parameter("park_on_goal"); - - pub_shift_cmd_ = - create_publisher("output/gear_cmd", durable_qos); - sub_control_cmd_ = create_subscription( - "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1)); - sub_autoware_state_ = create_subscription( - "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1)); - sub_current_gear_ = create_subscription( - "input/current_gear", queue_size, std::bind(&ShiftDecider::onCurrentGear, this, _1)); - - initTimer(0.1); -} - -void ShiftDecider::onControlCmd( - autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg) -{ - control_cmd_ = msg; -} - -void ShiftDecider::onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg) -{ - autoware_state_ = msg; -} - -void ShiftDecider::onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) -{ - current_gear_ptr_ = msg; -} - -void ShiftDecider::onTimer() -{ - if (!autoware_state_ || !control_cmd_ || !current_gear_ptr_) { - return; - } - - updateCurrentShiftCmd(); - pub_shift_cmd_->publish(shift_cmd_); -} - -void ShiftDecider::updateCurrentShiftCmd() -{ - using autoware_auto_system_msgs::msg::AutowareState; - using autoware_auto_vehicle_msgs::msg::GearCommand; - - shift_cmd_.stamp = now(); - static constexpr double vel_threshold = 0.01; // to prevent chattering - if (autoware_state_->state == AutowareState::DRIVING) { - if (control_cmd_->longitudinal.speed > vel_threshold) { - shift_cmd_.command = GearCommand::DRIVE; - } else if (control_cmd_->longitudinal.speed < -vel_threshold) { - shift_cmd_.command = GearCommand::REVERSE; - } else { - shift_cmd_.command = prev_shift_command; - } - } else { - if ( - (autoware_state_->state == AutowareState::ARRIVED_GOAL || - autoware_state_->state == AutowareState::WAITING_FOR_ROUTE) && - park_on_goal_) { - shift_cmd_.command = GearCommand::PARK; - } else { - shift_cmd_.command = current_gear_ptr_->report; - } - } - prev_shift_command = shift_cmd_.command; -} - -void ShiftDecider::initTimer(double period_s) -{ - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = - rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&ShiftDecider::onTimer, this)); -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(ShiftDecider) diff --git a/control/smart_mpc_trajectory_follower/CMakeLists.txt b/control/smart_mpc_trajectory_follower/CMakeLists.txt deleted file mode 100644 index 7aecab2597dcd..0000000000000 --- a/control/smart_mpc_trajectory_follower/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(smart_mpc_trajectory_follower) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -install(PROGRAMS - smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py - DESTINATION lib/${PROJECT_NAME} -) -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/control/smart_mpc_trajectory_follower/README.md b/control/smart_mpc_trajectory_follower/README.md deleted file mode 100644 index 8fcc13142d68b..0000000000000 --- a/control/smart_mpc_trajectory_follower/README.md +++ /dev/null @@ -1,338 +0,0 @@ -

- - - -

- - -# Smart MPC Trajectory Follower - -Smart MPC (Model Predictive Control) is a control algorithm that combines model predictive control and machine learning. While inheriting the advantages of model predictive control, it solves its disadvantage of modeling difficulty with a data-driven method using machine learning. - -This technology makes it relatively easy to operate model predictive control, which is expensive to implement, as long as an environment for collecting data can be prepared. - -

- - - -

- -## Setup - -After building autoware, move to `control/smart_mpc_trajectory_follower` and run the following command: - -```bash -pip3 install . -``` - -If you have already installed and want to update the package, run the following command instead: - -```bash -pip3 install -U . -``` - -## Provided features - -This package provides smart MPC logic for path-following control as well as mechanisms for learning and evaluation. These features are described below. - -### Trajectory following control based on iLQR/MPPI - -The control mode can be selected from "ilqr", "mppi", or "mppi_ilqr", and can be set as `mpc_parameter:system:mode` in [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml). -In "mppi_ilqr" mode, the initial value of iLQR is given by the MPPI solution. - -> [!NOTE] -> With the default settings, the performance of "mppi" mode is limited due to an insufficient number of samples. This issue is being addressed with ongoing work to introduce GPU support. - -To perform a simulation, run the following command: - -```bash -ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit trajectory_follower_mode:=smart_mpc_trajectory_follower -``` - -> [!NOTE] -> When running with the nominal model set in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml), set `trained_model_parameter:control_application:use_trained_model` to `false` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). To run using the trained model, set `trained_model_parameter:control_application:use_trained_model` to `true`, but the trained model must have been generated according to the following procedure. - -### Training of model and reflection in control - -To obtain training data, start autoware, perform a drive, and record rosbag data with the following commands. - -```bash -ros2 bag record /localization/kinematic_state /localization/acceleration /vehicle/status/steering_status /control/command/control_cmd /control/trajectory_follower/control_cmd /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw /system/operation_mode/state /vehicle/status/control_mode /sensing/imu/imu_data /debug_mpc_x_des /debug_mpc_y_des /debug_mpc_v_des /debug_mpc_yaw_des /debug_mpc_acc_des /debug_mpc_steer_des /debug_mpc_X_des_converted /debug_mpc_x_current /debug_mpc_error_prediction /debug_mpc_max_trajectory_err /debug_mpc_emergency_stop_mode /debug_mpc_goal_stop_mode /debug_mpc_total_ctrl_time /debug_mpc_calc_u_opt_time -``` - -Move [rosbag2.bash](./smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash) to the rosbag directory recorded above and execute the following command on the directory - -```bash -bash rosbag2.bash -``` - -This converts rosbag data into CSV format for training models. - -> [!NOTE] -> Note that a large number of terminals are automatically opened at runtime, but they are automatically closed after rosbag data conversion is completed. -> From the time you begin this process until all terminals are closed, autoware should not be running. - -Instead, the same result can be obtained by executing the following command in a python environment: - -```python -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model -model_trainer = train_drive_NN_model.train_drive_NN_model() -model_trainer.transform_rosbag_to_csv(rosbag_dir) -``` - -Here, `rosbag_dir` represents the rosbag directory. -At this time, all CSV files in `rosbag_dir` are automatically deleted first. - -The paths of the rosbag directories used for training, `dir_0`, `dir_1`, `dir_2`,... and the directory `save_dir` where you save the models, the model can be saved in the python environment as follows: - -```python -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model -model_trainer = train_drive_NN_model.train_drive_NN_model() -model_trainer.add_data_from_csv(dir_0) -model_trainer.add_data_from_csv(dir_1) -model_trainer.add_data_from_csv(dir_2) -... -model_trainer.get_trained_model() -model_trainer.save_models(save_dir) -``` - -After performing the polynomial regression, the NN can be trained on the residuals as follows: - -```python -model_trainer.get_trained_model(use_polynomial_reg=True) -``` - -> [!NOTE] -> In the default setting, regression is performed by several preselected polynomials. -> When `use_selected_polynomial=False` is set as the argument of get_trained_model, the `deg` argument allows setting the maximum degree of the polynomial to be used. - -If only polynomial regression is performed and no NN model is used, run the following command: - -```python -model_trainer.get_trained_model(use_polynomial_reg=True,force_NN_model_to_zero=True) -``` - -Move `model_for_test_drive.pth` and `polynomial_reg_info.npz` saved in `save_dir` to the home directory and set `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml) to `true` to reflect the trained model in the control. - -### Performance evaluation - -Here, as an example, we describe the verification of the adaptive performance when the wheel base of the sample_vehicle is 2.79 m, but an incorrect value of 2.0 m is given to the controller side. -To give the controller 2.0 m as the wheel base, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.0, and run the following command: - -```bash -python3 -m smart_mpc_trajectory_follower.clear_pycache -``` - -#### Test on autoware - -To perform a control test on autoware with the nominal model before training, make sure that `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml) is `false` and launch autoware in the manner described in "Trajectory following control based on iLQR/MPPI". This time, the following route will be used for the test: - -

- -Record rosbag and train the model in the manner described in "Training of model and reflection in control", and move the generated files `model_for_test_drive.pth` and `polynomial_reg_info.npz` to the home directory. - -> [!NOTE] -> Although the data used for training is small, for the sake of simplicity, we will see how much performance can be improved with this amount of data. - -To control using the trained model obtained here, set `trained_model_parameter:control_application:use_trained_model` to `true`, start autoware in the same way, and drive the same route recording rosbag. -After the driving is complete, convert the rosbag file to CSV format using the method described in "Training of model and reflection in control". -A plot of the lateral deviation is obtained by running the `lateral_error_visualize` function in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb` for the nominal and training model rosbag files `rosbag_nominal` and `rosbag_trained`, respectively, as follows: - -```python -lateral_error_visualize(dir_name=rosbag_nominal,ylim=[-1.2,1.2]) -lateral_error_visualize(dir_name=rosbag_trained,ylim=[-1.2,1.2]) -``` - -The following results were obtained. - -
- - -
- -#### Test on python simulator - -First, to give wheel base 2.79 m in the python simulator, create the following file and save it in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator` with the name `sim_setting.json`: - -```json -{ "wheel_base": 2.79 } -``` - -Next, run the following commands to test the slalom driving on the python simulator with the nominal model: - -```python -import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model -initial_error = [0.0, 0.03, 0.01, -0.01, 0.0, 0.0] -save_dir = "test_python_sim" -python_simulator.slalom_drive(save_dir=save_dir,use_trained_model=False,initial_error=initial_error) -``` - -Here, `initial_error` is the initial error from the target trajectory, in the order of x-coordinate, y-coordinate, longitudinal velocity, yaw angle, longitudinal acceleration, and steer angle, -and `save_dir` is the directory where the driving test results are saved. - -> [!NOTE] -> The value of `use_trained_model` given as the argument of `python_simulator.slalom_drive` takes precedence over the value of `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). - -Run the following commands to perform training using driving data of the nominal model. - -```python -model_trainer = train_drive_NN_model.train_drive_NN_model() -model_trainer.add_data_from_csv(save_dir) -model_trainer.save_train_data(save_dir) -model_trainer.get_trained_model(use_polynomial_reg=True) -model_trainer.save_models(save_dir=save_dir) -``` - -This way, files `model_for_test_drive.pth` and `polynomial_reg_info.npz` are saved in `save_dir`. -The following results were obtained. - -

- -

- -The center of the upper row represents the lateral deviation. - -Finally, to drive with the training model, run the following commands: - -```python -load_dir = save_dir -save_dir = "test_python_trained_sim" -python_simulator.slalom_drive(save_dir=save_dir,load_dir=load_dir,use_trained_model=True,initial_error=initial_error) -``` - -The following results were obtained. - -

- -

- -It can be seen that the lateral deviation has improved significantly. - -Here we have described wheel base, but the parameters that can be passed to the python simulator are as follows. - -| Parameter | Type | Description | -| ------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| steer_bias | float | steer bias [rad] | -| steer_rate_lim | float | steer rate limit [rad/s] | -| vel_rate_lim | float | acceleration limit [m/s^2] | -| wheel_base | float | wheel base [m] | -| steer_dead_band | float | steer dead band [rad] | -| adaptive_gear_ratio_coef | list[float] | List of floats of length 6 specifying information on speed-dependent gear ratios from tire angle to steering wheel angle. | -| acc_time_delay | float | acceleration time delay [s] | -| steer_time_delay | float | steer time delay [s] | -| acc_time_constant | float | acceleration time constant [s] | -| steer_time_constant | float | steer time constant [s] | -| accel_map_scale | float | Parameter that magnifies the corresponding distortion from acceleration input values to actual acceleration realizations.
Correspondence information is kept in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv`. | -| acc_scaling | float | acceleration scaling | -| steer_scaling | float | steer scaling | - -For example, to give the simulation side 0.01 [rad] of steer bias and 0.001 [rad] of steer dead band, edit the `sim_setting.json` as follows. - -```json -{ "steer_bias": 0.01, "steer_dead_band": 0.001 } -``` - -#### Auto test on python simulator - -Here, we describe a method for testing adaptive performance by giving the simulation side a predefined range of model parameters while the control side is given constant model parameters. - -First, to restore nominal model settings to default values, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.79, and run the following command: - -```bash -python3 -m smart_mpc_trajectory_follower.clear_pycache -``` - -To run a driving experiment within the parameter change range set in [run_sim.py](./smart_mpc_trajectory_follower/python_simulator/run_sim.py), for example, move to `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator` and run the following command: - -```bash -python3 run_sim.py --param_name steer_bias -``` - -Here we described the experimental procedure for steer bias, and the same method can be used for other parameters. - -If you want to do it for all parameters at once, run the following command: - -```bash -yes | python3 run_sim.py -``` - -In `run_sim.py`, the following parameters can be set: - -| Parameter | Type | Description | -| ------------------------- | ---- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| USE_TRAINED_MODEL_DIFF | bool | Whether the derivative of the trained model is reflected in the control | -| DATA_COLLECTION_MODE | str | Which method will be used to collect the training data　
"ff": Straight line driving with feed-forward input
"pp": Figure eight driving with pure pursuit control
"mpc": Slalom driving with mpc | -| USE_POLYNOMIAL_REGRESSION | bool | Whether to perform polynomial regression before NN | -| USE_SELECTED_POLYNOMIAL | bool | When USE_POLYNOMIAL_REGRESSION is True, perform polynomial regression using only some preselected polynomials.
The choice of polynomials is intended to be able to absorb the contribution of some parameter shifts based on the nominal model of the vehicle. | -| FORCE_NN_MODEL_TO_ZERO | bool | Whether to force the NN model to zero (i.e., erase the contribution of the NN model).
When USE_POLYNOMIAL_REGRESSION is True, setting FORCE_MODEL_TO_ZERO to True allows the control to reflect the results of polynomial regression only, without using NN models. | -| FIT_INTERCEPT | bool | Whether to include bias in polynomial regression.
If it is False, perform the regression with a polynomial of the first degree or higher. | -| USE_INTERCEPT | bool | When a polynomial regression including bias is performed, whether to use or discard the resulting bias information.
It is meaningful only if FIT_INTERCEPT is True.
If it is False, discard the bias in the polynomial regression in the hope that the NN model can remove the bias term, even if the polynomial regression is performed with the bias included. | - -> [!NOTE] -> When `run_sim.py` is run, the `use_trained_model_diff` set in `run_sim.py` takes precedence over the `trained_model_parameter:control_application:use_trained_model_diff` set in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). - -## Change of nominal parameters and their reloading - -The nominal parameters of vehicle model can be changed by editing the file [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml). -After changing the nominal parameters, the cache must be deleted by running the following command: - -```bash -python3 -m smart_mpc_trajectory_follower.clear_pycache -``` - -The nominal parameters include the following: - -| Parameter | Type | Description | -| ------------------------------------------------ | ----- | ------------------------------ | -| nominal_parameter:vehicle_info:wheel_base | float | wheel base [m] | -| nominal_parameter:acceleration:acc_time_delay | float | acceleration time delay [s] | -| nominal_parameter:acceleration:acc_time_constant | float | acceleration time constant [s] | -| nominal_parameter:steering:steer_time_delay | float | steer time delay [s] | -| nominal_parameter:steering:steer_time_constant | float | steer time constant [s] | - -## Change of control parameters and their reloading - -The control parameters can be changed by editing files [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml) and [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). -Although it is possible to reflect parameter changes by restarting autoware, the following command allows us to do so without leaving autoware running: - -```bash -ros2 topic pub /pympc_reload_mpc_param_trigger std_msgs/msg/String "data: ''" --once -``` - -The main parameters among the control parameters are as follows. - -### `mpc_param.yaml` - -| Parameter | Type | Description | -| ------------------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| mpc_parameter:system:mode | str | control mode
"ilqr": iLQR mode
"mppi": MPPI mode
"mppi_ilqr": the initial value of iLQR is given by the MPPI solution. | -| mpc_parameter:cost_parameters:Q | list[float] | Stage cost for states.
List of length 8, in order: straight deviation, lateral deviation, velocity deviation, yaw angle deviation, acceleration deviation, steer deviation, acceleration input deviation, steer input deviation cost weights. | -| mpc_parameter:cost_parameters:Q_c | list[float] | Cost in the horizon corresponding to the following timing_Q_c for the states.
The correspondence of the components of the list is the same as for Q. | -| mpc_parameter:cost_parameters:Q_f | list[float] | Termination cost for the states.
The correspondence of the components of the list is the same as for Q. | -| mpc_parameter:cost_parameters:R | list[float] | A list of length 2 where R[0] is weight of cost for the change rate of acceleration input value and R[1] is weight of cost for the change rate of steer input value. | -| mpc_parameter:mpc_setting:timing_Q_c | list[int] | Horizon numbers such that the stage cost for the states is set to Q_c. | - -### `trained_model_param.yaml` - -| Parameter | Type | Description | -| ------------------------------------------------------------------ | ---- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| trained_model_parameter:control_application:use_trained_model | bool | Whether the trained model is reflected in the control or not. | -| trained_model_parameter:control_application:use_trained_model_diff | bool | Whether the derivative of the trained model is reflected on the control or not.
It is meaningful only when use_trained_model is True, and if False, the nominal model is used for the derivative of the dynamics, and trained model is used only for prediction. | - -## Request to release the slow stop mode - -If the predicted trajectory deviates too far from the target trajectory, the system enters a slow stop mode and the vehicle stops moving. -To cancel the slow stop mode and make the vehicle ready to run again, run the following command: - -```bash -ros2 topic pub /pympc_stop_mode_reset_request std_msgs/msg/String "data: ''" --once -``` - -## Limitation - -- May not be able to start when initial position/posture is far from the target. - -- It may take some time until the end of the planning to compile numba functions at the start of the first control. - -- In the stopping action near the goal our control switches to another simple control law. As a result, the stopping action may not work except near the goal. Stopping is also difficult if the acceleration map is significantly shifted. diff --git a/control/smart_mpc_trajectory_follower/package.xml b/control/smart_mpc_trajectory_follower/package.xml deleted file mode 100644 index 70bd15f97d609..0000000000000 --- a/control/smart_mpc_trajectory_follower/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - smart_mpc_trajectory_follower - 1.0.0 - Nodes to follow a trajectory by generating control commands using smart mpc - - - Masayuki Aino - - Apache License 2.0 - - Masayuki Aino - - ament_cmake_auto - ament_cmake_python - autoware_cmake - - autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs - motion_utils - pybind11_vendor - python3-scipy - rclcpp - rclcpp_components - tier4_autoware_utils - - ros2launch - - ament_cmake_ros - ament_index_python - ament_lint_auto - autoware_lint_common - autoware_testing - ros_testing - - - ament_cmake - - diff --git a/control/smart_mpc_trajectory_follower/setup.py b/control/smart_mpc_trajectory_follower/setup.py deleted file mode 100644 index 4dca4d72929a8..0000000000000 --- a/control/smart_mpc_trajectory_follower/setup.py +++ /dev/null @@ -1,37 +0,0 @@ -# cspell: ignore numba -import glob -import json -import os -from pathlib import Path - -from setuptools import find_packages -from setuptools import setup - -os.system("pip3 install numba==0.58.1 --force-reinstall") -os.system("pip3 install pybind11") -os.system("pip3 install GPy") -os.system("pip3 install torch") -package_path = {} -package_path["path"] = str(Path(__file__).parent) -with open("smart_mpc_trajectory_follower/package_path.json", "w") as f: - json.dump(package_path, f) -build_cpp_command = "g++ -Ofast -Wall -shared -std=c++11 -fPIC $(python3 -m pybind11 --includes) " -build_cpp_command += "smart_mpc_trajectory_follower/scripts/proxima_calc.cpp " -build_cpp_command += ( - "-o smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) " -) -build_cpp_command += "-lrt -I/usr/include/eigen3" -os.system(build_cpp_command) - -so_path = ( - "scripts/" - + glob.glob("smart_mpc_trajectory_follower/scripts/proxima_calc.*.so")[0].split("/")[-1] -) -setup( - name="smart_mpc_trajectory_follower", - version="1.0.0", - packages=find_packages(), - package_data={ - "smart_mpc_trajectory_follower": ["package_path.json", so_path], - }, -) diff --git a/control/trajectory_follower_base/CMakeLists.txt b/control/trajectory_follower_base/CMakeLists.txt deleted file mode 100644 index 98f86e468e819..0000000000000 --- a/control/trajectory_follower_base/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(trajectory_follower_base) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/longitudinal_controller_base.cpp - src/lateral_controller_base.cpp -) - -ament_auto_package() diff --git a/control/trajectory_follower_base/README.md b/control/trajectory_follower_base/README.md deleted file mode 100644 index 6bd3d74e75271..0000000000000 --- a/control/trajectory_follower_base/README.md +++ /dev/null @@ -1,53 +0,0 @@ -# Trajectory Follower - -This is the design document for the `trajectory_follower` package. - -## Purpose / Use cases - - - - -This package provides the interface of longitudinal and lateral controllers used by the node of the `trajectory_follower_node` package. -We can implement a detailed controller by deriving the longitudinal and lateral base interfaces. - -## Design - -There are lateral and longitudinal base interface classes and each algorithm inherits from this class to implement. -The interface class has the following base functions. - -- `isReady()`: Check if the control is ready to compute. -- `run()`: Compute control commands and return to [Trajectory Follower Nodes](../trajectory_follower_node/README.md). This must be implemented by inherited algorithms. -- `sync()`: Input the result of running the other controller. - - steer angle convergence - - allow keeping stopped until steer is converged. - - velocity convergence(currently not used) - -See [the Design of Trajectory Follower Nodes](../trajectory_follower_node/README.md#Design) for how these functions work in the node. - -## Separated lateral (steering) and longitudinal (velocity) controls - -This longitudinal controller assumes that the roles of lateral and longitudinal control are separated as follows. - -- Lateral control computes a target steering to keep the vehicle on the trajectory, assuming perfect velocity tracking. -- Longitudinal control computes a target velocity/acceleration to keep the vehicle velocity on the trajectory speed, assuming perfect trajectory tracking. - -Ideally, dealing with the lateral and longitudinal control as a single mixed problem can achieve high performance. In contrast, there are two reasons to provide velocity controller as a stand-alone function, described below. - -### Complex requirements for longitudinal motion - -The longitudinal vehicle behavior that humans expect is difficult to express in a single logic. For example, the expected behavior just before stopping differs depending on whether the ego-position is ahead/behind of the stop line, or whether the current speed is higher/lower than the target speed to achieve a human-like movement. - -In addition, some vehicles have difficulty measuring the ego-speed at extremely low speeds. In such cases, a configuration that can improve the functionality of the longitudinal control without affecting the lateral control is important. - -There are many characteristics and needs that are unique to longitudinal control. Designing them separately from the lateral control keeps the modules less coupled and improves maintainability. - -### Nonlinear coupling of lateral and longitudinal motion - -The lat-lon mixed control problem is very complex and uses nonlinear optimization to achieve high performance. Since it is difficult to guarantee the convergence of the nonlinear optimization, a simple control logic is also necessary for development. - -Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed. - -## Related issues - - diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp deleted file mode 100644 index 52a82526a9548..0000000000000 --- a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2022 The Autoware Foundation -// -// 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 TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ -#define TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ - -#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" - -namespace autoware::motion::control::trajectory_follower -{ -struct InputData -{ - autoware_auto_planning_msgs::msg::Trajectory current_trajectory; - nav_msgs::msg::Odometry current_odometry; - autoware_auto_vehicle_msgs::msg::SteeringReport current_steering; - geometry_msgs::msg::AccelWithCovarianceStamped current_accel; - autoware_adapi_v1_msgs::msg::OperationModeState current_operation_mode; -}; -} // namespace autoware::motion::control::trajectory_follower - -#endif // TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp deleted file mode 100644 index 657c981414c32..0000000000000 --- a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2022 The Autoware Foundation -// -// 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 TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ -#define TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "trajectory_follower_base/input_data.hpp" -#include "trajectory_follower_base/sync_data.hpp" - -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" - -#include - -namespace autoware::motion::control::trajectory_follower -{ -struct LateralOutput -{ - autoware_auto_control_msgs::msg::AckermannLateralCommand control_cmd; - LateralSyncData sync_data; -}; - -class LateralControllerBase -{ -public: - virtual bool isReady(const InputData & input_data) = 0; - virtual LateralOutput run(InputData const & input_data) = 0; - void sync(LongitudinalSyncData const & longitudinal_sync_data); - -protected: - LongitudinalSyncData longitudinal_sync_data_; -}; - -} // namespace autoware::motion::control::trajectory_follower - -#endif // TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp deleted file mode 100644 index 0f9c0d57bb5cd..0000000000000 --- a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 The Autoware Foundation -// -// 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 TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ -#define TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "trajectory_follower_base/input_data.hpp" -#include "trajectory_follower_base/sync_data.hpp" - -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" - -#include - -namespace autoware::motion::control::trajectory_follower -{ -struct LongitudinalOutput -{ - autoware_auto_control_msgs::msg::LongitudinalCommand control_cmd; - LongitudinalSyncData sync_data; -}; -class LongitudinalControllerBase -{ -public: - virtual bool isReady(const InputData & input_data) = 0; - virtual LongitudinalOutput run(InputData const & input_data) = 0; - void sync(LateralSyncData const & lateral_sync_data); - // NOTE: This reset function should be called when the trajectory is replanned by changing ego - // pose or goal pose. - void reset(); - -protected: - LateralSyncData lateral_sync_data_; -}; - -} // namespace autoware::motion::control::trajectory_follower - -#endif // TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp deleted file mode 100644 index c2419427ac961..0000000000000 --- a/control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// 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 TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ -#define TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport) -#define TRAJECTORY_FOLLOWER_LOCAL -#else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport) -#define TRAJECTORY_FOLLOWER_LOCAL -#endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#elif defined(__linux__) -#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) -#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) -#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ diff --git a/control/trajectory_follower_base/package.xml b/control/trajectory_follower_base/package.xml deleted file mode 100644 index d896f874a3a20..0000000000000 --- a/control/trajectory_follower_base/package.xml +++ /dev/null @@ -1,50 +0,0 @@ - - - - trajectory_follower_base - 1.0.0 - Library for generating lateral and longitudinal controls following a trajectory - - - Takamasa Horibe - - Takayuki Murooka - - Apache 2.0 - - Takamasa Horibe - Maxime CLEMENT - Takayuki Murooka - - ament_cmake_auto - autoware_cmake - - autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs - diagnostic_msgs - diagnostic_updater - eigen - geometry_msgs - interpolation - motion_utils - osqp_interface - rclcpp - rclcpp_components - std_msgs - tf2 - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - vehicle_info_util - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/control/trajectory_follower_node/CMakeLists.txt b/control/trajectory_follower_node/CMakeLists.txt deleted file mode 100644 index 653b02eb39ed6..0000000000000 --- a/control/trajectory_follower_node/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(trajectory_follower_node) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -set(CONTROLLER_NODE controller_node) -ament_auto_add_library(${CONTROLLER_NODE} SHARED - include/trajectory_follower_node/controller_node.hpp - src/controller_node.cpp -) - -rclcpp_components_register_node(${CONTROLLER_NODE} - PLUGIN "autoware::motion::control::trajectory_follower_node::Controller" - EXECUTABLE ${CONTROLLER_NODE}_exe -) - -# simple trajectory follower -set(SIMPLE_TRAJECTORY_FOLLOWER_NODE simple_trajectory_follower) -ament_auto_add_library(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} SHARED - include/trajectory_follower_node/simple_trajectory_follower.hpp - src/simple_trajectory_follower.cpp -) - -rclcpp_components_register_node(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} - PLUGIN "simple_trajectory_follower::SimpleTrajectoryFollower" - EXECUTABLE ${SIMPLE_TRAJECTORY_FOLLOWER_NODE}_exe -) - -if(BUILD_TESTING) - set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_node) - ament_add_ros_isolated_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} - test/trajectory_follower_test_utils.hpp - test/test_controller_node.cpp - ) - ament_target_dependencies(${TRAJECTORY_FOLLOWER_NODES_TEST} fake_test_node) - target_link_libraries( - ${TRAJECTORY_FOLLOWER_NODES_TEST} ${CONTROLLER_NODE}) - - find_package(autoware_testing REQUIRED) - # smoke test for MPC controller - add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe - PARAM_FILENAMES "lateral/mpc.param.yaml longitudinal/pid.param.yaml -trajectory_follower_node.param.yaml" - TEST_PARAM_FILENAMES "test_controller_mpc.param.yaml test_controller_pid.param.yaml -test_vehicle_info.param.yaml test_nearest_search.param.yaml" - TARGET_INFIX "mpc" - ) - # smoke test for pure pursuit controller - add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe - PARAM_FILENAMES "lateral/pure_pursuit.param.yaml longitudinal/pid.param.yaml -trajectory_follower_node.param.yaml" - TEST_PARAM_FILENAMES "test_controller_pure_pursuit.param.yaml test_controller_pid.param.yaml -test_vehicle_info.param.yaml test_nearest_search.param.yaml" - TARGET_INFIX "pure_pursuit" - ) - -endif() - -ament_auto_package( - INSTALL_TO_SHARE - param - test - launch - config -) diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md deleted file mode 100644 index c3ea3ddf6b7e8..0000000000000 --- a/control/trajectory_follower_node/README.md +++ /dev/null @@ -1,156 +0,0 @@ -# Trajectory Follower Nodes - -## Purpose - -Generate control commands to follow a given Trajectory. - -## Design - -This is a node of the functionalities implemented in the controller class derived from [trajectory_follower_base](../trajectory_follower_base/README.md#trajectory-follower) package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands. - -By default, the controller instance with the `Controller` class as follows is used. - -```plantuml -@startuml -package trajectory_follower_base { -abstract class LateralControllerBase { -longitudinal_sync_data_ - - virtual isReady(InputData) - virtual run(InputData) - sync(LongitudinalSyncData) - reset() - -} -abstract class LongitudinalControllerBase { -lateral_sync_data_ - - virtual isReady(InputData) - virtual run(InputData) - sync(LateralSyncData) - reset() - -} - -struct InputData { -trajectory -odometry -steering -accel -} -struct LongitudinalSyncData { -is_steer_converged -} -struct LateralSyncData { -} -} - -package mpc_lateral_controller { -class MPCLateralController { -isReady(InputData) override -run(InputData) override -} -} -package pure_pursuit { -class PurePursuitLateralController { -isReady(InputData) override -run(InputData) override -} -} -package pid_longitudinal_controller { -class PIDLongitudinalController { -isReady(InputData) override -run(InputData) override -} -} - -package trajectory_follower_node { -class Controller { -longitudinal_controller_ -lateral_controller_ -onTimer() -createInputData(): InputData -} -} - -MPCLateralController --|> LateralControllerBase -PurePursuitLateralController --|> LateralControllerBase -PIDLongitudinalController --|> LongitudinalControllerBase - -LateralSyncData --> LongitudinalControllerBase -LateralSyncData --> LateralControllerBase -LongitudinalSyncData --> LongitudinalControllerBase -LongitudinalSyncData --> LateralControllerBase -InputData ..> LateralControllerBase -InputData ..> LongitudinalControllerBase - -LateralControllerBase --o Controller -LongitudinalControllerBase --o Controller -InputData ..> Controller -@enduml -``` - -The process flow of `Controller` class is as follows. - -```cpp -// 1. create input data -const auto input_data = createInputData(*get_clock()); -if (!input_data) { - return; -} - -// 2. check if controllers are ready -const bool is_lat_ready = lateral_controller_->isReady(*input_data); -const bool is_lon_ready = longitudinal_controller_->isReady(*input_data); -if (!is_lat_ready || !is_lon_ready) { - return; -} - -// 3. run controllers -const auto lat_out = lateral_controller_->run(*input_data); -const auto lon_out = longitudinal_controller_->run(*input_data); - -// 4. sync with each other controllers -longitudinal_controller_->sync(lat_out.sync_data); -lateral_controller_->sync(lon_out.sync_data); - -// 5. publish control command -control_cmd_pub_->publish(out); -``` - -Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true` - -- lateral controller - - `keep_steer_control_until_converged` -- longitudinal controller - - `enable_keep_stopped_until_steer_convergence` - -### Inputs / Outputs / API - -#### Inputs - -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. -- `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport` current steering - -#### Outputs - -- `autoware_auto_control_msgs/AckermannControlCommand`: message containing both lateral and longitudinal commands. - -#### Parameter - -- `ctrl_period`: control commands publishing period -- `timeout_thr_sec`: duration in second after which input messages are discarded. - - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `AckermannControlCommand` if the following two conditions are met. - 1. Both commands have been received. - 2. The last received commands are not older than defined by `timeout_thr_sec`. -- `lateral_controller_mode`: `mpc` or `pure_pursuit` - - (currently there is only `PID` for longitudinal controller) - -## Debugging - -Debug information are published by the lateral and longitudinal controller using `tier4_debug_msgs/Float32MultiArrayStamped` messages. - -A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging. - -In addition, the predicted MPC trajectory is published on topic `output/lateral/predicted_trajectory` and can be visualized in Rviz. diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp deleted file mode 100644 index be4fe885ae8c1..0000000000000 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ /dev/null @@ -1,134 +0,0 @@ -// Copyright 2021 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 TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ -#define TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_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/system/stop_watch.hpp" -#include "trajectory_follower_base/lateral_controller_base.hpp" -#include "trajectory_follower_base/longitudinal_controller_base.hpp" -#include "trajectory_follower_node/visibility_control.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include -#include -#include - -#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" -#include "geometry_msgs/msg/accel_stamped.hpp" -#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "tf2_msgs/msg/tf_message.hpp" -#include "visualization_msgs/msg/marker_array.hpp" -#include - -#include -#include -#include -#include - -namespace autoware::motion::control -{ -using trajectory_follower::LateralOutput; -using trajectory_follower::LongitudinalOutput; -namespace trajectory_follower_node -{ - -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; - -/// \classController -/// \brief The node class used for generating longitudinal control commands (velocity/acceleration) -class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node -{ -public: - explicit Controller(const rclcpp::NodeOptions & node_options); - virtual ~Controller() {} - -private: - rclcpp::TimerBase::SharedPtr timer_control_; - double timeout_thr_sec_; - boost::optional longitudinal_output_{boost::none}; - - std::shared_ptr longitudinal_controller_; - std::shared_ptr lateral_controller_; - - rclcpp::Subscription::SharedPtr sub_ref_path_; - rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr sub_steering_; - rclcpp::Subscription::SharedPtr sub_accel_; - rclcpp::Subscription::SharedPtr sub_operation_mode_; - rclcpp::Publisher::SharedPtr - control_cmd_pub_; - rclcpp::Publisher::SharedPtr pub_processing_time_lat_ms_; - rclcpp::Publisher::SharedPtr pub_processing_time_lon_ms_; - rclcpp::Publisher::SharedPtr debug_marker_pub_; - - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; - nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_; - autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_; - geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr_; - OperationModeState::SharedPtr current_operation_mode_ptr_; - - enum class LateralControllerMode { - INVALID = 0, - MPC = 1, - PURE_PURSUIT = 2, - }; - enum class LongitudinalControllerMode { - INVALID = 0, - PID = 1, - }; - - /** - * @brief compute control command, and publish periodically - */ - boost::optional createInputData(rclcpp::Clock & clock) const; - void callbackTimerControl(); - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); - void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); - void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg); - void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); - bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out); - LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; - LongitudinalControllerMode getLongitudinalControllerMode( - const std::string & algorithm_name) const; - void publishDebugMarker( - const trajectory_follower::InputData & input_data, - const trajectory_follower::LateralOutput & lat_out) const; - - std::unique_ptr logger_configure_; - - std::unique_ptr published_time_publisher_; - - void publishProcessingTime( - const double t_ms, const rclcpp::Publisher::SharedPtr pub); - StopWatch stop_watch_; -}; -} // namespace trajectory_follower_node -} // namespace autoware::motion::control - -#endif // TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp deleted file mode 100644 index e748bdf25d419..0000000000000 --- a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2022 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 TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ -#define TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ - -#include - -#include -#include -#include -#include -#include -#include - -#include - -namespace simple_trajectory_follower -{ -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::Twist; -using nav_msgs::msg::Odometry; - -class SimpleTrajectoryFollower : public rclcpp::Node -{ -public: - explicit SimpleTrajectoryFollower(const rclcpp::NodeOptions & options); - ~SimpleTrajectoryFollower() = default; - -private: - rclcpp::Subscription::SharedPtr sub_kinematics_; - rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Publisher::SharedPtr pub_cmd_; - rclcpp::TimerBase::SharedPtr timer_; - - Trajectory::SharedPtr trajectory_; - Odometry::SharedPtr odometry_; - TrajectoryPoint closest_traj_point_; - bool use_external_target_vel_; - double external_target_vel_; - double lateral_deviation_; - - void onTimer(); - bool checkData(); - void updateClosest(); - double calcSteerCmd(); - double calcAccCmd(); -}; - -} // namespace simple_trajectory_follower - -#endif // TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp deleted file mode 100644 index 36446c4144a0a..0000000000000 --- a/control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// 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 TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ -#define TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport) -#define TRAJECTORY_FOLLOWER_LOCAL -#else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport) -#define TRAJECTORY_FOLLOWER_LOCAL -#endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#elif defined(__linux__) -#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) -#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) -#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ diff --git a/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml b/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml deleted file mode 100644 index 0c4c3faac73a9..0000000000000 --- a/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml deleted file mode 100644 index 36b4d0108de78..0000000000000 --- a/control/trajectory_follower_node/package.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - trajectory_follower_node - 1.0.0 - Nodes to follow a trajectory by generating control commands separated into lateral and longitudinal commands - - - Takamasa Horibe - - Takayuki Murooka - - Apache License 2.0 - - Takamasa Horibe - Maxime CLEMENT - Takayuki Murooka - - ament_cmake_auto - autoware_cmake - - autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs - motion_utils - mpc_lateral_controller - pid_longitudinal_controller - pure_pursuit - rclcpp - rclcpp_components - tier4_autoware_utils - trajectory_follower_base - vehicle_info_util - visualization_msgs - - ros2launch - - ament_cmake_ros - ament_index_python - ament_lint_auto - autoware_lint_common - autoware_testing - fake_test_node - ros_testing - - - ament_cmake - - diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp deleted file mode 100644 index 801f42ad9a470..0000000000000 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ /dev/null @@ -1,281 +0,0 @@ -// Copyright 2021 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 "trajectory_follower_node/controller_node.hpp" - -#include "mpc_lateral_controller/mpc_lateral_controller.hpp" -#include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" -#include "pure_pursuit/pure_pursuit_lateral_controller.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" - -#include -#include -#include -#include -#include -#include - -namespace autoware::motion::control::trajectory_follower_node -{ -Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("controller", node_options) -{ - using std::placeholders::_1; - - const double ctrl_period = declare_parameter("ctrl_period"); - timeout_thr_sec_ = declare_parameter("timeout_thr_sec"); - - const auto lateral_controller_mode = - getLateralControllerMode(declare_parameter("lateral_controller_mode")); - switch (lateral_controller_mode) { - case LateralControllerMode::MPC: { - lateral_controller_ = std::make_shared(*this); - break; - } - case LateralControllerMode::PURE_PURSUIT: { - lateral_controller_ = std::make_shared(*this); - break; - } - default: - throw std::domain_error("[LateralController] invalid algorithm"); - } - - const auto longitudinal_controller_mode = - getLongitudinalControllerMode(declare_parameter("longitudinal_controller_mode")); - switch (longitudinal_controller_mode) { - case LongitudinalControllerMode::PID: { - longitudinal_controller_ = - std::make_shared(*this); - break; - } - default: - throw std::domain_error("[LongitudinalController] invalid algorithm"); - } - - sub_ref_path_ = create_subscription( - "~/input/reference_trajectory", rclcpp::QoS{1}, std::bind(&Controller::onTrajectory, this, _1)); - sub_steering_ = create_subscription( - "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1)); - sub_odometry_ = create_subscription( - "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1)); - sub_accel_ = create_subscription( - "~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1)); - sub_operation_mode_ = create_subscription( - "~/input/current_operation_mode", rclcpp::QoS{1}, - [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; }); - control_cmd_pub_ = create_publisher( - "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); - pub_processing_time_lat_ms_ = - create_publisher("~/lateral/debug/processing_time_ms", 1); - pub_processing_time_lon_ms_ = - create_publisher("~/longitudinal/debug/processing_time_ms", 1); - debug_marker_pub_ = - create_publisher("~/output/debug_marker", rclcpp::QoS{1}); - - // Timer - { - const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(ctrl_period)); - timer_control_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); - } - - logger_configure_ = std::make_unique(this); - - published_time_publisher_ = std::make_unique(this); -} - -Controller::LateralControllerMode Controller::getLateralControllerMode( - const std::string & controller_mode) const -{ - if (controller_mode == "mpc") return LateralControllerMode::MPC; - if (controller_mode == "pure_pursuit") return LateralControllerMode::PURE_PURSUIT; - - return LateralControllerMode::INVALID; -} - -Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode( - const std::string & controller_mode) const -{ - if (controller_mode == "pid") return LongitudinalControllerMode::PID; - - return LongitudinalControllerMode::INVALID; -} - -void Controller::onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) -{ - current_trajectory_ptr_ = msg; -} - -void Controller::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) -{ - current_odometry_ptr_ = msg; -} - -void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) -{ - current_steering_ptr_ = msg; -} - -void Controller::onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) -{ - current_accel_ptr_ = msg; -} - -bool Controller::isTimeOut( - const trajectory_follower::LongitudinalOutput & lon_out, - const trajectory_follower::LateralOutput & lat_out) -{ - const auto now = this->now(); - if ((now - lat_out.control_cmd.stamp).seconds() > timeout_thr_sec_) { - RCLCPP_ERROR_THROTTLE( - get_logger(), *get_clock(), 5000 /*ms*/, - "Lateral control command too old, control_cmd will not be published."); - return true; - } - if ((now - lon_out.control_cmd.stamp).seconds() > timeout_thr_sec_) { - RCLCPP_ERROR_THROTTLE( - get_logger(), *get_clock(), 5000 /*ms*/, - "Longitudinal control command too old, control_cmd will not be published."); - return true; - } - return false; -} - -boost::optional Controller::createInputData( - rclcpp::Clock & clock) const -{ - if (!current_trajectory_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for trajectory."); - return {}; - } - - if (!current_odometry_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current odometry."); - return {}; - } - - if (!current_steering_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current steering."); - return {}; - } - - if (!current_accel_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current accel."); - return {}; - } - - if (!current_operation_mode_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current operation mode."); - return {}; - } - - trajectory_follower::InputData input_data; - input_data.current_trajectory = *current_trajectory_ptr_; - input_data.current_odometry = *current_odometry_ptr_; - input_data.current_steering = *current_steering_ptr_; - input_data.current_accel = *current_accel_ptr_; - input_data.current_operation_mode = *current_operation_mode_ptr_; - - return input_data; -} - -void Controller::callbackTimerControl() -{ - // 1. create input data - const auto input_data = createInputData(*get_clock()); - if (!input_data) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 5000, "Control is skipped since input data is not ready."); - return; - } - - // 2. check if controllers are ready - const bool is_lat_ready = lateral_controller_->isReady(*input_data); - const bool is_lon_ready = longitudinal_controller_->isReady(*input_data); - if (!is_lat_ready || !is_lon_ready) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 5000, - "Control is skipped since lateral and/or longitudinal controllers are not ready to run."); - return; - } - - // 3. run controllers - stop_watch_.tic("lateral"); - const auto lat_out = lateral_controller_->run(*input_data); - publishProcessingTime(stop_watch_.toc("lateral"), pub_processing_time_lat_ms_); - - stop_watch_.tic("longitudinal"); - const auto lon_out = longitudinal_controller_->run(*input_data); - publishProcessingTime(stop_watch_.toc("longitudinal"), pub_processing_time_lon_ms_); - - // 4. sync with each other controllers - longitudinal_controller_->sync(lat_out.sync_data); - lateral_controller_->sync(lon_out.sync_data); - - // TODO(Horibe): Think specification. This comes from the old implementation. - if (isTimeOut(lon_out, lat_out)) return; - - // 5. publish control command - autoware_auto_control_msgs::msg::AckermannControlCommand out; - out.stamp = this->now(); - out.lateral = lat_out.control_cmd; - out.longitudinal = lon_out.control_cmd; - control_cmd_pub_->publish(out); - - // 6. publish debug - published_time_publisher_->publish_if_subscribed(control_cmd_pub_, out.stamp); - publishDebugMarker(*input_data, lat_out); -} - -void Controller::publishDebugMarker( - const trajectory_follower::InputData & input_data, - const trajectory_follower::LateralOutput & lat_out) const -{ - visualization_msgs::msg::MarkerArray debug_marker_array{}; - - // steer converged marker - { - auto marker = tier4_autoware_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)); - marker.pose = input_data.current_odometry.pose.pose; - - std::stringstream ss; - const double current = input_data.current_steering.steering_tire_angle; - const double cmd = lat_out.control_cmd.steering_tire_angle; - const double diff = current - cmd; - ss << "current:" << current << " cmd:" << cmd << " diff:" << diff - << (lat_out.sync_data.is_steer_converged ? " (converged)" : " (not converged)"); - marker.text = ss.str(); - - debug_marker_array.markers.push_back(marker); - } - - debug_marker_pub_->publish(debug_marker_array); -} - -void Controller::publishProcessingTime( - const double t_ms, const rclcpp::Publisher::SharedPtr pub) -{ - Float64Stamped msg{}; - msg.stamp = this->now(); - msg.data = t_ms; - pub->publish(msg); -} - -} // namespace autoware::motion::control::trajectory_follower_node - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::trajectory_follower_node::Controller) diff --git a/control/vehicle_cmd_gate/CMakeLists.txt b/control/vehicle_cmd_gate/CMakeLists.txt deleted file mode 100644 index c5db663ccc77e..0000000000000 --- a/control/vehicle_cmd_gate/CMakeLists.txt +++ /dev/null @@ -1,55 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(vehicle_cmd_gate) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(vehicle_cmd_gate_node SHARED - src/vehicle_cmd_gate.cpp - src/vehicle_cmd_filter.cpp - src/adapi_pause_interface.cpp - src/moderate_stop_interface.cpp -) - -rclcpp_components_register_node(vehicle_cmd_gate_node - PLUGIN "vehicle_cmd_gate::VehicleCmdGate" - EXECUTABLE vehicle_cmd_gate_exe -) - -rosidl_generate_interfaces( - ${PROJECT_NAME} - "msg/IsFilterActivated.msg" - DEPENDENCIES builtin_interfaces -) - -# to use same package defined message -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(vehicle_cmd_gate_node - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(vehicle_cmd_gate_node "${cpp_typesupport_target}") -endif() - - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_vehicle_cmd_gate - test/src/test_main.cpp - test/src/test_vehicle_cmd_filter.cpp - test/src/test_filter_in_vehicle_cmd_gate_node.cpp - ) - ament_target_dependencies(test_vehicle_cmd_gate - rclcpp - tier4_control_msgs - ) - target_link_libraries(test_vehicle_cmd_gate - vehicle_cmd_gate_node - ) -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md deleted file mode 100644 index 6ec2da84a7b6c..0000000000000 --- a/control/vehicle_cmd_gate/README.md +++ /dev/null @@ -1,85 +0,0 @@ -# vehicle_cmd_gate - -## Purpose - -`vehicle_cmd_gate` is the package to get information from emergency handler, planning module, external controller, and send a msg to vehicle. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ------------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------------------- | -| `~/input/steering` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | steering status | -| `~/input/auto/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from planning module | -| `~/input/auto/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from planning module | -| `~/input/auto/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from planning module | -| `~/input/auto/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from planning module | -| `~/input/external/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from external | -| `~/input/external/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from external | -| `~/input/external/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from external | -| `~/input/external/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from external | -| `~/input/external_emergency_stop_heartbeat` | `tier4_external_api_msgs::msg::Heartbeat` | heartbeat | -| `~/input/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | -| `~/input/emergency/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from emergency handler | -| `~/input/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | -| `~/input/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | -| `~/input/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | -| `~/input/operation_mode` | `autoware_adapi_v1_msgs::msg::OperationModeState` | operation mode of Autoware | - -### Output - -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------- | -| `~/output/vehicle_cmd_emergency` | `autoware_auto_system_msgs::msg::EmergencyState` | emergency state which was originally in vehicle command | -| `~/output/command/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity to vehicle | -| `~/output/command/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command to vehicle | -| `~/output/command/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command to vehicle | -| `~/output/command/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command to vehicle | -| `~/output/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | -| `~/output/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | -| `~/output/external_emergency` | `tier4_external_api_msgs::msg::Emergency` | external emergency signal | -| `~/output/operation_mode` | `tier4_system_msgs::msg::OperationMode` | current operation mode of the vehicle_cmd_gate | - -## Parameters - -| Parameter | Type | Description | -| ------------------------------------------- | -------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `update_period` | double | update period | -| `use_emergency_handling` | bool | true when emergency handler is used | -| `check_external_emergency_heartbeat` | bool | true when checking heartbeat for emergency stop | -| `system_emergency_heartbeat_timeout` | double | timeout for system emergency | -| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency | -| `filter_activated_count_threshold` | int | threshold for filter activation | -| `filter_activated_velocity_threshold` | double | velocity threshold for filter activation | -| `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop | -| `emergency_acceleration` | double | longitudinal acceleration cmd when vehicle stop with emergency | -| `moderate_stop_service_acceleration` | double | longitudinal acceleration cmd when vehicle stop with moderate stop service | -| `nominal.vel_lim` | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode) | -| `nominal.reference_speed_point` | | velocity point used as a reference when calculate control command limit (activated in AUTONOMOUS operation mode). The size of this array must be equivalent to the size of the limit array. | -| `nominal.lon_acc_lim` | | array of limits of longitudinal acceleration (activated in AUTONOMOUS operation mode) | -| `nominal.lon_jerk_lim` | | array of limits of longitudinal jerk (activated in AUTONOMOUS operation mode) | -| `nominal.lat_acc_lim` | | array of limits of lateral acceleration (activated in AUTONOMOUS operation mode) | -| `nominal.lat_jerk_lim` | | array of limits of lateral jerk (activated in AUTONOMOUS operation mode) | -| `on_transition.vel_lim` | double | limit of longitudinal velocity (activated in TRANSITION operation mode) | -| `on_transition.reference_speed_point` | | velocity point used as a reference when calculate control command limit (activated in TRANSITION operation mode). The size of this array must be equivalent to the size of the limit array. | -| `on_transition.lon_acc_lim` | | array of limits of longitudinal acceleration (activated in TRANSITION operation mode) | -| `on_transition.lon_jerk_lim` | | array of limits of longitudinal jerk (activated in TRANSITION operation mode) | -| `on_transition.lat_acc_lim` | | array of limits of lateral acceleration (activated in TRANSITION operation mode) | -| `on_transition.lat_jerk_lim` | | array of limits of lateral jerk (activated in TRANSITION operation mode) | - -## Filter function - -This module incorporates a limitation filter to the control command right before its published. Primarily for safety, this filter restricts the output range of all control commands published through Autoware. - -The limitation values are calculated based on the 1D interpolation of the limitation array parameters. Here is an example for the longitudinal jerk limit. - -![filter-example](./image/filter.png) - -Notation: this filter is not designed to enhance ride comfort. Its main purpose is to detect and remove abnormal values in the control outputs during the final stages of Autoware. If this filter is frequently active, it implies the control module may need tuning. If you're aiming to smoothen the signal via a low-pass filter or similar techniques, that should be handled in the control module. When the filter is activated, the topic `~/is_filter_activated` is published. - -## Assumptions / Known limits - -The parameter `check_external_emergency_heartbeat` (true by default) enables an emergency stop request from external modules. -This feature requires a `~/input/external_emergency_stop_heartbeat` topic for health monitoring of the external module, and the vehicle_cmd_gate module will not start without the topic. -The `check_external_emergency_heartbeat` parameter must be false when the "external emergency stop" function is not used. diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml deleted file mode 100644 index 22ae9da6d222e..0000000000000 --- a/control/vehicle_cmd_gate/package.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - vehicle_cmd_gate - 0.1.0 - The vehicle_cmd_gate package - Takamasa Horibe - Tomoya Kimura - Apache License 2.0 - - Hiroki OTA - - ament_cmake - autoware_cmake - - rosidl_default_generators - - autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_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 - tier4_system_msgs - tier4_vehicle_msgs - vehicle_info_util - visualization_msgs - - rosidl_default_runtime - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/control/vehicle_cmd_gate/src/marker_helper.hpp b/control/vehicle_cmd_gate/src/marker_helper.hpp deleted file mode 100644 index 44c43a3630151..0000000000000 --- a/control/vehicle_cmd_gate/src/marker_helper.hpp +++ /dev/null @@ -1,119 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MARKER_HELPER_HPP_ -#define MARKER_HELPER_HPP_ - -#include - -#include - -#include - -namespace vehicle_cmd_gate -{ -inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) -{ - geometry_msgs::msg::Point point; - - point.x = x; - point.y = y; - point.z = z; - - return point; -} - -inline geometry_msgs::msg::Quaternion createMarkerOrientation( - double x, double y, double z, double w) -{ - geometry_msgs::msg::Quaternion quaternion; - - quaternion.x = x; - quaternion.y = y; - quaternion.z = z; - quaternion.w = w; - - return quaternion; -} - -inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z) -{ - geometry_msgs::msg::Vector3 scale; - - scale.x = x; - scale.y = y; - scale.z = z; - - return scale; -} - -inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a) -{ - std_msgs::msg::ColorRGBA color; - - color.r = r; - color.g = g; - color.b = b; - color.a = a; - - return color; -} - -inline visualization_msgs::msg::Marker createMarker( - const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type, - geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale, - const std_msgs::msg::ColorRGBA & color) -{ - visualization_msgs::msg::Marker marker; - - marker.header.frame_id = frame_id; - marker.header.stamp = rclcpp::Time(0); - marker.ns = ns; - marker.id = id; - marker.type = type; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration::from_seconds(0.2); - marker.pose.position = point; - marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); - marker.scale = scale; - marker.color = color; - marker.frame_locked = false; - - return marker; -} - -inline visualization_msgs::msg::Marker createStringMarker( - const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type, - geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale, - const std_msgs::msg::ColorRGBA & color, const std::string text) -{ - visualization_msgs::msg::Marker marker; - - marker = createMarker(frame_id, ns, id, type, point, scale, color); - marker.text = text; - - return marker; -} - -inline void appendMarkerArray( - const visualization_msgs::msg::MarkerArray & additional_marker_array, - visualization_msgs::msg::MarkerArray * marker_array) -{ - for (const auto & marker : additional_marker_array.markers) { - marker_array->markers.push_back(marker); - } -} -} // namespace vehicle_cmd_gate - -#endif // MARKER_HELPER_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp deleted file mode 100644 index e71fb405beda1..0000000000000 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 VEHICLE_CMD_FILTER_HPP_ -#define VEHICLE_CMD_FILTER_HPP_ - -#include -#include - -#include - -#include - -namespace vehicle_cmd_gate -{ -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using vehicle_cmd_gate::msg::IsFilterActivated; -using LimitArray = std::vector; - -struct VehicleCmdFilterParam -{ - double wheel_base; - double vel_lim; - LimitArray reference_speed_points; - LimitArray lon_acc_lim; - LimitArray lon_jerk_lim; - LimitArray lat_acc_lim; - LimitArray lat_jerk_lim; - LimitArray steer_lim; - LimitArray steer_rate_lim; - LimitArray actual_steer_diff_lim; -}; -class VehicleCmdFilter -{ -public: - VehicleCmdFilter(); - ~VehicleCmdFilter() = default; - - void setWheelBase(double v) { param_.wheel_base = v; } - void setVelLim(double v) { param_.vel_lim = v; } - void setSteerLim(LimitArray v); - void setSteerRateLim(LimitArray v); - void setLonAccLim(LimitArray v); - void setLonJerkLim(LimitArray v); - void setLatAccLim(LimitArray v); - void setLatJerkLim(LimitArray v); - void setActualSteerDiffLim(LimitArray v); - void setCurrentSpeed(double v) { current_speed_ = v; } - void setParam(const VehicleCmdFilterParam & p); - VehicleCmdFilterParam getParam() const; - void setPrevCmd(const AckermannControlCommand & v) { prev_cmd_ = v; } - - void limitLongitudinalWithVel(AckermannControlCommand & input) const; - void limitLongitudinalWithAcc(const double dt, AckermannControlCommand & input) const; - void limitLongitudinalWithJerk(const double dt, AckermannControlCommand & input) const; - void limitLateralWithLatAcc(const double dt, AckermannControlCommand & input) const; - void limitLateralWithLatJerk(const double dt, AckermannControlCommand & input) const; - void limitActualSteerDiff( - const double current_steer_angle, AckermannControlCommand & input) const; - void limitLateralSteer(AckermannControlCommand & input) const; - void limitLateralSteerRate(const double dt, AckermannControlCommand & input) const; - void filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & input, - IsFilterActivated & is_activated) const; - static IsFilterActivated checkIsActivated( - const AckermannControlCommand & c1, const AckermannControlCommand & c2, - const double tol = 1.0e-3); - - AckermannControlCommand getPrevCmd() { return prev_cmd_; } - -private: - VehicleCmdFilterParam param_; - AckermannControlCommand prev_cmd_; - double current_speed_ = 0.0; - - bool setParameterWithValidation(const VehicleCmdFilterParam & p); - - double calcLatAcc(const AckermannControlCommand & cmd) const; - double calcLatAcc(const AckermannControlCommand & cmd, const double v) const; - double calcSteerFromLatacc(const double v, const double latacc) const; - double limitDiff(const double curr, const double prev, const double diff_lim) const; - - double interpolateFromSpeed(const LimitArray & limits) const; - double getLonAccLim() const; - double getLonJerkLim() const; - double getLatAccLim() const; - double getLatJerkLim() const; - double getSteerLim() const; - double getSteerRateLim() const; - double getSteerDiffLim() const; -}; -} // namespace vehicle_cmd_gate - -#endif // VEHICLE_CMD_FILTER_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp deleted file mode 100644 index 85bc183361b94..0000000000000 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ /dev/null @@ -1,261 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 VEHICLE_CMD_GATE_HPP_ -#define VEHICLE_CMD_GATE_HPP_ - -#include "adapi_pause_interface.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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace vehicle_cmd_gate -{ - -using autoware_adapi_v1_msgs::msg::MrmState; -using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_control_msgs::msg::LongitudinalCommand; -using autoware_auto_vehicle_msgs::msg::GearCommand; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; -using geometry_msgs::msg::AccelWithCovarianceStamped; -using std_srvs::srv::Trigger; -using tier4_control_msgs::msg::GateMode; -using tier4_debug_msgs::msg::BoolStamped; -using tier4_external_api_msgs::msg::Emergency; -using tier4_external_api_msgs::msg::Heartbeat; -using tier4_external_api_msgs::srv::SetEmergency; -using tier4_system_msgs::msg::MrmBehaviorStatus; -using tier4_vehicle_msgs::msg::VehicleEmergencyStamped; -using vehicle_cmd_gate::msg::IsFilterActivated; -using visualization_msgs::msg::MarkerArray; - -using diagnostic_msgs::msg::DiagnosticStatus; -using nav_msgs::msg::Odometry; - -using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; -using EngageSrv = tier4_external_api_msgs::srv::Engage; - -using motion_utils::VehicleStopChecker; -struct Commands -{ - AckermannControlCommand control; - TurnIndicatorsCommand turn_indicator; - HazardLightsCommand hazard_light; - GearCommand gear; - explicit Commands(const uint8_t & default_gear = GearCommand::PARK) - { - gear.command = default_gear; - } -}; - -class VehicleCmdGate : public rclcpp::Node -{ -public: - explicit VehicleCmdGate(const rclcpp::NodeOptions & node_options); - -private: - // Publisher - rclcpp::Publisher::SharedPtr vehicle_cmd_emergency_pub_; - rclcpp::Publisher::SharedPtr control_cmd_pub_; - rclcpp::Publisher::SharedPtr gear_cmd_pub_; - rclcpp::Publisher::SharedPtr turn_indicator_cmd_pub_; - rclcpp::Publisher::SharedPtr hazard_light_cmd_pub_; - rclcpp::Publisher::SharedPtr gate_mode_pub_; - rclcpp::Publisher::SharedPtr engage_pub_; - rclcpp::Publisher::SharedPtr operation_mode_pub_; - rclcpp::Publisher::SharedPtr is_filter_activated_pub_; - rclcpp::Publisher::SharedPtr filter_activated_marker_pub_; - rclcpp::Publisher::SharedPtr filter_activated_marker_raw_pub_; - rclcpp::Publisher::SharedPtr filter_activated_flag_pub_; - // Parameter callback - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - rcl_interfaces::msg::SetParametersResult onParameter( - const std::vector & parameters); - // Subscription - rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; - 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(MrmState::ConstSharedPtr msg); - - bool is_engaged_; - bool is_system_emergency_ = false; - bool is_external_emergency_stop_ = false; - bool is_filtered_marker_published_ = false; - double current_steer_ = 0; - GateMode current_gate_mode_; - MrmState current_mrm_state_; - Odometry current_kinematics_; - double current_acceleration_ = 0.0; - int filter_activated_count_ = 0; - - // Heartbeat - std::shared_ptr emergency_state_heartbeat_received_time_; - bool is_emergency_state_heartbeat_timeout_ = false; - std::shared_ptr external_emergency_stop_heartbeat_received_time_; - bool is_external_emergency_stop_heartbeat_timeout_ = false; - bool isHeartbeatTimeout( - const std::shared_ptr & heartbeat_received_time, const double timeout); - - // Check initialization - bool isDataReady(); - - // Subscriber for auto - Commands auto_commands_; - rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; - rclcpp::Subscription::SharedPtr auto_turn_indicator_cmd_sub_; - rclcpp::Subscription::SharedPtr auto_hazard_light_cmd_sub_; - rclcpp::Subscription::SharedPtr auto_gear_cmd_sub_; - void onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); - - // Subscription for external - Commands remote_commands_; - rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; - rclcpp::Subscription::SharedPtr remote_turn_indicator_cmd_sub_; - rclcpp::Subscription::SharedPtr remote_hazard_light_cmd_sub_; - rclcpp::Subscription::SharedPtr remote_gear_cmd_sub_; - void onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); - - // Subscription for emergency - Commands emergency_commands_; - rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; - rclcpp::Subscription::SharedPtr emergency_hazard_light_cmd_sub_; - rclcpp::Subscription::SharedPtr emergency_gear_cmd_sub_; - void onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); - - // Parameter - bool use_emergency_handling_; - bool check_external_emergency_heartbeat_; - double system_emergency_heartbeat_timeout_; - double external_emergency_stop_heartbeat_timeout_; - double stop_hold_acceleration_; - double emergency_acceleration_; - double moderate_stop_service_acceleration_; - bool enable_cmd_limit_filter_; - int filter_activated_count_threshold_; - double filter_activated_velocity_threshold_; - - // Service - rclcpp::Service::SharedPtr srv_engage_; - rclcpp::Service::SharedPtr srv_external_emergency_; - rclcpp::Publisher::SharedPtr pub_external_emergency_; - void onEngageService( - const EngageSrv::Request::SharedPtr request, const EngageSrv::Response::SharedPtr response); - void onExternalEmergencyStopService( - const std::shared_ptr request_header, - const SetEmergency::Request::SharedPtr request, - const SetEmergency::Response::SharedPtr response); - - // TODO(Takagi, Isamu): deprecated - rclcpp::Subscription::SharedPtr engage_sub_; - rclcpp::Service::SharedPtr srv_external_emergency_stop_; - rclcpp::Service::SharedPtr srv_clear_external_emergency_stop_; - void onEngage(EngageMsg::ConstSharedPtr msg); - bool onSetExternalEmergencyStopService( - const std::shared_ptr req_header, const Trigger::Request::SharedPtr req, - const Trigger::Response::SharedPtr res); - bool onClearExternalEmergencyStopService( - const std::shared_ptr req_header, const Trigger::Request::SharedPtr req, - const Trigger::Response::SharedPtr res); - - // Timer / Event - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::TimerBase::SharedPtr timer_pub_status_; - - void onTimer(); - void publishControlCommands(const Commands & input_msg); - void publishEmergencyStopControlCommands(); - void publishStatus(); - - // Diagnostics Updater - diagnostic_updater::Updater updater_; - - void checkExternalEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat); - - // Algorithm - AckermannControlCommand prev_control_cmd_; - AckermannControlCommand createStopControlCmd() const; - LongitudinalCommand createLongitudinalStopControlCmd() const; - AckermannControlCommand createEmergencyStopControlCmd() const; - - std::shared_ptr prev_time_; - double getDt(); - AckermannControlCommand getActualStatusAsCommand(); - - VehicleCmdFilter filter_; - AckermannControlCommand filterControlCommand(const AckermannControlCommand & msg); - - // filtering on transition - OperationModeState current_operation_mode_; - VehicleCmdFilter filter_on_transition_; - - // Pause interface for API - std::unique_ptr adapi_pause_; - std::unique_ptr moderate_stop_interface_; - - // stop checker - std::unique_ptr vehicle_stop_checker_; - double stop_check_duration_; - - // debug - MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); - void publishMarkers(const IsFilterActivated & filter_activated); - - std::unique_ptr logger_configure_; - - std::unique_ptr published_time_publisher_; -}; - -} // namespace vehicle_cmd_gate -#endif // VEHICLE_CMD_GATE_HPP_ diff --git a/evaluator/autoware_control_evaluator/CMakeLists.txt b/evaluator/autoware_control_evaluator/CMakeLists.txt new file mode 100644 index 0000000000000..189745349a592 --- /dev/null +++ b/evaluator/autoware_control_evaluator/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_control_evaluator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(pluginlib REQUIRED) + +ament_auto_add_library(control_evaluator_node SHARED + src/control_evaluator_node.cpp + src/metrics/deviation_metrics.cpp +) + +rclcpp_components_register_node(control_evaluator_node + PLUGIN "control_diagnostics::controlEvaluatorNode" + EXECUTABLE control_evaluator +) + + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) 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/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml new file mode 100644 index 0000000000000..49631c566eaf5 --- /dev/null +++ b/evaluator/autoware_control_evaluator/package.xml @@ -0,0 +1,33 @@ + + + + autoware_control_evaluator + 0.1.0 + ROS 2 node for evaluating control + Daniel SANCHEZ + takayuki MUROOKA + Apache License 2.0 + + Daniel SANCHEZ + takayuki MUROOKA + + 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 + autoware_lint_common + + + ament_cmake + + 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/autoware_planning_evaluator/README.md b/evaluator/autoware_planning_evaluator/README.md new file mode 100644 index 0000000000000..eeeba99da9b8b --- /dev/null +++ b/evaluator/autoware_planning_evaluator/README.md @@ -0,0 +1,96 @@ +# Planning Evaluator + +## Purpose + +This package provides nodes that generate metrics to evaluate the quality of planning and control. + +## Inner-workings / Algorithms + +The evaluation node calculates metrics each time it receives a trajectory `T(0)`. +Metrics are calculated using the following information: + +- the trajectory `T(0)` itself. +- the previous trajectory `T(-1)`. +- the _reference_ trajectory assumed to be used as the reference to plan `T(0)`. +- the current ego pose. +- the set of objects in the environment. + +These information are maintained by an instance of class `MetricsCalculator` +which is also responsible for calculating metrics. + +### Stat + +Each metric is calculated using a `Stat` instance which contains +the minimum, maximum, and mean values calculated for the metric +as well as the number of values measured. + +### Metric calculation and adding more metrics + +All possible metrics are defined in the `Metric` enumeration defined +`include/planning_evaluator/metrics/metric.hpp`. +This file also defines conversions from/to string as well as human readable descriptions +to be used as header of the output file. + +The `MetricsCalculator` is responsible for calculating metric statistics +through calls to function: + +```C++ +Stat MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const; +``` + +Adding a new metric `M` requires the following steps: + +- `metrics/metric.hpp`: add `M` to the `enum`, to the from/to string conversion maps, and to the description map. +- `metrics_calculator.cpp`: add `M` to the `switch/case` statement of the `calculate` function. +- Add `M` to the `selected_metrics` parameters. + +## Inputs / Outputs + +### Inputs + +| Name | Type | Description | +| ------------------------------ | ------------------------------------------------- | ------------------------------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | Main trajectory to evaluate | +| `~/input/reference_trajectory` | `autoware_planning_msgs::msg::Trajectory` | Reference trajectory to use for deviation metrics | +| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Obstacles | + +### Outputs + +Each metric is published on a topic named after the metric name. + +| Name | Type | Description | +| ----------- | --------------------------------------- | ------------------------------------------------------- | +| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | DiagnosticArray with a DiagnosticStatus for each metric | + +When shut down, the evaluation node writes the values of the metrics measured during its lifetime +to a file as specified by the `output_file` parameter. + +## Parameters + +| Name | Type | Description | +| :-------------------------------- | :------- | :-------------------------------------------------------------------------- | +| `output_file` | `string` | file used to write metrics | +| `ego_frame` | `string` | frame used for the ego pose | +| `selected_metrics` | List | metrics to measure and publish | +| `trajectory.min_point_dist_m` | `double` | minimum distance between two successive points to use for angle calculation | +| `trajectory.lookahead.max_dist_m` | `double` | maximum distance from ego along the trajectory to use for calculation | +| `trajectory.lookahead.max_time_m` | `double` | maximum time ahead of ego along the trajectory to use for calculation | +| `obstacle.dist_thr_m` | `double` | distance between ego and the obstacle below which a collision is considered | + +## Assumptions / Known limits + +There is a strong assumption that when receiving a trajectory `T(0)`, +it has been generated using the last received reference trajectory and objects. +This can be wrong if a new reference trajectory or objects are published while `T(0)` is being calculated. + +Precision is currently limited by the resolution of the trajectories. +It is possible to interpolate the trajectory and reference trajectory to increase precision but would make computation significantly more expensive. + +## Future extensions / Unimplemented parts + +- Use `Route` or `Path` messages as reference trajectory. +- RSS metrics (done in another node ). +- Add option to publish the `min` and `max` metric values. For now only the `mean` value is published. +- `motion_evaluator_node`. + - Node which constructs a trajectory over time from the real motion of ego. + - Only a proof of concept is currently implemented. diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp new file mode 100644 index 0000000000000..8d7cfbd30d604 --- /dev/null +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp @@ -0,0 +1,83 @@ +// Copyright 2021 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__PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ + +#include "autoware/planning_evaluator/stat.hpp" + +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +namespace planning_diagnostics +{ +namespace metrics +{ +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +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] traj input trajectory + * @return calculated statistics + */ +Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj); + +/** + * @brief calculate yaw deviation of the given trajectory from the reference trajectory + * @param [in] ref reference trajectory + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj); + +/** + * @brief calculate velocity deviation of the given trajectory from the reference trajectory + * @param [in] ref reference trajectory + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj); + +/** + * @brief calculate longitudinal deviation of the given ego pose from the modified goal pose + * @param [in] base_pose base pose + * @param [in] target_point target point + * @return calculated statistics + */ +Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point); + +/** + * @brief calculate lateral deviation of the given ego pose from the modified goal pose + * @param [in] base_pose base pose + * @param [in] target_point target point + * @return calculated statistics + */ +Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point); + +/** + * @brief calculate yaw deviation of the given ego pose from the modified goal pose + * @param [in] base_pose base pose + * @param [in] target_pose target pose + * @return calculated statistics + */ +Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose); + +} // namespace metrics +} // namespace planning_diagnostics + +#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/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp new file mode 100644 index 0000000000000..2ade316ba7ad5 --- /dev/null +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp @@ -0,0 +1,43 @@ +// Copyright 2021 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__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ + +#include "autoware/planning_evaluator/stat.hpp" + +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +namespace planning_diagnostics +{ +namespace metrics +{ +namespace utils +{ +using autoware_planning_msgs::msg::Trajectory; + +/** + * @brief find the index in the trajectory at the given distance of the given index + * @param [in] traj input trajectory + * @param [in] curr_id index + * @param [in] distance distance + * @return index of the trajectory point at distance ahead of traj[curr_id] + */ +size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance); + +} // namespace utils +} // namespace metrics +} // namespace planning_diagnostics +#endif // AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp new file mode 100644 index 0000000000000..c4d468b4dacd5 --- /dev/null +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp @@ -0,0 +1,51 @@ +// Copyright 2021 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__PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ + +#include "autoware/planning_evaluator/stat.hpp" + +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" + +namespace planning_diagnostics +{ +namespace metrics +{ +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; + +/** + * @brief calculate the distance to the closest obstacle at each point of the trajectory + * @param [in] obstacles obstacles + * @param [in] traj trajectory + * @return calculated statistics + */ +Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj); + +/** + * @brief calculate the time to collision of the trajectory with the given obstacles + * Assume that "now" corresponds to the first trajectory point + * @param [in] obstacles obstacles + * @param [in] traj trajectory + * @return calculated statistics + */ +Stat calcTimeToCollision( + const PredictedObjects & obstacles, const Trajectory & traj, const double distance_threshold); + +} // namespace metrics +} // namespace planning_diagnostics + +#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 77% 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 c4bf1fe901604..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,18 +12,18 @@ // 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_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; /** * @brief calculate the discrete Frechet distance between two trajectories @@ -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 83% 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 b55ad245d8425..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,20 +12,20 @@ // 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_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; /** * @brief calculate relative angle metric (angle between successive points) @@ -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 81% 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 851678e55d755..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,26 +12,26 @@ // 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" - -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.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" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" #include namespace planning_diagnostics { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -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 77% 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 28a4b2cba8365..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,17 +12,17 @@ // 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" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include @@ -33,8 +33,8 @@ namespace planning_diagnostics { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; /** * @brief Node for planning evaluation @@ -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/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp new file mode 100644 index 0000000000000..cd894fecc2331 --- /dev/null +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp @@ -0,0 +1,49 @@ +// Copyright 2021 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__PLANNING_EVALUATOR__PARAMETERS_HPP_ +#define AUTOWARE__PLANNING_EVALUATOR__PARAMETERS_HPP_ + +#include "autoware/planning_evaluator/metrics/metric.hpp" + +#include + +namespace planning_diagnostics +{ +/** + * @brief Enumeration of trajectory metrics + */ +struct Parameters +{ + std::array(Metric::SIZE)> metrics{}; // default values to false + + struct + { + double min_point_dist_m = 0.1; + struct + { + double max_dist_m = 5.0; + double max_time_s = 3.0; + } lookahead; + } trajectory; + + struct + { + double dist_thr_m = 1.0; + } obstacle; +}; // struct Parameters + +} // namespace planning_diagnostics + +#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 84% 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 888eea855295c..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,19 +12,19 @@ // 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" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -36,14 +36,13 @@ namespace planning_diagnostics { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; - /** * @brief Node for planning evaluation */ @@ -120,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/autoware_planning_evaluator/launch/motion_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/motion_evaluator.launch.xml new file mode 100644 index 0000000000000..6558d0b568c94 --- /dev/null +++ b/evaluator/autoware_planning_evaluator/launch/motion_evaluator.launch.xml @@ -0,0 +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 82% rename from evaluator/planning_evaluator/launch/planning_evaluator.launch.xml rename to evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml index 1b7510c2ced69..2e358f6272379 100644 --- a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -1,14 +1,14 @@ - + - - + + diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml new file mode 100644 index 0000000000000..5bd500f200eac --- /dev/null +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -0,0 +1,37 @@ + + + + autoware_planning_evaluator + 0.1.0 + ROS 2 node for evaluating planners + Maxime CLEMENT + Kyoichi Sugahara + Apache License 2.0 + + Maxime CLEMENT + + ament_cmake_auto + autoware_cmake + + autoware_motion_utils + autoware_perception_msgs + autoware_planning_msgs + autoware_universe_utils + diagnostic_msgs + eigen + geometry_msgs + nav_msgs + pluginlib + rclcpp + rclcpp_components + tf2 + tf2_ros + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp new file mode 100644 index 0000000000000..8ce8a009652d8 --- /dev/null +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -0,0 +1,105 @@ +// Copyright 2021 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/planning_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 planning_diagnostics +{ +namespace metrics +{ +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj) +{ + Stat stat; + + if (ref.points.empty() || traj.points.empty()) { + return stat; + } + + /** TODO(Maxime CLEMENT): + * need more precise calculation, e.g., lateral distance from spline of the reference traj + */ + for (TrajectoryPoint p : traj.points) { + 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; +} + +Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) +{ + Stat stat; + + if (ref.points.empty() || traj.points.empty()) { + return stat; + } + + /** TODO(Maxime CLEMENT): + * need more precise calculation, e.g., yaw distance from spline of the reference traj + */ + for (TrajectoryPoint p : traj.points) { + 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; +} + +Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj) +{ + Stat stat; + + if (ref.points.empty() || traj.points.empty()) { + return stat; + } + + // TODO(Maxime CLEMENT) need more precise calculation + for (TrajectoryPoint p : traj.points) { + 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; +} + +Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) +{ + Stat stat; + 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(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(autoware::universe_utils::calcYawDeviation(base_pose, target_pose))); + return stat; +} +} // namespace metrics +} // namespace planning_diagnostics diff --git a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp similarity index 82% rename from evaluator/planning_evaluator/src/metrics/metrics_utils.cpp rename to evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp index 6134a100f31a4..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_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::calcDistance2d; +using autoware::universe_utils::calcDistance2d; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; 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 89% rename from evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp rename to evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp index 82d6dcc51097e..3cdaf3d7eaaae 100644 --- a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -12,13 +12,13 @@ // 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 -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include @@ -27,8 +27,8 @@ namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using tier4_autoware_utils::calcDistance2d; +using autoware::universe_utils::calcDistance2d; +using autoware_planning_msgs::msg::TrajectoryPoint; Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp new file mode 100644 index 0000000000000..4d1c02faa406f --- /dev/null +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -0,0 +1,100 @@ +// Copyright 2021 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/planning_evaluator/metrics/stability_metrics.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include + +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +#include + +namespace planning_diagnostics +{ +namespace metrics +{ +using autoware_planning_msgs::msg::TrajectoryPoint; + +Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2) +{ + Stat stat; + + if (traj1.points.empty() || traj2.points.empty()) { + return stat; + } + + Eigen::MatrixXd ca = Eigen::MatrixXd::Zero(traj1.points.size(), traj2.points.size()); + + for (size_t i = 0; i < traj1.points.size(); ++i) { + for (size_t j = 0; j < traj2.points.size(); ++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*/) { + ca(i, j) = std::max(ca(i - 1, 0), dist); + } else if (j > 0 /*&& i == 0*/) { + ca(i, j) = std::max(ca(0, j - 1), dist); + } else { /* i == j == 0 */ + ca(i, j) = dist; + } + } + } + stat.add(ca(traj1.points.size() - 1, traj2.points.size() - 1)); + return stat; +} + +Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2) +{ + Stat stat; + if (traj1.points.empty()) { + return stat; + } + for (const auto & point : traj2.points) { + const auto p0 = autoware::universe_utils::getPoint(point); + // find nearest segment + 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 && + 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 = autoware::universe_utils::calcDistance2d(traj1.points.back(), p0); + } else if ( // NOLINT + nearest_segment_idx == 0 && autoware::motion_utils::calcLongitudinalOffsetToSegment( + traj1.points, nearest_segment_idx, p0) <= 0) { + // distance to first point + dist = autoware::universe_utils::calcDistance2d(traj1.points.front(), p0); + } else { + // orthogonal distance + 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)); + } + stat.add(dist); + } + return stat; +} + +} // namespace metrics +} // namespace planning_diagnostics 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 96% rename from evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp rename to evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp index 4f2ab014a79d6..14f8338acfed4 100644 --- a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/test/test_planning_evaluator_node.cpp @@ -18,10 +18,10 @@ #include "rclcpp/time.hpp" #include "tf2_ros/transform_broadcaster.h" -#include +#include -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -33,9 +33,9 @@ #include using EvalNode = planning_diagnostics::PlanningEvaluatorNode; -using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; -using Objects = autoware_auto_perception_msgs::msg::PredictedObjects; +using Trajectory = autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; +using Objects = autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using diagnostic_msgs::msg::DiagnosticArray; using nav_msgs::msg::Odometry; @@ -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"}); @@ -409,7 +410,7 @@ TEST_F(EvalTest, TestObstacleDistance) { setTargetMetric(planning_diagnostics::Metric::obstacle_distance); Objects objs; - autoware_auto_perception_msgs::msg::PredictedObject obj; + autoware_perception_msgs::msg::PredictedObject obj; obj.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; obj.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; objs.objects.push_back(obj); @@ -425,7 +426,7 @@ TEST_F(EvalTest, TestObstacleTTC) { setTargetMetric(planning_diagnostics::Metric::obstacle_ttc); Objects objs; - autoware_auto_perception_msgs::msg::PredictedObject obj; + autoware_perception_msgs::msg::PredictedObject obj; obj.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; obj.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; objs.objects.push_back(obj); diff --git a/evaluator/control_evaluator/CMakeLists.txt b/evaluator/control_evaluator/CMakeLists.txt deleted file mode 100644 index b97a14e29cdcd..0000000000000 --- a/evaluator/control_evaluator/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(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 -) - -rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "control_diagnostics::controlEvaluatorNode" - EXECUTABLE ${PROJECT_NAME} -) - - -ament_auto_package( - INSTALL_TO_SHARE - param - launch -) 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/package.xml b/evaluator/control_evaluator/package.xml deleted file mode 100644 index 10686e9b73761..0000000000000 --- a/evaluator/control_evaluator/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - control_evaluator - 0.1.0 - ROS 2 node for evaluating control - Daniel SANCHEZ - takayuki MUROOKA - Apache License 2.0 - - Daniel SANCHEZ - takayuki MUROOKA - - ament_cmake_auto - autoware_cmake - - diagnostic_msgs - pluginlib - rclcpp - rclcpp_components - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - 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 7129221f6c27b..3642e0cf94604 100644 --- a/evaluator/kinematic_evaluator/package.xml +++ b/evaluator/kinematic_evaluator/package.xml @@ -17,8 +17,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_planning_msgs + autoware_universe_utils diagnostic_msgs eigen geometry_msgs @@ -27,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 075bc83a1f985..8d3b788a2b8d9 100644 --- a/evaluator/localization_evaluator/package.xml +++ b/evaluator/localization_evaluator/package.xml @@ -12,8 +12,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_planning_msgs + autoware_universe_utils diagnostic_msgs eigen geometry_msgs @@ -23,7 +23,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils ament_cmake_gtest ament_lint_auto diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md index 7ee93d2ffddf2..7df375ac236d0 100644 --- a/evaluator/perception_online_evaluator/README.md +++ b/evaluator/perception_online_evaluator/README.md @@ -155,11 +155,11 @@ where: ## Inputs / Outputs -| Name | Type | Description | -| ----------------- | ------------------------------------------------------ | ------------------------------------------------- | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. | -| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. | -| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. | +| Name | Type | Description | +| ----------------- | ------------------------------------------------- | ------------------------------------------------- | +| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. | +| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. | +| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. | ## Parameters diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp index 5715f22669fdc..010f1497da3da 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp @@ -21,8 +21,8 @@ #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include #include @@ -37,8 +37,8 @@ namespace perception_diagnostics { namespace metrics { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; struct DetectionRange { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp index da7a23b6980b6..b480eaa3e67d6 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp @@ -17,7 +17,7 @@ #include "perception_online_evaluator/stat.hpp" -#include +#include #include #include @@ -26,7 +26,7 @@ namespace perception_diagnostics { namespace metrics { -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Pose; /** diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index 926fbb7435f3a..eaa07f2317940 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace perception_diagnostics @@ -36,7 +37,11 @@ enum class Metric { SIZE, }; +// Each metric has a different return type. (statistic or just a one value etc). +// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant using MetricStatMap = std::unordered_map>; +using MetricValueMap = std::unordered_map; +using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index 96b1c01ee2d16..a9b1388281ce8 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -25,8 +25,8 @@ #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "geometry_msgs/msg/pose.hpp" #include @@ -41,8 +41,8 @@ namespace perception_diagnostics { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using metrics::DetectionCounter; @@ -96,7 +96,7 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return map of string describing the requested metric and the calculated value */ - std::optional calculate(const Metric & metric) const; + std::optional calculate(const Metric & metric) const; /** * @brief set the dynamic objects used to calculate obstacle metrics @@ -143,7 +143,7 @@ class MetricsCalculator PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const; - MetricStatMap calcObjectsCountMetrics() const; + MetricValueMap calcObjectsCountMetrics() const; bool hasPassedTime(const rclcpp::Time stamp) const; bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index e5c41ff28c4da..1bc427e667a2a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -22,7 +22,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "nav_msgs/msg/odometry.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -35,8 +35,8 @@ namespace perception_diagnostics { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; @@ -61,6 +61,8 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node DiagnosticStatus generateDiagnosticStatus( const std::string metric, const Stat & metric_stat) const; + DiagnosticStatus generateDiagnosticStatus( + const std::string & metric, const double metric_value) const; private: // Subscribers and publishers 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 0831d580248d3..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 @@ -15,9 +15,9 @@ #ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ #define PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ -#include +#include -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include #include @@ -30,11 +30,11 @@ namespace marker_utils { -using autoware_auto_perception_msgs::msg::PredictedObject; +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; @@ -45,10 +45,10 @@ inline int64_t bitShift(int64_t original_id) void addFootprintMarker( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); MarkerArray createPointsMarkerArray( diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp index ee348b108d139..7adab46f42d2f 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp @@ -17,9 +17,9 @@ #include "perception_online_evaluator/parameters.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -34,9 +34,9 @@ namespace perception_diagnostics { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using ClassObjectsMap = std::unordered_map; diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index 3f855e2f1f887..082a9bc2d1366 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -17,13 +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 @@ -31,8 +33,6 @@ rclcpp_components tf2 tf2_ros - tier4_autoware_utils - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml index 807f247807d98..16d4b6fe458cd 100644 --- a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml +++ b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml @@ -13,7 +13,7 @@ prediction_time_horizons: [1.0, 2.0, 3.0, 5.0] stopped_velocity_threshold: 1.0 - detection_radius_list: [50.0, 100.0, 150.0, 200.0] + detection_radius_list: [20.0, 40.0, 60.0, 80.0, 100.0, 120.0, 140.0, 160.0, 180.0, 200.0] detection_height_list: [10.0] detection_count_purge_seconds: 36000.0 objects_count_window_seconds: 1.0 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 9c1e0667e4fef..212af8711a62e 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -14,19 +14,19 @@ #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 +std::optional MetricsCalculator::calculate(const Metric & metric) const { // clang-format off const bool use_past_objects = metric == Metric::lateral_deviation || @@ -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; @@ -455,15 +455,14 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas return metric_stat_map; } -MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const +MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const { - MetricStatMap metric_stat_map; + MetricValueMap metric_stat_map; // calculate the average number of objects in the detection area in all past frames const auto overall_average_count = detection_counter_.getOverallAverageCount(); for (const auto & [label, range_and_count] : overall_average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } // calculate the average number of objects in the detection area in the past @@ -472,8 +471,8 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const detection_counter_.getAverageCount(parameters_->objects_count_window_seconds); for (const auto & [label, range_and_count] : average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] - .add(count); + metric_stat_map + ["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } @@ -481,8 +480,7 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const const auto total_count = detection_counter_.getTotalCount(); for (const auto & [label, range_and_count] : total_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } @@ -496,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); @@ -559,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; @@ -605,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 @@ -647,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; } } @@ -664,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 0fcdd77f4d515..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" @@ -69,16 +69,25 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() // calculate metrics for (const Metric & metric : parameters_->metrics) { - const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); - if (!metric_stat_map.has_value()) { + const auto metric_result = metrics_calculator_.calculate(Metric(metric)); + if (!metric_result.has_value()) { continue; } - for (const auto & [metric, stat] : metric_stat_map.value()) { - if (stat.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); - } - } + std::visit( + [&metrics_msg, this](auto && arg) { + using T = std::decay_t; + for (const auto & [metric, value] : arg) { + if constexpr (std::is_same_v) { + if (value.count() > 0) { + metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + } + } else if constexpr (std::is_same_v) { + metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + } + } + }, + metric_result.value()); } // publish metrics @@ -111,6 +120,22 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( return status; } +DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( + const std::string & metric, const double value) const +{ + DiagnosticStatus status; + + status.level = status.OK; + status.name = metric; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metric_value"; + key_value.value = std::to_string(value); + status.values.push_back(key_value); + + return status; +} + void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_); @@ -131,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; @@ -211,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_; @@ -280,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 523e11883e755..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,36 +28,36 @@ 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( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const double half_width = vehicle_info.vehicle_width_m / 2.0; const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; 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()); } MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) { const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); @@ -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 95d6f07cca5d9..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,11 +16,11 @@ #include "rclcpp/time.hpp" #include +#include #include -#include -#include -#include +#include +#include #include #include @@ -35,15 +35,15 @@ #include using EvalNode = perception_diagnostics::PerceptionOnlineEvaluatorNode; -using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; -using PredictedObject = autoware_auto_perception_msgs::msg::PredictedObject; +using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; +using PredictedObject = autoware_perception_msgs::msg::PredictedObject; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; using MarkerArray = visualization_msgs::msg::MarkerArray; -using ObjectClassification = autoware_auto_perception_msgs::msg::ObjectClassification; +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; @@ -141,7 +141,19 @@ class EvalTest : public ::testing::Test [=](const DiagnosticArray::ConstSharedPtr msg) { const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); if (it != msg->status.end()) { - metric_value_ = boost::lexical_cast(it->values[2].value); + const auto mean_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "mean"; }); + if (mean_it != it->values.end()) { + metric_value_ = boost::lexical_cast(mean_it->value); + } else { + const auto metric_value_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "metric_value"; }); + if (metric_value_it != it->values.end()) { + metric_value_ = boost::lexical_cast(metric_value_it->value); + } + } metric_updated_ = true; } }); @@ -172,7 +184,7 @@ class EvalTest : public ::testing::Test object.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; object.kinematics.initial_twist_with_covariance.twist.linear.z = 0.0; - autoware_auto_perception_msgs::msg::PredictedPath path; + autoware_perception_msgs::msg::PredictedPath path; for (size_t i = 0; i < predicted_path.size(); ++i) { geometry_msgs::msg::Pose pose; pose.position.x = predicted_path[i].first; 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/evaluator/planning_evaluator/README.md b/evaluator/planning_evaluator/README.md deleted file mode 100644 index 23c8101429c35..0000000000000 --- a/evaluator/planning_evaluator/README.md +++ /dev/null @@ -1,96 +0,0 @@ -# Planning Evaluator - -## Purpose - -This package provides nodes that generate metrics to evaluate the quality of planning and control. - -## Inner-workings / Algorithms - -The evaluation node calculates metrics each time it receives a trajectory `T(0)`. -Metrics are calculated using the following information: - -- the trajectory `T(0)` itself. -- the previous trajectory `T(-1)`. -- the _reference_ trajectory assumed to be used as the reference to plan `T(0)`. -- the current ego pose. -- the set of objects in the environment. - -These information are maintained by an instance of class `MetricsCalculator` -which is also responsible for calculating metrics. - -### Stat - -Each metric is calculated using a `Stat` instance which contains -the minimum, maximum, and mean values calculated for the metric -as well as the number of values measured. - -### Metric calculation and adding more metrics - -All possible metrics are defined in the `Metric` enumeration defined -`include/planning_evaluator/metrics/metric.hpp`. -This file also defines conversions from/to string as well as human readable descriptions -to be used as header of the output file. - -The `MetricsCalculator` is responsible for calculating metric statistics -through calls to function: - -```C++ -Stat MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const; -``` - -Adding a new metric `M` requires the following steps: - -- `metrics/metric.hpp`: add `M` to the `enum`, to the from/to string conversion maps, and to the description map. -- `metrics_calculator.cpp`: add `M` to the `switch/case` statement of the `calculate` function. -- Add `M` to the `selected_metrics` parameters. - -## Inputs / Outputs - -### Inputs - -| Name | Type | Description | -| ------------------------------ | ------------------------------------------------------ | ------------------------------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Main trajectory to evaluate | -| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory to use for deviation metrics | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Obstacles | - -### Outputs - -Each metric is published on a topic named after the metric name. - -| Name | Type | Description | -| ----------- | --------------------------------------- | ------------------------------------------------------- | -| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | DiagnosticArray with a DiagnosticStatus for each metric | - -When shut down, the evaluation node writes the values of the metrics measured during its lifetime -to a file as specified by the `output_file` parameter. - -## Parameters - -| Name | Type | Description | -| :-------------------------------- | :------- | :-------------------------------------------------------------------------- | -| `output_file` | `string` | file used to write metrics | -| `ego_frame` | `string` | frame used for the ego pose | -| `selected_metrics` | List | metrics to measure and publish | -| `trajectory.min_point_dist_m` | `double` | minimum distance between two successive points to use for angle calculation | -| `trajectory.lookahead.max_dist_m` | `double` | maximum distance from ego along the trajectory to use for calculation | -| `trajectory.lookahead.max_time_m` | `double` | maximum time ahead of ego along the trajectory to use for calculation | -| `obstacle.dist_thr_m` | `double` | distance between ego and the obstacle below which a collision is considered | - -## Assumptions / Known limits - -There is a strong assumption that when receiving a trajectory `T(0)`, -it has been generated using the last received reference trajectory and objects. -This can be wrong if a new reference trajectory or objects are published while `T(0)` is being calculated. - -Precision is currently limited by the resolution of the trajectories. -It is possible to interpolate the trajectory and reference trajectory to increase precision but would make computation significantly more expensive. - -## Future extensions / Unimplemented parts - -- Use `Route` or `Path` messages as reference trajectory. -- RSS metrics (done in another node ). -- Add option to publish the `min` and `max` metric values. For now only the `mean` value is published. -- `motion_evaluator_node`. - - Node which constructs a trajectory over time from the real motion of ego. - - Only a proof of concept is currently implemented. diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp deleted file mode 100644 index 04a5b758d62e1..0000000000000 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright 2021 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 PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#define PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ - -#include "planning_evaluator/stat.hpp" - -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - -namespace planning_diagnostics -{ -namespace metrics -{ -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -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] traj input trajectory - * @return calculated statistics - */ -Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj); - -/** - * @brief calculate yaw deviation of the given trajectory from the reference trajectory - * @param [in] ref reference trajectory - * @param [in] traj input trajectory - * @return calculated statistics - */ -Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj); - -/** - * @brief calculate velocity deviation of the given trajectory from the reference trajectory - * @param [in] ref reference trajectory - * @param [in] traj input trajectory - * @return calculated statistics - */ -Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj); - -/** - * @brief calculate longitudinal deviation of the given ego pose from the modified goal pose - * @param [in] base_pose base pose - * @param [in] target_point target point - * @return calculated statistics - */ -Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point); - -/** - * @brief calculate lateral deviation of the given ego pose from the modified goal pose - * @param [in] base_pose base pose - * @param [in] target_point target point - * @return calculated statistics - */ -Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point); - -/** - * @brief calculate yaw deviation of the given ego pose from the modified goal pose - * @param [in] base_pose base pose - * @param [in] target_pose target pose - * @return calculated statistics - */ -Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose); - -} // namespace metrics -} // namespace planning_diagnostics - -#endif // PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp deleted file mode 100644 index 4806446d4270f..0000000000000 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2021 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 PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ -#define PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ - -#include "planning_evaluator/stat.hpp" - -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - -namespace planning_diagnostics -{ -namespace metrics -{ -namespace utils -{ -using autoware_auto_planning_msgs::msg::Trajectory; - -/** - * @brief find the index in the trajectory at the given distance of the given index - * @param [in] traj input trajectory - * @param [in] curr_id index - * @param [in] distance distance - * @return index of the trajectory point at distance ahead of traj[curr_id] - */ -size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance); - -} // namespace utils -} // namespace metrics -} // namespace planning_diagnostics -#endif // PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp deleted file mode 100644 index 848d92c91f13e..0000000000000 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2021 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 PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ -#define PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ - -#include "planning_evaluator/stat.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" - -namespace planning_diagnostics -{ -namespace metrics -{ -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; - -/** - * @brief calculate the distance to the closest obstacle at each point of the trajectory - * @param [in] obstacles obstacles - * @param [in] traj trajectory - * @return calculated statistics - */ -Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj); - -/** - * @brief calculate the time to collision of the trajectory with the given obstacles - * Assume that "now" corresponds to the first trajectory point - * @param [in] obstacles obstacles - * @param [in] traj trajectory - * @return calculated statistics - */ -Stat calcTimeToCollision( - const PredictedObjects & obstacles, const Trajectory & traj, const double distance_threshold); - -} // namespace metrics -} // namespace planning_diagnostics - -#endif // PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ diff --git a/evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp b/evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp deleted file mode 100644 index fb4acb53888f2..0000000000000 --- a/evaluator/planning_evaluator/include/planning_evaluator/parameters.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2021 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 PLANNING_EVALUATOR__PARAMETERS_HPP_ -#define PLANNING_EVALUATOR__PARAMETERS_HPP_ - -#include "planning_evaluator/metrics/metric.hpp" - -#include - -namespace planning_diagnostics -{ -/** - * @brief Enumeration of trajectory metrics - */ -struct Parameters -{ - std::array(Metric::SIZE)> metrics{}; // default values to false - - struct - { - double min_point_dist_m = 0.1; - struct - { - double max_dist_m = 5.0; - double max_time_s = 3.0; - } lookahead; - } trajectory; - - struct - { - double dist_thr_m = 1.0; - } obstacle; -}; // struct Parameters - -} // namespace planning_diagnostics - -#endif // PLANNING_EVALUATOR__PARAMETERS_HPP_ diff --git a/evaluator/planning_evaluator/launch/motion_evaluator.launch.xml b/evaluator/planning_evaluator/launch/motion_evaluator.launch.xml deleted file mode 100644 index d6cf76da860dc..0000000000000 --- a/evaluator/planning_evaluator/launch/motion_evaluator.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/evaluator/planning_evaluator/package.xml b/evaluator/planning_evaluator/package.xml deleted file mode 100644 index fb3270fb0d89b..0000000000000 --- a/evaluator/planning_evaluator/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - planning_evaluator - 0.1.0 - ROS 2 node for evaluating planners - Maxime CLEMENT - Kyoichi Sugahara - Apache License 2.0 - - Maxime CLEMENT - - ament_cmake_auto - autoware_cmake - - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_planning_msgs - 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 - autoware_lint_common - - - ament_cmake - - diff --git a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp deleted file mode 100644 index e1ad9c1a2eeec..0000000000000 --- a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp +++ /dev/null @@ -1,102 +0,0 @@ -// Copyright 2021 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 "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" - -namespace planning_diagnostics -{ -namespace metrics -{ -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; - -Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj) -{ - Stat stat; - - if (ref.points.empty() || traj.points.empty()) { - return stat; - } - - /** TODO(Maxime CLEMENT): - * 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)); - } - return stat; -} - -Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) -{ - Stat stat; - - if (ref.points.empty() || traj.points.empty()) { - return stat; - } - - /** TODO(Maxime CLEMENT): - * 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)); - } - return stat; -} - -Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj) -{ - Stat stat; - - if (ref.points.empty() || traj.points.empty()) { - return stat; - } - - // 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); - stat.add(p.longitudinal_velocity_mps - ref.points[nearest_index].longitudinal_velocity_mps); - } - return stat; -} - -Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) -{ - Stat stat; - stat.add(std::abs(tier4_autoware_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))); - 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))); - return stat; -} -} // namespace metrics -} // namespace planning_diagnostics diff --git a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp deleted file mode 100644 index 7a3418da881c6..0000000000000 --- a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright 2021 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 "planning_evaluator/metrics/stability_metrics.hpp" - -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" - -#include - -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - -#include - -namespace planning_diagnostics -{ -namespace metrics -{ -using autoware_auto_planning_msgs::msg::TrajectoryPoint; - -Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2) -{ - Stat stat; - - if (traj1.points.empty() || traj2.points.empty()) { - return stat; - } - - Eigen::MatrixXd ca = Eigen::MatrixXd::Zero(traj1.points.size(), traj2.points.size()); - - 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]); - 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*/) { - ca(i, j) = std::max(ca(i - 1, 0), dist); - } else if (j > 0 /*&& i == 0*/) { - ca(i, j) = std::max(ca(0, j - 1), dist); - } else { /* i == j == 0 */ - ca(i, j) = dist; - } - } - } - stat.add(ca(traj1.points.size() - 1, traj2.points.size() - 1)); - return stat; -} - -Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2) -{ - Stat stat; - if (traj1.points.empty()) { - return stat; - } - for (const auto & point : traj2.points) { - const auto p0 = tier4_autoware_utils::getPoint(point); - // find nearest segment - const size_t nearest_segment_idx = 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( - traj1.points[nearest_segment_idx], traj1.points[nearest_segment_idx + 1])) { - // distance to last point - dist = tier4_autoware_utils::calcDistance2d(traj1.points.back(), p0); - } else if ( // NOLINT - nearest_segment_idx == 0 && - motion_utils::calcLongitudinalOffsetToSegment(traj1.points, nearest_segment_idx, p0) <= 0) { - // distance to first point - dist = tier4_autoware_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]); - 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)); - } - stat.add(dist); - } - return stat; -} - -} // namespace metrics -} // namespace planning_diagnostics diff --git a/launch/tier4_control_launch/control_launch.drawio.svg b/launch/tier4_control_launch/control_launch.drawio.svg index 5d13b84a0b34b..21d97ba08c0b3 100644 --- a/launch/tier4_control_launch/control_launch.drawio.svg +++ b/launch/tier4_control_launch/control_launch.drawio.svg @@ -259,17 +259,17 @@ >
- lane_departure_checker_node + autoware_lane_departure_checker_node

- package: lane_departure_checker_node + package: autoware_lane_departure_checker_node
- lane_departure_checker_node... + autoware_lane_departure_checker_node... diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 77140b0e0f630..a61f29014dcce 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -53,7 +53,8 @@ def launch_setup(context, *args, **kwargs): with open(LaunchConfiguration("control_validator_param_path").perform(context), "r") as f: control_validator_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open( - LaunchConfiguration("operation_mode_transition_manager_param_path").perform(context), "r" + LaunchConfiguration("operation_mode_transition_manager_param_path").perform(context), + "r", ) as f: operation_mode_transition_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("shift_decider_param_path").perform(context), "r") as f: @@ -69,7 +70,7 @@ def launch_setup(context, *args, **kwargs): trajectory_follower_mode = LaunchConfiguration("trajectory_follower_mode").perform(context) controller_component = ComposableNode( - package="trajectory_follower_node", + package="autoware_trajectory_follower_node", plugin="autoware::motion::control::trajectory_follower_node::Controller", name="controller_node_exe", namespace="trajectory_follower", @@ -102,8 +103,8 @@ def launch_setup(context, *args, **kwargs): # lane departure checker lane_departure_component = ComposableNode( - package="lane_departure_checker", - plugin="lane_departure_checker::LaneDepartureCheckerNode", + package="autoware_lane_departure_checker", + plugin="autoware::lane_departure_checker::LaneDepartureCheckerNode", name="lane_departure_checker_node", namespace="trajectory_follower", remappings=[ @@ -122,9 +123,9 @@ def launch_setup(context, *args, **kwargs): # shift decider shift_decider_component = ComposableNode( - package="shift_decider", - plugin="ShiftDecider", - name="shift_decider", + package="autoware_shift_decider", + plugin="autoware::shift_decider::ShiftDecider", + name="autoware_shift_decider", remappings=[ ("input/control_cmd", "/control/trajectory_follower/control_cmd"), ("input/state", "/autoware/state"), @@ -139,7 +140,7 @@ def launch_setup(context, *args, **kwargs): # autonomous emergency braking autonomous_emergency_braking = ComposableNode( - package="autonomous_emergency_braking", + package="autoware_autonomous_emergency_braking", plugin="autoware::motion::control::autonomous_emergency_braking::AEB", name="autonomous_emergency_braking", remappings=[ @@ -194,8 +195,8 @@ def launch_setup(context, *args, **kwargs): # vehicle cmd gate vehicle_cmd_gate_component = ComposableNode( - package="vehicle_cmd_gate", - plugin="vehicle_cmd_gate::VehicleCmdGate", + package="autoware_vehicle_cmd_gate", + plugin="autoware::vehicle_cmd_gate::VehicleCmdGate", name="vehicle_cmd_gate", remappings=[ ("input/steering", "/vehicle/status/steering_status"), @@ -245,10 +246,10 @@ def launch_setup(context, *args, **kwargs): ) # operation mode transition manager - operation_mode_transition_manager_component = ComposableNode( - package="operation_mode_transition_manager", - plugin="operation_mode_transition_manager::OperationModeTransitionManager", - name="operation_mode_transition_manager", + autoware_operation_mode_transition_manager_component = ComposableNode( + package="autoware_operation_mode_transition_manager", + plugin="autoware::operation_mode_transition_manager::OperationModeTransitionManager", + name="autoware_operation_mode_transition_manager", remappings=[ # input ("kinematics", "/localization/kinematic_state"), @@ -272,7 +273,10 @@ def launch_setup(context, *args, **kwargs): # external cmd selector external_cmd_selector_loader = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("external_cmd_selector"), "/launch/external_cmd_selector.launch.py"] + [ + FindPackageShare("autoware_external_cmd_selector"), + "/launch/external_cmd_selector.launch.py", + ] ), launch_arguments=[ ("use_intra_process", LaunchConfiguration("use_intra_process")), @@ -287,7 +291,10 @@ def launch_setup(context, *args, **kwargs): # external cmd converter external_cmd_converter_loader = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("external_cmd_converter"), "/launch/external_cmd_converter.launch.py"] + [ + FindPackageShare("autoware_external_cmd_converter"), + "/launch/external_cmd_converter.launch.py", + ] ), launch_arguments=[ ("use_intra_process", LaunchConfiguration("use_intra_process")), @@ -341,7 +348,7 @@ def launch_setup(context, *args, **kwargs): lane_departure_component, shift_decider_component, vehicle_cmd_gate_component, - operation_mode_transition_manager_component, + autoware_operation_mode_transition_manager_component, glog_component, ], ) @@ -356,7 +363,7 @@ def launch_setup(context, *args, **kwargs): lane_departure_component, shift_decider_component, vehicle_cmd_gate_component, - operation_mode_transition_manager_component, + autoware_operation_mode_transition_manager_component, glog_component, ], ) @@ -367,12 +374,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"), ], ) @@ -383,8 +392,8 @@ def launch_setup(context, *args, **kwargs): # control validator checker control_validator_component = ComposableNode( - package="control_validator", - plugin="control_validator::ControlValidator", + package="autoware_control_validator", + plugin="autoware::control_validator::ControlValidator", name="control_validator", remappings=[ ("~/input/kinematics", "/localization/kinematic_state"), @@ -432,7 +441,7 @@ def launch_setup(context, *args, **kwargs): ) smart_mpc_trajectory_follower = Node( - package="smart_mpc_trajectory_follower", + package="autoware_smart_mpc_trajectory_follower", executable="pympc_trajectory_follower.py", name="pympc_trajectory_follower", ) diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index 801fa274dd086..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 - control_evaluator - external_cmd_converter - external_cmd_selector - lane_departure_checker - shift_decider - trajectory_follower_node - vehicle_cmd_gate + 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 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_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 132d1ec9be3ea..c58c0848eb5b3 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -22,7 +22,7 @@ - + diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index 8fb41076204a8..05643e354cfce 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -4,7 +4,6 @@ tier4_map_launch 0.1.0 The tier4_map_launch package - Ryohsuke Mitsudome Ryu Yamamoto Koji Minoda Kento Yabuuchi @@ -14,6 +13,8 @@ Shintaro Sakoda Masahiro Sakamoto Apache License 2.0 + Ryohsuke Mitsudome + Yamato Ando ament_cmake_auto autoware_cmake 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 9c3e815e8abb4..754d07c6d99bf 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 @@ -2,10 +2,11 @@ + - + @@ -65,10 +66,10 @@ - + - + @@ -78,6 +79,12 @@ + + + + + + @@ -98,6 +105,8 @@ + + @@ -138,6 +147,12 @@ + + + + + + @@ -159,16 +174,17 @@ + - + - + @@ -189,10 +205,17 @@ + + + + + + + @@ -217,9 +240,16 @@ + + + + + + + @@ -227,8 +257,8 @@ - - + + 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..fe0ba0e614e60 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 @@ -94,7 +94,7 @@ - + @@ -234,12 +234,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 f941bc8d771b1..274b39be681b8 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,16 +2,41 @@ - + - + + + + + + + + + + + + + + + + + + + + + + + + + + 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..e78823429195f 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 @@ -6,7 +6,7 @@ - + @@ -45,12 +45,4 @@ - - - - - - - - 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/detector/radar_detector.launch.xml deleted file mode 100644 index b09684281d33a..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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..7047aec77c660 --- /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..b3704f130232a --- /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/filter/radar_filter.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml new file mode 100644 index 0000000000000..d837505649cf1 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..5bcfe49643dce 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,23 +1,55 @@ - - - - - + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + - @@ -44,81 +76,52 @@ - - + + - - - - - + + - - + + + - - - - - - - - + - - - - - - - - - + + + + - - - - - + + + - - - - - - - - - - - - - - - - - - + + + + + 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..07ab78cd37f72 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,39 +1,78 @@ - - - - - + - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + - + - - @@ -60,91 +99,63 @@ - - + + - - - - - + + - - - - - - - - - + + + - + - - - - - - - - - + + + + - - - - - + + + - - - - - - - - - - - - - - - - - - + + + + + + - - - + + + 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..f0ce19db6c342 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/prediction/prediction.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml index fa343f49840ad..a9ba980b3ce47 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml @@ -5,7 +5,7 @@ - + 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 fed268084792a..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,39 +1,50 @@ - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - + + @@ -41,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 d60f54c2e641a..dd80ab5b2cde7 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -22,6 +22,7 @@ + @@ -46,7 +47,7 @@ - + @@ -77,7 +78,7 @@ - + - + @@ -216,6 +217,8 @@ + + @@ -237,6 +240,7 @@ + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 3be8678113728..ed7ff059a6e53 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -17,7 +17,7 @@ - + @@ -158,7 +158,7 @@ - + @@ -171,7 +171,7 @@ - + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 2d007ae8c5ff0..6ec706a4aad32 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -12,9 +12,10 @@ ament_cmake_auto autoware_cmake + autoware_crosswalk_traffic_light_estimator + autoware_map_based_prediction cluster_merger compare_map_segmentation - crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker @@ -23,7 +24,6 @@ image_projection_based_fusion image_transport_decompressor lidar_apollo_instance_segmentation - map_based_prediction multi_object_tracker object_merger object_range_splitter diff --git a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml index 0885f48a6827d..cd33e7517ddc3 100644 --- a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml @@ -1,11 +1,11 @@ - + - + - + diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 0e693943a4d03..e71253c8a978b 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -32,8 +32,8 @@ - - + + @@ -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 aa649710836da..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 @@ -7,9 +7,9 @@ - + - + @@ -32,9 +32,7 @@ - - @@ -42,57 +40,57 @@ @@ -101,88 +99,78 @@ - - - + @@ -206,9 +194,9 @@ - + - + @@ -220,7 +208,7 @@ - + @@ -242,7 +230,7 @@ - + @@ -261,9 +249,7 @@ - - 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 18de04fd9e317..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,6 +2,30 @@ + + + + + + + + + + + + @@ -10,7 +34,7 @@ - + @@ -32,7 +56,7 @@ - + @@ -42,18 +66,18 @@ - + - + - + - + @@ -62,11 +86,11 @@ - + - + @@ -80,10 +104,10 @@ - + - + @@ -91,24 +115,35 @@ - + - + - - - + + + + + + + + - - - - + + + + - + + + + + + + @@ -119,9 +154,9 @@ - + - + @@ -144,7 +179,7 @@ - + @@ -169,9 +204,9 @@ - + - + @@ -182,7 +217,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index 98315919b540a..e0fdcb1e30408 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -4,7 +4,7 @@ - + @@ -18,7 +18,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 0a30204ca3c99..a252986f9d957 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -3,7 +3,7 @@ - + @@ -19,24 +19,24 @@ - + - + - - - + + + - + - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index d04a405a61c7b..1c65c975bc5bf 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -57,24 +57,24 @@ ament_cmake_auto autoware_cmake + autoware_behavior_path_planner + autoware_behavior_velocity_planner + autoware_costmap_generator + autoware_external_cmd_selector + autoware_external_velocity_limit_selector + autoware_freespace_planner + 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 - behavior_path_planner - behavior_velocity_planner - costmap_generator - external_cmd_selector - external_velocity_limit_selector - freespace_planner + autoware_scenario_selector + autoware_surround_obstacle_checker + autoware_velocity_smoother glog_component - mission_planner - motion_velocity_smoother - obstacle_avoidance_planner - obstacle_cruise_planner obstacle_stop_planner - planning_evaluator - planning_topic_converter - planning_validator - scenario_selector - surround_obstacle_checker ament_lint_auto autoware_lint_common diff --git a/launch/tier4_sensing_launch/launch/sensing.launch.xml b/launch/tier4_sensing_launch/launch/sensing.launch.xml index 391a1f8bad641..9981c99a8f3fc 100644 --- a/launch/tier4_sensing_launch/launch/sensing.launch.xml +++ b/launch/tier4_sensing_launch/launch/sensing.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/launch/tier4_sensing_launch/package.xml b/launch/tier4_sensing_launch/package.xml index a1f10ee743db5..410f685285b7d 100644 --- a/launch/tier4_sensing_launch/package.xml +++ b/launch/tier4_sensing_launch/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - vehicle_info_util + autoware_vehicle_info_utils 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/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index a1bbfc883de7c..6253dce864ddf 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -25,11 +25,11 @@ - - - - - + + + + + @@ -57,6 +57,13 @@ + + + + + + + @@ -70,8 +77,20 @@ + + + + + + + + + + + + - + @@ -85,52 +104,31 @@ - + + - - + + + - - - - - - - - - - + + + - + - - - - - - - - - - - - - - - - diff --git a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml index 0e1d22bfd1827..ffd32ac92c0d5 100644 --- a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml +++ b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml @@ -6,7 +6,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/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CMakeLists.txt b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CMakeLists.txt new file mode 100644 index 0000000000000..8b2487ef79ea9 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_ar_tag_based_localizer) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(OpenCV REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/ar_tag_based_localizer.cpp +) + +target_include_directories(${PROJECT_NAME} + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} +) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "ArTagBasedLocalizer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_${PROJECT_NAME} + test/test.cpp + src/ar_tag_based_localizer.cpp + ) + target_include_directories(test_${PROJECT_NAME} + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} + ) + target_link_libraries(test_${PROJECT_NAME} ${OpenCV_LIBRARIES}) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md new file mode 100644 index 0000000000000..37596ee9820b9 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md @@ -0,0 +1,80 @@ +# AR Tag Based Localizer + +**ArTagBasedLocalizer** is a vision-based localization node. + +ar_tag_image + +This node uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. +The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. + +## Inputs / Outputs + +### `ar_tag_based_localizer` node + +#### Input + +| Name | Type | Description | +| :--------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `~/input/lanelet2_map` | `autoware_map_msgs::msg::LaneletMapBin` | Data of lanelet2 | +| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | +| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | +| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. | + +#### Output + +| Name | Type | Description | +| :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- | +| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose | +| `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | +| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] Loaded landmarks to visualize in Rviz as thin boards | +| `/tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | + +## Parameters + +{{ json_to_markdown("localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json") }} + +## How to launch + +When launching Autoware, set `artag` for `pose_source`. + +```bash +ros2 launch autoware_launch ... \ + pose_source:=artag \ + ... +``` + +### Rosbag + +#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1uMVwQQFcfs8JOqfoA1FqfH_fLPwQ71jK/view) + +This data is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). +Essentially, AR tag-based self-localization is not intended for such public road driving, but for driving in a smaller area, so the max driving speed is set at 15 km/h. + +It is a known problem that the timing of when each AR tag begins to be detected can cause significant changes in estimation. + +![sample_result_in_awsim](./doc_image/sample_result_in_awsim.png) + +#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) + +Please remap the topic names and play it. + +```bash +ros2 bag play /path/to/ar_tag_based_localizer_sample_bag/ -r 0.5 -s sqlite3 \ + --remap /sensing/camera/front/image:=/sensing/camera/traffic_light/image_raw \ + /sensing/camera/front/image/info:=/sensing/camera/traffic_light/camera_info +``` + +This dataset contains issues such as missing IMU data, and overall the accuracy is low. Even when running AR tag-based self-localization, significant difference from the true trajectory can be observed. + +The image below shows the trajectory when the sample is executed and plotted. + +![sample_result](./doc_image/sample_result.png) + +The pull request video below should also be helpful. + + + +## Principle + +![principle](../doc_image/principle.png) 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 272338905c3f0..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/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml new file mode 100644 index 0000000000000..bf40719d7a5e6 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml @@ -0,0 +1,36 @@ + + + + autoware_ar_tag_based_localizer + 0.0.0 + The autoware_ar_tag_based_localizer package + Shintaro Sakoda + Masahiro Sakamoto + Yamato Ando + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto + Apache License 2.0 + + ament_cmake + autoware_cmake + + ament_index_cpp + aruco + autoware_landmark_manager + cv_bridge + diagnostic_msgs + lanelet2_extension + localization_util + rclcpp + rclcpp_components + tf2_eigen + tf2_geometry_msgs + tf2_ros + visualization_msgs + + + ament_cmake + + 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 95% 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 43ac1e1098453..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,10 +62,10 @@ #include #endif -#include +#include ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) -: Node("ar_tag_based_localizer", options), cam_info_received_(false) +: rclcpp::Node("ar_tag_based_localizer", options), cam_info_received_(false) { /* Declare node parameters @@ -111,7 +111,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) Subscribers */ using std::placeholders::_1; - map_bin_sub_ = this->create_subscription( + map_bin_sub_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), std::bind(&ArTagBasedLocalizer::map_bin_callback, this, _1)); @@ -140,7 +140,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); } -void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) +void ArTagBasedLocalizer::map_bin_callback(const LaneletMapBin::ConstSharedPtr & msg) { landmark_manager_.parse_landmarks(msg, "apriltag_16h5"); const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); @@ -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); @@ -346,3 +346,6 @@ std::vector ArTagBasedLocalizer::detect_landmarks( return landmarks; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ArTagBasedLocalizer) 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 95% 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 f70821a39ffe8..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 @@ -70,7 +70,7 @@ class ArTagBasedLocalizer : public rclcpp::Node { - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using Image = sensor_msgs::msg::Image; using CameraInfo = sensor_msgs::msg::CameraInfo; using Pose = geometry_msgs::msg::Pose; @@ -86,7 +86,7 @@ class ArTagBasedLocalizer : public rclcpp::Node explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: - void map_bin_callback(const HADMapBin::ConstSharedPtr & msg); + void map_bin_callback(const LaneletMapBin::ConstSharedPtr & msg); void image_callback(const Image::ConstSharedPtr & msg); void cam_info_callback(const CameraInfo::ConstSharedPtr & msg); void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg); @@ -106,7 +106,7 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr tf_listener_; // Subscribers - rclcpp::Subscription::SharedPtr map_bin_sub_; + rclcpp::Subscription::SharedPtr map_bin_sub_; rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr cam_info_sub_; rclcpp::Subscription::SharedPtr ekf_pose_sub_; 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/autoware_landmark_based_localizer/autoware_landmark_manager/CMakeLists.txt b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/CMakeLists.txt new file mode 100644 index 0000000000000..6c67ff7a320e8 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_landmark_manager) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(landmark_manager SHARED + src/landmark_manager.cpp +) + +ament_auto_package( + INSTALL_TO_SHARE +) 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 85% 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 c2b0751d91f9a..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,14 +12,14 @@ // 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" #include -#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" #include #include #include @@ -41,7 +41,7 @@ class LandmarkManager { public: void parse_landmarks( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & msg, const std::string & target_subtype); [[nodiscard]] std::vector get_landmarks() const; @@ -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/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml new file mode 100644 index 0000000000000..1471b5182def8 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml @@ -0,0 +1,31 @@ + + + + autoware_landmark_manager + 0.0.0 + The autoware_landmark_manager package + Shintaro Sakoda + Masahiro Sakamoto + Yamato Ando + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto + Apache License 2.0 + + ament_cmake + autoware_cmake + + eigen + + autoware_map_msgs + geometry_msgs + lanelet2_extension + localization_util + rclcpp + tf2_eigen + + + ament_cmake + + 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 94% 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 57bfcde461af6..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 @@ -28,7 +28,7 @@ namespace landmark_manager { void LandmarkManager::parse_landmarks( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & msg, const std::string & target_subtype) { std::vector landmarks = @@ -50,7 +50,7 @@ void LandmarkManager::parse_landmarks( const auto & v2 = vertices[2]; const auto & v3 = vertices[3]; const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; - const double volume_threshold = 1e-5; + const double volume_threshold = 1e-3; if (volume > volume_threshold) { continue; } @@ -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/launch/pose_covariance_modifier.launch.xml b/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml index 0395496ae0b78..e2b90b2160a62 100755 --- a/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml +++ b/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml @@ -5,7 +5,7 @@ - + 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/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 9944cbb84d97c..13e63ddf3c170 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -13,7 +13,7 @@ include_directories( ament_auto_find_build_dependencies() -ament_auto_add_library(ekf_localizer_lib SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/ekf_localizer.cpp src/covariance.cpp src/diagnostics.cpp @@ -24,21 +24,20 @@ ament_auto_add_library(ekf_localizer_lib SHARED src/ekf_module.cpp ) -target_link_libraries(ekf_localizer_lib Eigen3::Eigen) - -ament_auto_add_executable(ekf_localizer src/ekf_localizer_node.cpp) - -target_compile_options(ekf_localizer PUBLIC -g -Wall -Wextra -Wpedantic -Werror) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "EKFLocalizer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) -target_link_libraries(ekf_localizer ekf_localizer_lib) -target_include_directories(ekf_localizer PUBLIC include) +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" ekf_localizer_lib) + target_link_libraries("${test_name}" ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp b/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp index 737c25f8ce024..880ab82418e06 100644 --- a/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp @@ -22,11 +22,11 @@ template class AgedObjectQueue { public: - explicit AgedObjectQueue(const int max_age) : max_age_(max_age) {} + explicit AgedObjectQueue(const size_t max_age) : max_age_(max_age) {} - bool empty() const { return this->size() == 0; } + [[nodiscard]] bool empty() const { return this->size() == 0; } - size_t size() const { return objects_.size(); } + [[nodiscard]] size_t size() const { return objects_.size(); } Object back() const { return objects_.back(); } @@ -39,7 +39,7 @@ class AgedObjectQueue Object pop_increment_age() { const Object object = objects_.front(); - const int age = ages_.front() + 1; + const size_t age = ages_.front() + 1; objects_.pop(); ages_.pop(); @@ -54,13 +54,13 @@ class AgedObjectQueue void clear() { objects_ = std::queue(); - ages_ = std::queue(); + ages_ = std::queue(); } private: - const int max_age_; + const size_t max_age_; std::queue objects_; - std::queue ages_; + std::queue ages_; }; #endif // EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/covariance.hpp b/localization/ekf_localizer/include/ekf_localizer/covariance.hpp index a4448ecda2f45..713877c1307d2 100644 --- a/localization/ekf_localizer/include/ekf_localizer/covariance.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/covariance.hpp @@ -17,7 +17,7 @@ #include "ekf_localizer/matrix_types.hpp" -std::array ekfCovarianceToPoseMessageCovariance(const Matrix6d & P); -std::array ekfCovarianceToTwistMessageCovariance(const Matrix6d & P); +std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P); +std::array ekf_covariance_to_twist_message_covariance(const Matrix6d & P); #endif // EKF_LOCALIZER__COVARIANCE_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp index f4dc6436f6a40..4c92b2947642b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp @@ -20,21 +20,21 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated); +diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size( const std::string & measurement_type, const size_t queue_size); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate( const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, const double delay_time_threshold); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate( const std::string & measurement_type, const bool is_passed_mahalanobis_gate, const double mahalanobis_distance, const double mahalanobis_distance_threshold); -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array); #endif // EKF_LOCALIZER__DIAGNOSTICS_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 6925e8b63c66b..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 @@ -58,17 +58,15 @@ class Simple1DFilter x_ = 0; dev_ = 1e9; proc_dev_x_c_ = 0.0; - return; }; - void init(const double init_obs, const double obs_dev, const rclcpp::Time time) + void init(const double init_obs, const double obs_dev, const rclcpp::Time & time) { x_ = init_obs; dev_ = obs_dev; latest_time_ = time; initialized_ = true; - return; }; - void update(const double obs, const double obs_dev, const rclcpp::Time time) + void update(const double obs, const double obs_dev, const rclcpp::Time & time) { if (!initialized_) { init(obs, obs_dev, time); @@ -86,10 +84,9 @@ class Simple1DFilter dev_ = (1 - kalman_gain) * dev_; latest_time_ = time; - return; }; void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; } - double get_x() const { return x_; } + [[nodiscard]] double get_x() const { return x_; } private: bool initialized_; @@ -102,7 +99,7 @@ class Simple1DFilter class EKFLocalizer : public rclcpp::Node { public: - EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit EKFLocalizer(const rclcpp::NodeOptions & options); private: const std::shared_ptr warning_; @@ -145,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_; @@ -158,10 +155,9 @@ class EKFLocalizer : public rclcpp::Node double ekf_dt_; /* process noise variance for discrete model */ - double proc_cov_yaw_d_; //!< @brief discrete yaw process noise - double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise - double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0 - double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0 + double proc_cov_yaw_d_; //!< @brief discrete yaw process noise + double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0 + double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0 bool is_activated_; @@ -174,44 +170,45 @@ class EKFLocalizer : public rclcpp::Node /** * @brief computes update & prediction of EKF for each ekf_dt_[s] time */ - void timerCallback(); + void timer_callback(); /** * @brief publish tf for tf_rate [Hz] */ - void timerTFCallback(); + void timer_tf_callback(); /** - * @brief set poseWithCovariance measurement + * @brief set pose with covariance measurement */ - void callbackPoseWithCovariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); /** - * @brief set twistWithCovariance measurement + * @brief set twist with covariance measurement */ - void callbackTwistWithCovariance(geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void callback_twist_with_covariance( + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); /** * @brief set initial_pose to current EKF pose */ - void callbackInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void callback_initial_pose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); /** * @brief update predict frequency */ - void updatePredictFrequency(const rclcpp::Time & current_time); + void update_predict_frequency(const rclcpp::Time & current_time); /** * @brief get transform from frame_id */ - bool getTransformFromTF( + bool get_transform_from_tf( std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform); /** * @brief publish current EKF estimation result */ - void publishEstimateResult( + void publish_estimate_result( const geometry_msgs::msg::PoseStamped & current_ekf_pose, const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, const geometry_msgs::msg::TwistStamped & current_ekf_twist); @@ -219,27 +216,27 @@ class EKFLocalizer : public rclcpp::Node /** * @brief publish diagnostics message */ - void publishDiagnostics(const rclcpp::Time & current_time); + void publish_diagnostics(const rclcpp::Time & current_time); /** - * @brief update simple1DFilter + * @brief update simple 1d filter */ - void updateSimple1DFilters( + void update_simple_1d_filters( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step); /** - * @brief initialize simple1DFilter + * @brief initialize simple 1d filter */ - void initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + void init_simple_1d_filters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); /** * @brief trigger node */ - void serviceTriggerNode( + void service_trigger_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/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index e88a59ffdfab9..ee360e798f492 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -34,24 +34,13 @@ struct EKFDiagnosticInfo { - EKFDiagnosticInfo() - : no_update_count(0), - queue_size(0), - is_passed_delay_gate(true), - delay_time(0), - delay_time_threshold(0), - is_passed_mahalanobis_gate(true), - mahalanobis_distance(0) - { - } - - size_t no_update_count; - size_t queue_size; - bool is_passed_delay_gate; - double delay_time; - double delay_time_threshold; - bool is_passed_mahalanobis_gate; - double mahalanobis_distance; + size_t no_update_count{0}; + size_t queue_size{0}; + bool is_passed_delay_gate{true}; + double delay_time{0.0}; + double delay_time_threshold{0.0}; + bool is_passed_mahalanobis_gate{true}; + double mahalanobis_distance{0.0}; }; class EKFModule @@ -63,31 +52,32 @@ class EKFModule using Twist = geometry_msgs::msg::TwistStamped; public: - EKFModule(std::shared_ptr warning, const HyperParameters params); + EKFModule(std::shared_ptr warning, const HyperParameters & params); void initialize( const PoseWithCovariance & initial_pose, const geometry_msgs::msg::TransformStamped & transform); - geometry_msgs::msg::PoseStamped getCurrentPose( + [[nodiscard]] geometry_msgs::msg::PoseStamped get_current_pose( const rclcpp::Time & current_time, const double z, const double roll, const double pitch, bool get_biased_yaw) const; - geometry_msgs::msg::TwistStamped getCurrentTwist(const rclcpp::Time & current_time) const; - double getYawBias() const; - std::array getCurrentPoseCovariance() const; - std::array getCurrentTwistCovariance() const; + [[nodiscard]] geometry_msgs::msg::TwistStamped get_current_twist( + const rclcpp::Time & current_time) const; + [[nodiscard]] double get_yaw_bias() const; + [[nodiscard]] std::array get_current_pose_covariance() const; + [[nodiscard]] std::array get_current_twist_covariance() const; - size_t find_closest_delay_time_index(double target_value) const; + [[nodiscard]] size_t find_closest_delay_time_index(double target_value) const; void accumulate_delay_time(const double dt); - void predictWithDelay(const double dt); - bool measurementUpdatePose( + void predict_with_delay(const double dt); + bool measurement_update_pose( const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info); - bool measurementUpdateTwist( + bool measurement_update_twist( const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info); - geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay( + geometry_msgs::msg::PoseWithCovarianceStamped compensate_pose_with_z_delay( const PoseWithCovariance & pose, const double delay_time); private: diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index 56a13d1ecaf55..8d76102e5d64d 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -68,14 +68,14 @@ class HyperParameters const double tf_rate_; const bool publish_tf_; const bool enable_yaw_bias_estimation; - const int extend_state_step; + const size_t extend_state_step; const std::string pose_frame_id; const double pose_additional_delay; const double pose_gate_dist; - const int pose_smoothing_steps; + const size_t pose_smoothing_steps; const double twist_additional_delay; const double twist_gate_dist; - const int twist_smoothing_steps; + const size_t twist_smoothing_steps; const double proc_stddev_vx_c; //!< @brief vx process noise const double proc_stddev_wz_c; //!< @brief wz process noise const double proc_stddev_yaw_c; //!< @brief yaw process noise diff --git a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp index f2b9dc626e67a..1f0d75c5958d4 100644 --- a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp @@ -18,7 +18,7 @@ #include #include -double squaredMahalanobis( +double squared_mahalanobis( const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); diff --git a/localization/ekf_localizer/include/ekf_localizer/measurement.hpp b/localization/ekf_localizer/include/ekf_localizer/measurement.hpp index 4b2169f1b182f..396aaf7b9a1b4 100644 --- a/localization/ekf_localizer/include/ekf_localizer/measurement.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/measurement.hpp @@ -17,11 +17,11 @@ #include -Eigen::Matrix poseMeasurementMatrix(); -Eigen::Matrix twistMeasurementMatrix(); -Eigen::Matrix3d poseMeasurementCovariance( +Eigen::Matrix pose_measurement_matrix(); +Eigen::Matrix twist_measurement_matrix(); +Eigen::Matrix3d pose_measurement_covariance( const std::array & covariance, const size_t smoothing_step); -Eigen::Matrix2d twistMeasurementCovariance( +Eigen::Matrix2d twist_measurement_covariance( const std::array & covariance, const size_t smoothing_step); #endif // EKF_LOCALIZER__MEASUREMENT_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp index 6554f80aee014..358a2750bd3a8 100644 --- a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp @@ -19,12 +19,12 @@ #include -inline bool hasInf(const Eigen::MatrixXd & v) +inline bool has_inf(const Eigen::MatrixXd & v) { return v.array().isInf().any(); } -inline bool hasNan(const Eigen::MatrixXd & v) +inline bool has_nan(const Eigen::MatrixXd & v) { return v.array().isNaN().any(); } diff --git a/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp index 85a65828e4ee4..09a87e5fe154b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp @@ -17,10 +17,10 @@ #include "ekf_localizer/matrix_types.hpp" -double normalizeYaw(const double & yaw); -Vector6d predictNextState(const Vector6d & X_curr, const double dt); -Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt); -Matrix6d processNoiseCovariance( +double normalize_yaw(const double & yaw); +Vector6d predict_next_state(const Vector6d & X_curr, const double dt); +Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt); +Matrix6d process_noise_covariance( const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d); #endif // EKF_LOCALIZER__STATE_TRANSITION_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/string.hpp b/localization/ekf_localizer/include/ekf_localizer/string.hpp index a4cd1f320367c..0154e84b88004 100644 --- a/localization/ekf_localizer/include/ekf_localizer/string.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/string.hpp @@ -17,7 +17,7 @@ #include -inline std::string eraseLeadingSlash(const std::string & s) +inline std::string erase_leading_slash(const std::string & s) { std::string a = s; if (a.front() == '/') { diff --git a/localization/ekf_localizer/include/ekf_localizer/warning.hpp b/localization/ekf_localizer/include/ekf_localizer/warning.hpp index a3c8800242e2b..e7456d73ecfdd 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning.hpp @@ -29,7 +29,7 @@ class Warning RCLCPP_WARN(node_->get_logger(), "%s", message.c_str()); } - void warnThrottle(const std::string & message, const int duration_milliseconds) const + void warn_throttle(const std::string & message, const int duration_milliseconds) const { RCLCPP_WARN_THROTTLE( node_->get_logger(), *(node_->get_clock()), diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index e1eafdc6f7948..36c0eabd588fa 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -17,11 +17,12 @@ #include -std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold); -std::string twistDelayStepWarningMessage( +std::string pose_delay_step_warning_message( const double delay_time, const double delay_time_threshold); -std::string poseDelayTimeWarningMessage(const double delay_time); -std::string twistDelayTimeWarningMessage(const double delay_time); -std::string mahalanobisWarningMessage(const double distance, const double max_distance); +std::string twist_delay_step_warning_message( + const double delay_time, const double delay_time_threshold); +std::string pose_delay_time_warning_message(const double delay_time); +std::string twist_delay_time_warning_message(const double delay_time); +std::string mahalanobis_warning_message(const double distance, const double max_distance); #endif // EKF_LOCALIZER__WARNING_MESSAGE_HPP_ diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml index b6a1bd06185c2..3c66fabe34650 100644 --- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -17,7 +17,7 @@ - + diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index e9d756e547f26..07efca6339b98 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -23,16 +23,17 @@ eigen + autoware_universe_utils diagnostic_msgs fmt geometry_msgs kalman_filter nav_msgs rclcpp + rclcpp_components 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 0c98ac6857ea5..4655ea8a89855 100644 --- a/localization/ekf_localizer/src/covariance.cpp +++ b/localization/ekf_localizer/src/covariance.cpp @@ -14,14 +14,14 @@ #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 ekfCovarianceToPoseMessageCovariance(const Matrix6d & P) +std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P) { - std::array covariance; + std::array covariance{}; covariance.fill(0.); covariance[COV_IDX::X_X] = P(IDX::X, IDX::X); @@ -37,9 +37,9 @@ std::array ekfCovarianceToPoseMessageCovariance(const Matrix6d & P) return covariance; } -std::array ekfCovarianceToTwistMessageCovariance(const Matrix6d & P) +std::array ekf_covariance_to_twist_message_covariance(const Matrix6d & P) { - std::array covariance; + std::array covariance{}; covariance.fill(0.); covariance[COV_IDX::X_X] = P(IDX::VX, IDX::VX); diff --git a/localization/ekf_localizer/src/diagnostics.cpp b/localization/ekf_localizer/src/diagnostics.cpp index 9ff36561abaa9..ecbeaf8b2445a 100644 --- a/localization/ekf_localizer/src/diagnostics.cpp +++ b/localization/ekf_localizer/src/diagnostics.cpp @@ -19,7 +19,7 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated) +diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -38,7 +38,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activ return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error) { @@ -69,7 +69,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size( const std::string & measurement_type, const size_t queue_size) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -85,7 +85,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate( const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, const double delay_time_threshold) { @@ -112,7 +112,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate( const std::string & measurement_type, const bool is_passed_mahalanobis_gate, const double mahalanobis_distance, const double mahalanobis_distance_threshold) { @@ -141,7 +141,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( // The highest level within the stat_array will be reflected in the merged_stat. // When all stat_array entries are 'OK,' the message of merged_stat will be "OK" -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array) { diagnostic_msgs::msg::DiagnosticStatus merged_stat; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index f77481d45a534..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 @@ -34,14 +34,14 @@ #include // clang-format off -#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl -#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} +#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl // NOLINT +#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} // NOLINT // clang-format on using std::placeholders::_1; -EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options), +EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("ekf_localizer", node_options), warning_(std::make_shared(this)), params_(this), ekf_dt_(params_.ekf_dt), @@ -58,12 +58,12 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti /* initialize ros system */ timer_control_ = rclcpp::create_timer( this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_), - std::bind(&EKFLocalizer::timerCallback, this)); + std::bind(&EKFLocalizer::timer_callback, this)); if (params_.publish_tf_) { timer_tf_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), - std::bind(&EKFLocalizer::timerTFCallback, this)); + std::bind(&EKFLocalizer::timer_tf_callback, this)); } pub_pose_ = create_publisher("ekf_pose", 1); @@ -79,22 +79,24 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti "ekf_biased_pose_with_covariance", 1); pub_diag_ = this->create_publisher("/diagnostics", 10); sub_initialpose_ = create_subscription( - "initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1)); + "initialpose", 1, std::bind(&EKFLocalizer::callback_initial_pose, this, _1)); sub_pose_with_cov_ = create_subscription( - "in_pose_with_covariance", 1, std::bind(&EKFLocalizer::callbackPoseWithCovariance, this, _1)); + "in_pose_with_covariance", 1, + std::bind(&EKFLocalizer::callback_pose_with_covariance, this, _1)); sub_twist_with_cov_ = create_subscription( - "in_twist_with_covariance", 1, std::bind(&EKFLocalizer::callbackTwistWithCovariance, this, _1)); + "in_twist_with_covariance", 1, + std::bind(&EKFLocalizer::callback_twist_with_covariance, this, _1)); service_trigger_node_ = create_service( "trigger_node_srv", std::bind( - &EKFLocalizer::serviceTriggerNode, this, std::placeholders::_1, std::placeholders::_2), + &EKFLocalizer::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile()); tf_br_ = std::make_shared( 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); @@ -102,9 +104,9 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti } /* - * updatePredictFrequency + * update_predict_frequency */ -void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) +void EKFLocalizer::update_predict_frequency(const rclcpp::Time & current_time) { if (last_predict_time_) { if (current_time < *last_predict_time_) { @@ -119,7 +121,7 @@ void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) ekf_dt_ = 10.0; RCLCPP_WARN( get_logger(), "Large ekf_dt_ detected!! (%f sec) Capped to 10.0 seconds", ekf_dt_); - } else if (ekf_dt_ > params_.pose_smoothing_steps / params_.ekf_rate) { + } else if (ekf_dt_ > static_cast(params_.pose_smoothing_steps) / params_.ekf_rate) { RCLCPP_WARN( get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_); } @@ -137,28 +139,28 @@ void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) } /* - * timerCallback + * timer_callback */ -void EKFLocalizer::timerCallback() +void EKFLocalizer::timer_callback() { const rclcpp::Time current_time = this->now(); if (!is_activated_) { - warning_->warnThrottle( + warning_->warn_throttle( "The node is not activated. Provide initial pose to pose_initializer", 2000); - publishDiagnostics(current_time); + publish_diagnostics(current_time); return; } DEBUG_INFO(get_logger(), "========================= timer called ========================="); /* update predict frequency with measured timer rate */ - updatePredictFrequency(current_time); + update_predict_frequency(current_time); /* predict model in EKF */ stop_watch_.tic(); DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------"); - ekf_module_->predictWithDelay(ekf_dt_); + ekf_module_->predict_with_delay(ekf_dt_); DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); @@ -177,22 +179,22 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = current_time; const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurementUpdatePose(*pose, t_curr, pose_diag_info_); + bool is_updated = ekf_module_->measurement_update_pose(*pose, current_time, pose_diag_info_); if (is_updated) { pose_is_updated = true; // Update Simple 1D filter with considering change of z value due to measurement pose delay const double delay_time = - (t_curr - pose->header.stamp).seconds() + params_.pose_additional_delay; - const auto pose_with_z_delay = ekf_module_->compensatePoseWithZDelay(*pose, delay_time); - updateSimple1DFilters(pose_with_z_delay, params_.pose_smoothing_steps); + (current_time - pose->header.stamp).seconds() + params_.pose_additional_delay; + const auto pose_with_z_delay = ekf_module_->compensate_pose_with_z_delay(*pose, delay_time); + update_simple_1d_filters(pose_with_z_delay, params_.pose_smoothing_steps); } } - DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); + DEBUG_INFO( + get_logger(), "[EKF] measurement_update_pose calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); } pose_diag_info_.no_update_count = pose_is_updated ? 0 : (pose_diag_info_.no_update_count + 1); @@ -212,16 +214,17 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = current_time; const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurementUpdateTwist(*twist, t_curr, twist_diag_info_); + bool is_updated = + ekf_module_->measurement_update_twist(*twist, current_time, twist_diag_info_); if (is_updated) { twist_is_updated = true; } } - DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); + DEBUG_INFO( + get_logger(), "[EKF] measurement_update_twist calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); } twist_diag_info_.no_update_count = twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1); @@ -230,27 +233,27 @@ void EKFLocalizer::timerCallback() const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); const geometry_msgs::msg::PoseStamped current_ekf_pose = - ekf_module_->getCurrentPose(current_time, z, roll, pitch, false); + ekf_module_->get_current_pose(current_time, z, roll, pitch, false); const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = - ekf_module_->getCurrentPose(current_time, z, roll, pitch, true); + ekf_module_->get_current_pose(current_time, z, roll, pitch, true); const geometry_msgs::msg::TwistStamped current_ekf_twist = - ekf_module_->getCurrentTwist(current_time); + ekf_module_->get_current_twist(current_time); /* publish ekf result */ - publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); - publishDiagnostics(current_time); + publish_estimate_result(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); + publish_diagnostics(current_time); } /* - * timerTFCallback + * timer_tf_callback */ -void EKFLocalizer::timerTFCallback() +void EKFLocalizer::timer_tf_callback() { if (!is_activated_) { return; } - if (params_.pose_frame_id == "") { + if (params_.pose_frame_id.empty()) { return; } @@ -261,16 +264,16 @@ void EKFLocalizer::timerTFCallback() const rclcpp::Time current_time = this->now(); geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped = tier4_autoware_utils::pose2transform( - ekf_module_->getCurrentPose(current_time, z, roll, pitch, false), "base_link"); + 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); } /* - * getTransformFromTF + * get_transform_from_tf */ -bool EKFLocalizer::getTransformFromTF( +bool EKFLocalizer::get_transform_from_tf( std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform) { @@ -278,8 +281,8 @@ bool EKFLocalizer::getTransformFromTF( tf2_ros::TransformListener tf_listener(tf_buffer); rclcpp::sleep_for(std::chrono::milliseconds(100)); - parent_frame = eraseLeadingSlash(parent_frame); - child_frame = eraseLeadingSlash(child_frame); + parent_frame = erase_leading_slash(parent_frame); + child_frame = erase_leading_slash(child_frame); for (int i = 0; i < 50; ++i) { try { @@ -294,25 +297,25 @@ bool EKFLocalizer::getTransformFromTF( } /* - * callbackInitialPose + * callback_initial_pose */ -void EKFLocalizer::callbackInitialPose( - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr initialpose) +void EKFLocalizer::callback_initial_pose( + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { geometry_msgs::msg::TransformStamped transform; - if (!getTransformFromTF(params_.pose_frame_id, initialpose->header.frame_id, transform)) { + if (!get_transform_from_tf(params_.pose_frame_id, msg->header.frame_id, transform)) { RCLCPP_ERROR( get_logger(), "[EKF] TF transform failed. parent = %s, child = %s", - params_.pose_frame_id.c_str(), initialpose->header.frame_id.c_str()); + params_.pose_frame_id.c_str(), msg->header.frame_id.c_str()); } - ekf_module_->initialize(*initialpose, transform); - initSimple1DFilters(*initialpose); + ekf_module_->initialize(*msg, transform); + init_simple_1d_filters(*msg); } /* - * callbackPoseWithCovariance + * callback_pose_with_covariance */ -void EKFLocalizer::callbackPoseWithCovariance( +void EKFLocalizer::callback_pose_with_covariance( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { if (!is_activated_) { @@ -323,9 +326,9 @@ void EKFLocalizer::callbackPoseWithCovariance( } /* - * callbackTwistWithCovariance + * callback_twist_with_covariance */ -void EKFLocalizer::callbackTwistWithCovariance( +void EKFLocalizer::callback_twist_with_covariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { // Ignore twist if velocity is too small. @@ -337,9 +340,9 @@ void EKFLocalizer::callbackTwistWithCovariance( } /* - * publishEstimateResult + * publish_estimate_result */ -void EKFLocalizer::publishEstimateResult( +void EKFLocalizer::publish_estimate_result( const geometry_msgs::msg::PoseStamped & current_ekf_pose, const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, const geometry_msgs::msg::TwistStamped & current_ekf_twist) @@ -353,7 +356,7 @@ void EKFLocalizer::publishEstimateResult( pose_cov.header.stamp = current_ekf_pose.header.stamp; pose_cov.header.frame_id = current_ekf_pose.header.frame_id; pose_cov.pose.pose = current_ekf_pose.pose; - pose_cov.pose.covariance = ekf_module_->getCurrentPoseCovariance(); + pose_cov.pose.covariance = ekf_module_->get_current_pose_covariance(); pub_pose_cov_->publish(pose_cov); geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; @@ -368,13 +371,13 @@ void EKFLocalizer::publishEstimateResult( twist_cov.header.stamp = current_ekf_twist.header.stamp; twist_cov.header.frame_id = current_ekf_twist.header.frame_id; twist_cov.twist.twist = current_ekf_twist.twist; - twist_cov.twist.covariance = ekf_module_->getCurrentTwistCovariance(); + twist_cov.twist.covariance = ekf_module_->get_current_twist_covariance(); pub_twist_cov_->publish(twist_cov); /* publish yaw bias */ tier4_debug_msgs::msg::Float64Stamped yawb; yawb.stamp = current_ekf_twist.header.stamp; - yawb.data = ekf_module_->getYawBias(); + yawb.data = ekf_module_->get_yaw_bias(); pub_yaw_bias_->publish(yawb); /* publish latest odometry */ @@ -387,38 +390,38 @@ void EKFLocalizer::publishEstimateResult( pub_odom_->publish(odometry); } -void EKFLocalizer::publishDiagnostics(const rclcpp::Time & current_time) +void EKFLocalizer::publish_diagnostics(const rclcpp::Time & current_time) { std::vector diag_status_array; - diag_status_array.push_back(checkProcessActivated(is_activated_)); + diag_status_array.push_back(check_process_activated(is_activated_)); if (is_activated_) { - diag_status_array.push_back(checkMeasurementUpdated( + diag_status_array.push_back(check_measurement_updated( "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn, params_.pose_no_update_count_threshold_error)); - diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_diag_info_.queue_size)); - diag_status_array.push_back(checkMeasurementDelayGate( + diag_status_array.push_back(check_measurement_queue_size("pose", pose_diag_info_.queue_size)); + diag_status_array.push_back(check_measurement_delay_gate( "pose", pose_diag_info_.is_passed_delay_gate, pose_diag_info_.delay_time, pose_diag_info_.delay_time_threshold)); - diag_status_array.push_back(checkMeasurementMahalanobisGate( + diag_status_array.push_back(check_measurement_mahalanobis_gate( "pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance, params_.pose_gate_dist)); - diag_status_array.push_back(checkMeasurementUpdated( + diag_status_array.push_back(check_measurement_updated( "twist", twist_diag_info_.no_update_count, params_.twist_no_update_count_threshold_warn, params_.twist_no_update_count_threshold_error)); - diag_status_array.push_back(checkMeasurementQueueSize("twist", twist_diag_info_.queue_size)); - diag_status_array.push_back(checkMeasurementDelayGate( + diag_status_array.push_back(check_measurement_queue_size("twist", twist_diag_info_.queue_size)); + diag_status_array.push_back(check_measurement_delay_gate( "twist", twist_diag_info_.is_passed_delay_gate, twist_diag_info_.delay_time, twist_diag_info_.delay_time_threshold)); - diag_status_array.push_back(checkMeasurementMahalanobisGate( + diag_status_array.push_back(check_measurement_mahalanobis_gate( "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance, params_.twist_gate_dist)); } diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; - diag_merged_status = mergeDiagnosticStatus(diag_status_array); + diag_merged_status = merge_diagnostic_status(diag_status_array); diag_merged_status.name = "localization: " + std::string(this->get_name()); diag_merged_status.hardware_id = this->get_name(); @@ -428,14 +431,14 @@ void EKFLocalizer::publishDiagnostics(const rclcpp::Time & current_time) pub_diag_->publish(diag_msg); } -void EKFLocalizer::updateSimple1DFilters( +void EKFLocalizer::update_simple_1d_filters( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step) { 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 = @@ -446,13 +449,14 @@ void EKFLocalizer::updateSimple1DFilters( pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp); } -void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void EKFLocalizer::init_simple_1d_filters( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { 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]; @@ -465,7 +469,7 @@ void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovaria /** * @brief trigger node */ -void EKFLocalizer::serviceTriggerNode( +void EKFLocalizer::service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) { @@ -477,5 +481,7 @@ void EKFLocalizer::serviceTriggerNode( is_activated_ = false; } res->success = true; - return; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(EKFLocalizer) diff --git a/localization/ekf_localizer/src/ekf_localizer_node.cpp b/localization/ekf_localizer/src/ekf_localizer_node.cpp deleted file mode 100644 index 0f75f19c5d50b..0000000000000 --- a/localization/ekf_localizer/src/ekf_localizer_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// 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 "ekf_localizer/ekf_localizer.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("ekf_localizer", node_options); - - rclcpp::spin(node); - - return 0; -} diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 8977d82f34138..ba6b7dedca82c 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -22,66 +22,64 @@ #include "ekf_localizer/state_transition.hpp" #include "ekf_localizer/warning_message.hpp" -#include -#include +#include +#include #include #include #include // clang-format off -#define DEBUG_PRINT_MAT(X) {\ - if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}\ -} +#define DEBUG_PRINT_MAT(X) {if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}} // NOLINT // clang-format on -EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters params) +EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters & params) : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz accumulated_delay_times_(params.extend_state_step, 1.0E15), params_(params) { - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y - P(IDX::YAW, IDX::YAW) = 50.0; // for yaw + Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1); + Eigen::MatrixXd p = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y + p(IDX::YAW, IDX::YAW) = 50.0; // for yaw if (params_.enable_yaw_bias_estimation) { - P(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias + p(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias } - P(IDX::VX, IDX::VX) = 1000.0; // for vx - P(IDX::WZ, IDX::WZ) = 50.0; // for wz + p(IDX::VX, IDX::VX) = 1000.0; // for vx + p(IDX::WZ, IDX::WZ) = 50.0; // for wz - kalman_filter_.init(X, P, params_.extend_state_step); + kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); } void EKFModule::initialize( const PoseWithCovariance & initial_pose, const geometry_msgs::msg::TransformStamped & transform) { - Eigen::MatrixXd X(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); + Eigen::MatrixXd x(dim_x_, 1); + Eigen::MatrixXd p = Eigen::MatrixXd::Zero(dim_x_, dim_x_); - X(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x; - X(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y; - X(IDX::YAW) = + x(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x; + x(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y; + x(IDX::YAW) = tf2::getYaw(initial_pose.pose.pose.orientation) + tf2::getYaw(transform.transform.rotation); - X(IDX::YAWB) = 0.0; - X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; + x(IDX::YAWB) = 0.0; + x(IDX::VX) = 0.0; + x(IDX::WZ) = 0.0; - using COV_IDX = tier4_autoware_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]; + 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]; if (params_.enable_yaw_bias_estimation) { - P(IDX::YAWB, IDX::YAWB) = 0.0001; + p(IDX::YAWB, IDX::YAWB) = 0.0001; } - P(IDX::VX, IDX::VX) = 0.01; - P(IDX::WZ, IDX::WZ) = 0.01; + p(IDX::VX, IDX::VX) = 0.01; + p(IDX::WZ, IDX::WZ) = 0.01; - kalman_filter_.init(X, P, params_.extend_state_step); + kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); } -geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( +geometry_msgs::msg::PoseStamped EKFModule::get_current_pose( const rclcpp::Time & current_time, const double z, const double roll, const double pitch, bool get_biased_yaw) const { @@ -99,18 +97,19 @@ geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( 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; } -geometry_msgs::msg::TwistStamped EKFModule::getCurrentTwist(const rclcpp::Time & current_time) const +geometry_msgs::msg::TwistStamped EKFModule::get_current_twist( + const rclcpp::Time & current_time) const { const double vx = kalman_filter_.getXelement(IDX::VX); const double wz = kalman_filter_.getXelement(IDX::WZ); @@ -123,17 +122,17 @@ geometry_msgs::msg::TwistStamped EKFModule::getCurrentTwist(const rclcpp::Time & return current_ekf_twist; } -std::array EKFModule::getCurrentPoseCovariance() const +std::array EKFModule::get_current_pose_covariance() const { - return ekfCovarianceToPoseMessageCovariance(kalman_filter_.getLatestP()); + return ekf_covariance_to_pose_message_covariance(kalman_filter_.getLatestP()); } -std::array EKFModule::getCurrentTwistCovariance() const +std::array EKFModule::get_current_twist_covariance() const { - return ekfCovarianceToTwistMessageCovariance(kalman_filter_.getLatestP()); + return ekf_covariance_to_twist_message_covariance(kalman_filter_.getLatestP()); } -double EKFModule::getYawBias() const +double EKFModule::get_yaw_bias() const { return kalman_filter_.getLatestX()(IDX::YAWB); } @@ -153,17 +152,17 @@ size_t EKFModule::find_closest_delay_time_index(double target_value) const // If else, take the closest element. if (lower == accumulated_delay_times_.begin()) { return 0; - } else if (lower == accumulated_delay_times_.end()) { + } + if (lower == accumulated_delay_times_.end()) { return accumulated_delay_times_.size() - 1; - } else { - // Compare the target with the lower bound and the previous element. - auto prev = lower - 1; - bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); - - // Return the index of the closer element. - return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) - : std::distance(accumulated_delay_times_.begin(), lower); } + // Compare the target with the lower bound and the previous element. + auto prev = lower - 1; + bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); + + // Return the index of the closer element. + return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) + : std::distance(accumulated_delay_times_.begin(), lower); } void EKFModule::accumulate_delay_time(const double dt) @@ -180,69 +179,70 @@ void EKFModule::accumulate_delay_time(const double dt) } } -void EKFModule::predictWithDelay(const double dt) +void EKFModule::predict_with_delay(const double dt) { - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); const double proc_cov_vx_d = std::pow(params_.proc_stddev_vx_c * dt, 2.0); const double proc_cov_wz_d = std::pow(params_.proc_stddev_wz_c * dt, 2.0); const double proc_cov_yaw_d = std::pow(params_.proc_stddev_yaw_c * dt, 2.0); - const Vector6d X_next = predictNextState(X_curr, dt); - const Matrix6d A = createStateTransitionMatrix(X_curr, dt); - const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d); - kalman_filter_.predictWithDelay(X_next, A, Q); + const Vector6d x_next = predict_next_state(x_curr, dt); + const Matrix6d a = create_state_transition_matrix(x_curr, dt); + const Matrix6d q = process_noise_covariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d); + kalman_filter_.predictWithDelay(x_next, a, q); } -bool EKFModule::measurementUpdatePose( +bool EKFModule::measurement_update_pose( const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info) { if (pose.header.frame_id != params_.pose_frame_id) { - warning_->warnThrottle( + warning_->warn_throttle( fmt::format( "pose frame_id is %s, but pose_frame is set as %s. They must be same.", pose.header.frame_id.c_str(), params_.pose_frame_id.c_str()), 2000); } - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_curr.transpose()); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_curr.transpose()); constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output /* Calculate delay step */ double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay; if (delay_time < 0.0) { - warning_->warnThrottle(poseDelayTimeWarningMessage(delay_time), 1000); + warning_->warn_throttle(pose_delay_time_warning_message(delay_time), 1000); } delay_time = std::max(delay_time, 0.0); - const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); + const size_t delay_step = find_closest_delay_time_index(delay_time); pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time); pose_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { pose_diag_info.is_passed_delay_gate = false; - warning_->warnThrottle( - poseDelayStepWarningMessage(pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), + warning_->warn_throttle( + pose_delay_step_warning_message( + pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), 2000); return false; } /* Since the kalman filter cannot handle the rotation angle directly, - offset the yaw angle so that the difference from the yaw angle that ekf holds internally is less - than 2 pi. */ + offset the yaw angle so that the difference from the yaw angle that ekf holds internally + is less than 2 pi. */ double yaw = tf2::getYaw(pose.pose.pose.orientation); const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW); - const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi + const double yaw_error = normalize_yaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi yaw = yaw_error + ekf_yaw; /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw; - if (hasNan(y) || hasInf(y)) { + if (has_nan(y) || has_inf(y)) { warning_->warn( "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); return false; @@ -252,15 +252,15 @@ bool EKFModule::measurementUpdatePose( const Eigen::Vector3d y_ekf( kalman_filter_.getXelement(delay_step * dim_x_ + IDX::X), kalman_filter_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); - const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd p_y = p_curr.block(0, 0, dim_y, dim_y); - const double distance = mahalanobis(y_ekf, y, P_y); + const double distance = mahalanobis(y_ekf, y, p_y); pose_diag_info.mahalanobis_distance = std::max(distance, pose_diag_info.mahalanobis_distance); if (distance > params_.pose_gate_dist) { pose_diag_info.is_passed_mahalanobis_gate = false; - warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000); - warning_->warnThrottle("Ignore the measurement data.", 2000); + warning_->warn_throttle(mahalanobis_warning_message(distance, params_.pose_gate_dist), 2000); + warning_->warn_throttle("Ignore the measurement data.", 2000); return false; } @@ -268,24 +268,24 @@ bool EKFModule::measurementUpdatePose( DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); - const Eigen::Matrix C = poseMeasurementMatrix(); - const Eigen::Matrix3d R = - poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps); + const Eigen::Matrix c = pose_measurement_matrix(); + const Eigen::Matrix3d r = + pose_measurement_covariance(pose.pose.covariance, params_.pose_smoothing_steps); - kalman_filter_.updateWithDelay(y, C, R, delay_step); + kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); // debug - const Eigen::MatrixXd X_result = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_result.transpose()); + DEBUG_PRINT_MAT((x_result - x_curr).transpose()); return true; } -geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDelay( +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; @@ -293,34 +293,34 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDela return pose_with_z_delay; } -bool EKFModule::measurementUpdateTwist( +bool EKFModule::measurement_update_twist( const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info) { if (twist.header.frame_id != "base_link") { - warning_->warnThrottle("twist frame_id must be base_link", 2000); + warning_->warn_throttle("twist frame_id must be base_link", 2000); } - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_curr.transpose()); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_curr.transpose()); constexpr int dim_y = 2; // vx, wz /* Calculate delay step */ double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay; if (delay_time < 0.0) { - warning_->warnThrottle(twistDelayTimeWarningMessage(delay_time), 1000); + warning_->warn_throttle(twist_delay_time_warning_message(delay_time), 1000); } delay_time = std::max(delay_time, 0.0); - const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); + const size_t delay_step = find_closest_delay_time_index(delay_time); twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time); twist_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { twist_diag_info.is_passed_delay_gate = false; - warning_->warnThrottle( - twistDelayStepWarningMessage( + warning_->warn_throttle( + twist_delay_step_warning_message( twist_diag_info.delay_time, twist_diag_info.delay_time_threshold), 2000); return false; @@ -330,7 +330,7 @@ bool EKFModule::measurementUpdateTwist( Eigen::MatrixXd y(dim_y, 1); y << twist.twist.twist.linear.x, twist.twist.twist.angular.z; - if (hasNan(y) || hasInf(y)) { + if (has_nan(y) || has_inf(y)) { warning_->warn( "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); return false; @@ -339,15 +339,15 @@ bool EKFModule::measurementUpdateTwist( const Eigen::Vector2d y_ekf( kalman_filter_.getXelement(delay_step * dim_x_ + IDX::VX), kalman_filter_.getXelement(delay_step * dim_x_ + IDX::WZ)); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); - const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd p_y = p_curr.block(4, 4, dim_y, dim_y); - const double distance = mahalanobis(y_ekf, y, P_y); + const double distance = mahalanobis(y_ekf, y, p_y); twist_diag_info.mahalanobis_distance = std::max(distance, twist_diag_info.mahalanobis_distance); if (distance > params_.twist_gate_dist) { twist_diag_info.is_passed_mahalanobis_gate = false; - warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000); - warning_->warnThrottle("Ignore the measurement data.", 2000); + warning_->warn_throttle(mahalanobis_warning_message(distance, params_.twist_gate_dist), 2000); + warning_->warn_throttle("Ignore the measurement data.", 2000); return false; } @@ -355,16 +355,16 @@ bool EKFModule::measurementUpdateTwist( DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); - const Eigen::Matrix C = twistMeasurementMatrix(); - const Eigen::Matrix2d R = - twistMeasurementCovariance(twist.twist.covariance, params_.twist_smoothing_steps); + const Eigen::Matrix c = twist_measurement_matrix(); + const Eigen::Matrix2d r = + twist_measurement_covariance(twist.twist.covariance, params_.twist_smoothing_steps); - kalman_filter_.updateWithDelay(y, C, R, delay_step); + kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); // debug - const Eigen::MatrixXd X_result = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_result.transpose()); + DEBUG_PRINT_MAT((x_result - x_curr).transpose()); return true; } diff --git a/localization/ekf_localizer/src/mahalanobis.cpp b/localization/ekf_localizer/src/mahalanobis.cpp index ff5ef13892b73..a71482ab696f6 100644 --- a/localization/ekf_localizer/src/mahalanobis.cpp +++ b/localization/ekf_localizer/src/mahalanobis.cpp @@ -14,7 +14,7 @@ #include "ekf_localizer/mahalanobis.hpp" -double squaredMahalanobis( +double squared_mahalanobis( const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) { const Eigen::VectorXd d = x - y; @@ -23,5 +23,5 @@ double squaredMahalanobis( double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) { - return std::sqrt(squaredMahalanobis(x, y, C)); + return std::sqrt(squared_mahalanobis(x, y, C)); } diff --git a/localization/ekf_localizer/src/measurement.cpp b/localization/ekf_localizer/src/measurement.cpp index 0b3e65bd60f5d..faf8768938b7d 100644 --- a/localization/ekf_localizer/src/measurement.cpp +++ b/localization/ekf_localizer/src/measurement.cpp @@ -14,43 +14,43 @@ #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 poseMeasurementMatrix() +Eigen::Matrix pose_measurement_matrix() { - Eigen::Matrix C = Eigen::Matrix::Zero(); - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - return C; + Eigen::Matrix c = Eigen::Matrix::Zero(); + c(0, IDX::X) = 1.0; // for pos x + c(1, IDX::Y) = 1.0; // for pos y + c(2, IDX::YAW) = 1.0; // for yaw + return c; } -Eigen::Matrix twistMeasurementMatrix() +Eigen::Matrix twist_measurement_matrix() { - Eigen::Matrix C = Eigen::Matrix::Zero(); - C(0, IDX::VX) = 1.0; // for vx - C(1, IDX::WZ) = 1.0; // for wz - return C; + Eigen::Matrix c = Eigen::Matrix::Zero(); + c(0, IDX::VX) = 1.0; // for vx + c(1, IDX::WZ) = 1.0; // for wz + return c; } -Eigen::Matrix3d poseMeasurementCovariance( +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; - R << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), + Eigen::Matrix3d r; + 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); - return R * static_cast(smoothing_step); + return r * static_cast(smoothing_step); } -Eigen::Matrix2d twistMeasurementCovariance( +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; - R << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), + Eigen::Matrix2d r; + 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); + return r * static_cast(smoothing_step); } diff --git a/localization/ekf_localizer/src/state_transition.cpp b/localization/ekf_localizer/src/state_transition.cpp index 88fc9f76d7168..22b1f2f8a1c67 100644 --- a/localization/ekf_localizer/src/state_transition.cpp +++ b/localization/ekf_localizer/src/state_transition.cpp @@ -17,7 +17,7 @@ #include -double normalizeYaw(const double & yaw) +double normalize_yaw(const double & yaw) { // FIXME(IshitaTakeshi) I think the computation here can be simplified // FIXME(IshitaTakeshi) Rename the function. This is not normalization @@ -36,7 +36,7 @@ double normalizeYaw(const double & yaw) * (b_k : yaw_bias_k) */ -Vector6d predictNextState(const Vector6d & X_curr, const double dt) +Vector6d predict_next_state(const Vector6d & X_curr, const double dt) { const double x = X_curr(IDX::X); const double y = X_curr(IDX::Y); @@ -45,14 +45,14 @@ Vector6d predictNextState(const Vector6d & X_curr, const double dt) const double vx = X_curr(IDX::VX); const double wz = X_curr(IDX::WZ); - Vector6d X_next; - X_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) - X_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) - X_next(IDX::YAW) = normalizeYaw(yaw + wz * dt); // dyaw = omega + omega_bias - X_next(IDX::YAWB) = yaw_bias; - X_next(IDX::VX) = vx; - X_next(IDX::WZ) = wz; - return X_next; + Vector6d x_next; + x_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) + x_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) + x_next(IDX::YAW) = normalize_yaw(yaw + wz * dt); // dyaw = omega + omega_bias + x_next(IDX::YAWB) = yaw_bias; + x_next(IDX::VX) = vx; + x_next(IDX::WZ) = wz; + return x_next; } /* == Linearized model == @@ -64,32 +64,32 @@ Vector6d predictNextState(const Vector6d & X_curr, const double dt) * [ 0, 0, 0, 0, 1, 0] * [ 0, 0, 0, 0, 0, 1] */ -Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt) +Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt) { const double yaw = X_curr(IDX::YAW); const double yaw_bias = X_curr(IDX::YAWB); const double vx = X_curr(IDX::VX); - Matrix6d A = Matrix6d::Identity(); - A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; - A(IDX::YAW, IDX::WZ) = dt; - return A; + Matrix6d a = Matrix6d::Identity(); + a(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; + a(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; + a(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; + a(IDX::YAW, IDX::WZ) = dt; + return a; } -Matrix6d processNoiseCovariance( +Matrix6d process_noise_covariance( const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d) { - Matrix6d Q = Matrix6d::Zero(); - Q(IDX::X, IDX::X) = 0.0; - Q(IDX::Y, IDX::Y) = 0.0; - Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d; // for yaw - Q(IDX::YAWB, IDX::YAWB) = 0.0; - Q(IDX::VX, IDX::VX) = proc_cov_vx_d; // for vx - Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d; // for wz - return Q; + Matrix6d q = Matrix6d::Zero(); + q(IDX::X, IDX::X) = 0.0; + q(IDX::Y, IDX::Y) = 0.0; + q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d; // for yaw + q(IDX::YAWB, IDX::YAWB) = 0.0; + q(IDX::VX, IDX::VX) = proc_cov_vx_d; // for vx + q(IDX::WZ, IDX::WZ) = proc_cov_wz_d; // for wz + return q; } diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index c69f30b2d6601..ae74c6bfb5fbc 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -18,7 +18,8 @@ #include -std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) +std::string pose_delay_step_warning_message( + const double delay_time, const double delay_time_threshold) { const std::string s = "Pose delay exceeds the compensation limit, ignored. " @@ -26,7 +27,8 @@ std::string poseDelayStepWarningMessage(const double delay_time, const double de return fmt::format(s, delay_time, delay_time_threshold); } -std::string twistDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) +std::string twist_delay_step_warning_message( + const double delay_time, const double delay_time_threshold) { const std::string s = "Twist delay exceeds the compensation limit, ignored. " @@ -34,19 +36,19 @@ std::string twistDelayStepWarningMessage(const double delay_time, const double d return fmt::format(s, delay_time, delay_time_threshold); } -std::string poseDelayTimeWarningMessage(const double delay_time) +std::string pose_delay_time_warning_message(const double delay_time) { const std::string s = "Pose time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; return fmt::format(s, delay_time); } -std::string twistDelayTimeWarningMessage(const double delay_time) +std::string twist_delay_time_warning_message(const double delay_time) { const std::string s = "Twist time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; return fmt::format(s, delay_time); } -std::string mahalanobisWarningMessage(const double distance, const double max_distance) +std::string mahalanobis_warning_message(const double distance, const double max_distance) { const std::string s = "The Mahalanobis distance {:.4f} is over the limit {:.4f}."; return fmt::format(s, distance, max_distance); diff --git a/localization/ekf_localizer/test/test_covariance.cpp b/localization/ekf_localizer/test/test_covariance.cpp index 23458fb18a838..ed87ebdea029b 100644 --- a/localization/ekf_localizer/test/test_covariance.cpp +++ b/localization/ekf_localizer/test/test_covariance.cpp @@ -19,18 +19,18 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) { { - Matrix6d P = Matrix6d::Zero(); - P(0, 0) = 1.; - P(0, 1) = 2.; - P(0, 2) = 3.; - P(1, 0) = 4.; - P(1, 1) = 5.; - P(1, 2) = 6.; - P(2, 0) = 7.; - P(2, 1) = 8.; - P(2, 2) = 9.; + Matrix6d p = Matrix6d::Zero(); + p(0, 0) = 1.; + p(0, 1) = 2.; + p(0, 2) = 3.; + p(1, 0) = 4.; + p(1, 1) = 5.; + p(1, 2) = 6.; + p(2, 0) = 7.; + p(2, 1) = 8.; + p(2, 2) = 9.; - std::array covariance = ekfCovarianceToPoseMessageCovariance(P); + std::array covariance = ekf_covariance_to_pose_message_covariance(p); EXPECT_EQ(covariance[0], 1.); EXPECT_EQ(covariance[1], 2.); EXPECT_EQ(covariance[5], 3.); @@ -44,8 +44,8 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) // ensure other elements are zero { - Matrix6d P = Matrix6d::Zero(); - std::array covariance = ekfCovarianceToPoseMessageCovariance(P); + Matrix6d p = Matrix6d::Zero(); + std::array covariance = ekf_covariance_to_pose_message_covariance(p); for (double e : covariance) { EXPECT_EQ(e, 0.); } @@ -55,13 +55,13 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) { { - Matrix6d P = Matrix6d::Zero(); - P(4, 4) = 1.; - P(4, 5) = 2.; - P(5, 4) = 3.; - P(5, 5) = 4.; + Matrix6d p = Matrix6d::Zero(); + p(4, 4) = 1.; + p(4, 5) = 2.; + p(5, 4) = 3.; + p(5, 5) = 4.; - std::array covariance = ekfCovarianceToTwistMessageCovariance(P); + std::array covariance = ekf_covariance_to_twist_message_covariance(p); EXPECT_EQ(covariance[0], 1.); EXPECT_EQ(covariance[5], 2.); EXPECT_EQ(covariance[30], 3.); @@ -70,8 +70,8 @@ TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) // ensure other elements are zero { - Matrix6d P = Matrix6d::Zero(); - std::array covariance = ekfCovarianceToTwistMessageCovariance(P); + Matrix6d p = Matrix6d::Zero(); + std::array covariance = ekf_covariance_to_twist_message_covariance(p); for (double e : covariance) { EXPECT_EQ(e, 0.); } diff --git a/localization/ekf_localizer/test/test_diagnostics.cpp b/localization/ekf_localizer/test/test_diagnostics.cpp index f506dca1cb230..ef0d7a6493756 100644 --- a/localization/ekf_localizer/test/test_diagnostics.cpp +++ b/localization/ekf_localizer/test/test_diagnostics.cpp @@ -16,20 +16,20 @@ #include -TEST(TestEkfDiagnostics, CheckProcessActivated) +TEST(TestEkfDiagnostics, check_process_activated) { diagnostic_msgs::msg::DiagnosticStatus stat; bool is_activated = true; - stat = checkProcessActivated(is_activated); + stat = check_process_activated(is_activated); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); is_activated = false; - stat = checkProcessActivated(is_activated); + stat = check_process_activated(is_activated); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestEkfDiagnostics, checkMeasurementUpdated) +TEST(TestEkfDiagnostics, check_measurement_updated) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -38,58 +38,58 @@ TEST(TestEkfDiagnostics, checkMeasurementUpdated) const size_t no_update_count_threshold_error = 250; size_t no_update_count = 0; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); no_update_count = 1; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); no_update_count = 49; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); no_update_count = 50; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); no_update_count = 249; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); no_update_count = 250; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); } -TEST(TestEkfDiagnostics, CheckMeasurementQueueSize) +TEST(TestEkfDiagnostics, check_measurement_queue_size) { diagnostic_msgs::msg::DiagnosticStatus stat; const std::string measurement_type = "pose"; // not effect for stat.level size_t queue_size = 0; // not effect for stat.level - stat = checkMeasurementQueueSize(measurement_type, queue_size); + stat = check_measurement_queue_size(measurement_type, queue_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); queue_size = 1; // not effect for stat.level - stat = checkMeasurementQueueSize(measurement_type, queue_size); + stat = check_measurement_queue_size(measurement_type, queue_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); } -TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) +TEST(TestEkfDiagnostics, check_measurement_delay_gate) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -98,17 +98,17 @@ TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) const double delay_time_threshold = 1.0; // not effect for stat.level bool is_passed_delay_gate = true; - stat = checkMeasurementDelayGate( + stat = check_measurement_delay_gate( measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); is_passed_delay_gate = false; - stat = checkMeasurementDelayGate( + stat = check_measurement_delay_gate( measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) +TEST(TestEkfDiagnostics, check_measurement_mahalanobis_gate) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -117,19 +117,19 @@ TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) const double mahalanobis_distance_threshold = 1.0; // not effect for stat.level bool is_passed_mahalanobis_gate = true; - stat = checkMeasurementMahalanobisGate( + stat = check_measurement_mahalanobis_gate( measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, mahalanobis_distance_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); is_passed_mahalanobis_gate = false; - stat = checkMeasurementMahalanobisGate( + stat = check_measurement_mahalanobis_gate( measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, mahalanobis_distance_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) +TEST(TestLocalizationErrorMonitorDiagnostics, merge_diagnostic_status) { diagnostic_msgs::msg::DiagnosticStatus merged_stat; std::vector stat_array(2); @@ -138,7 +138,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat_array.at(1).message = "OK"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); EXPECT_EQ(merged_stat.message, "OK"); @@ -146,7 +146,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat_array.at(1).message = "OK"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0"); @@ -154,7 +154,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat_array.at(1).message = "WARN1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN1"); @@ -162,7 +162,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat_array.at(1).message = "WARN1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); @@ -170,7 +170,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR1"); @@ -178,7 +178,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); @@ -186,7 +186,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "ERROR0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); } diff --git a/localization/ekf_localizer/test/test_mahalanobis.cpp b/localization/ekf_localizer/test/test_mahalanobis.cpp index d208c1e8da06b..db7d538b533c3 100644 --- a/localization/ekf_localizer/test/test_mahalanobis.cpp +++ b/localization/ekf_localizer/test/test_mahalanobis.cpp @@ -18,44 +18,44 @@ constexpr double tolerance = 1e-8; -TEST(SquaredMahalanobis, SmokeTest) +TEST(squared_mahalanobis, SmokeTest) { { Eigen::Vector2d x(0, 1); Eigen::Vector2d y(3, 2); - Eigen::Matrix2d C; - C << 10, 0, 0, 10; + Eigen::Matrix2d c; + c << 10, 0, 0, 10; - EXPECT_NEAR(squaredMahalanobis(x, y, C), 1.0, tolerance); + EXPECT_NEAR(squared_mahalanobis(x, y, c), 1.0, tolerance); } { Eigen::Vector2d x(4, 1); Eigen::Vector2d y(1, 5); - Eigen::Matrix2d C; - C << 5, 0, 0, 5; + Eigen::Matrix2d c; + c << 5, 0, 0, 5; - EXPECT_NEAR(squaredMahalanobis(x, y, C), 5.0, tolerance); + EXPECT_NEAR(squared_mahalanobis(x, y, c), 5.0, tolerance); } } -TEST(Mahalanobis, SmokeTest) +TEST(mahalanobis, SmokeTest) { { Eigen::Vector2d x(0, 1); Eigen::Vector2d y(3, 2); - Eigen::Matrix2d C; - C << 10, 0, 0, 10; + Eigen::Matrix2d c; + c << 10, 0, 0, 10; - EXPECT_NEAR(mahalanobis(x, y, C), 1.0, tolerance); + EXPECT_NEAR(mahalanobis(x, y, c), 1.0, tolerance); } { Eigen::Vector2d x(4, 1); Eigen::Vector2d y(1, 5); - Eigen::Matrix2d C; - C << 5, 0, 0, 5; + Eigen::Matrix2d c; + c << 5, 0, 0, 5; - EXPECT_NEAR(mahalanobis(x, y, C), std::sqrt(5.0), tolerance); + EXPECT_NEAR(mahalanobis(x, y, c), std::sqrt(5.0), tolerance); } } diff --git a/localization/ekf_localizer/test/test_measurement.cpp b/localization/ekf_localizer/test/test_measurement.cpp index 9d63cb056d9d3..c77e39a67d15c 100644 --- a/localization/ekf_localizer/test/test_measurement.cpp +++ b/localization/ekf_localizer/test/test_measurement.cpp @@ -16,66 +16,66 @@ #include -TEST(Measurement, PoseMeasurementMatrix) +TEST(Measurement, pose_measurement_matrix) { - const Eigen::Matrix M = poseMeasurementMatrix(); + const Eigen::Matrix m = pose_measurement_matrix(); Eigen::Matrix expected; expected << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0; - EXPECT_EQ((M - expected).norm(), 0); + EXPECT_EQ((m - expected).norm(), 0); } -TEST(Measurement, TwistMeasurementMatrix) +TEST(Measurement, twist_measurement_matrix) { - const Eigen::Matrix M = twistMeasurementMatrix(); + const Eigen::Matrix m = twist_measurement_matrix(); Eigen::Matrix expected; expected << 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; - EXPECT_EQ((M - expected).norm(), 0); + EXPECT_EQ((m - expected).norm(), 0); } -TEST(Measurement, PoseMeasurementCovariance) +TEST(Measurement, pose_measurement_covariance) { { const std::array covariance = {1, 2, 0, 0, 0, 3, 4, 5, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 7, 8, 0, 0, 0, 9}; - const Eigen::Matrix3d M = poseMeasurementCovariance(covariance, 2); + const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2); Eigen::Matrix3d expected; expected << 2, 4, 6, 8, 10, 12, 14, 16, 18; - EXPECT_EQ((M - expected).norm(), 0.); + EXPECT_EQ((m - expected).norm(), 0.); } { // Make sure that other elements are not changed - std::array covariance; + std::array covariance{}; covariance.fill(0); - const Eigen::Matrix3d M = poseMeasurementCovariance(covariance, 2.); - EXPECT_EQ(M.norm(), 0); + const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2.); + EXPECT_EQ(m.norm(), 0); } } -TEST(Measurement, TwistMeasurementCovariance) +TEST(Measurement, twist_measurement_covariance) { { const std::array covariance = {1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 4}; - const Eigen::Matrix2d M = twistMeasurementCovariance(covariance, 2); + const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2); Eigen::Matrix2d expected; expected << 2, 4, 6, 8; - EXPECT_EQ((M - expected).norm(), 0.); + EXPECT_EQ((m - expected).norm(), 0.); } { // Make sure that other elements are not changed - std::array covariance; + std::array covariance{}; covariance.fill(0); - const Eigen::Matrix2d M = twistMeasurementCovariance(covariance, 2.); - EXPECT_EQ(M.norm(), 0); + const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2.); + EXPECT_EQ(m.norm(), 0); } } diff --git a/localization/ekf_localizer/test/test_numeric.cpp b/localization/ekf_localizer/test/test_numeric.cpp index baf4f46db79a9..f84c9aa1d0bf2 100644 --- a/localization/ekf_localizer/test/test_numeric.cpp +++ b/localization/ekf_localizer/test/test_numeric.cpp @@ -18,30 +18,30 @@ #include -TEST(Numeric, HasNan) +TEST(Numeric, has_nan) { const Eigen::VectorXd empty(0); const double inf = std::numeric_limits::infinity(); const double nan = std::nan(""); - EXPECT_FALSE(hasNan(empty)); - EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 0., 1.))); - EXPECT_FALSE(hasNan(Eigen::Vector3d(1e16, 0., 1.))); - EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 1., inf))); + EXPECT_FALSE(has_nan(empty)); + EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 0., 1.))); + EXPECT_FALSE(has_nan(Eigen::Vector3d(1e16, 0., 1.))); + EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 1., inf))); - EXPECT_TRUE(hasNan(Eigen::Vector3d(nan, 1., 0.))); + EXPECT_TRUE(has_nan(Eigen::Vector3d(nan, 1., 0.))); } -TEST(Numeric, HasInf) +TEST(Numeric, has_inf) { const Eigen::VectorXd empty(0); const double inf = std::numeric_limits::infinity(); const double nan = std::nan(""); - EXPECT_FALSE(hasInf(empty)); - EXPECT_FALSE(hasInf(Eigen::Vector3d(0., 0., 1.))); - EXPECT_FALSE(hasInf(Eigen::Vector3d(1e16, 0., 1.))); - EXPECT_FALSE(hasInf(Eigen::Vector3d(nan, 1., 0.))); + EXPECT_FALSE(has_inf(empty)); + EXPECT_FALSE(has_inf(Eigen::Vector3d(0., 0., 1.))); + EXPECT_FALSE(has_inf(Eigen::Vector3d(1e16, 0., 1.))); + EXPECT_FALSE(has_inf(Eigen::Vector3d(nan, 1., 0.))); - EXPECT_TRUE(hasInf(Eigen::Vector3d(0., 1., inf))); + EXPECT_TRUE(has_inf(Eigen::Vector3d(0., 1., inf))); } diff --git a/localization/ekf_localizer/test/test_state_transition.cpp b/localization/ekf_localizer/test/test_state_transition.cpp index 9cb7975a964a9..3b6bd93fd3dd8 100644 --- a/localization/ekf_localizer/test/test_state_transition.cpp +++ b/localization/ekf_localizer/test/test_state_transition.cpp @@ -17,43 +17,44 @@ #include "ekf_localizer/state_transition.hpp" #include -#include -TEST(StateTransition, NormalizeYaw) +#include + +TEST(StateTransition, normalize_yaw) { const double tolerance = 1e-6; - EXPECT_NEAR(normalizeYaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance); - EXPECT_NEAR(normalizeYaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance); - EXPECT_NEAR(normalizeYaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance); - EXPECT_NEAR(normalizeYaw(M_PI * 4), M_PI * 0, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalize_yaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 4), M_PI * 0, tolerance); } -TEST(PredictNextState, PredictNextState) +TEST(predict_next_state, predict_next_state) { // This function is the definition of state transition so we just check // if the calculation is done according to the formula - Vector6d X_curr; - X_curr(0) = 2.; - X_curr(1) = 3.; - X_curr(2) = M_PI / 2.; - X_curr(3) = M_PI / 4.; - X_curr(4) = 10.; - X_curr(5) = 2. * M_PI / 3.; + Vector6d x_curr; + x_curr(0) = 2.; + x_curr(1) = 3.; + x_curr(2) = M_PI / 2.; + x_curr(3) = M_PI / 4.; + x_curr(4) = 10.; + x_curr(5) = 2. * M_PI / 3.; const double dt = 0.5; - const Vector6d X_next = predictNextState(X_curr, dt); + const Vector6d x_next = predict_next_state(x_curr, dt); const double tolerance = 1e-10; - EXPECT_NEAR(X_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); - EXPECT_NEAR(X_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); - EXPECT_NEAR(X_next(2), normalizeYaw(M_PI / 2. + M_PI / 3.), tolerance); - EXPECT_NEAR(X_next(3), X_curr(3), tolerance); - EXPECT_NEAR(X_next(4), X_curr(4), tolerance); - EXPECT_NEAR(X_next(5), X_curr(5), tolerance); + EXPECT_NEAR(x_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(x_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(x_next(2), normalize_yaw(M_PI / 2. + M_PI / 3.), tolerance); + EXPECT_NEAR(x_next(3), x_curr(3), tolerance); + EXPECT_NEAR(x_next(4), x_curr(4), tolerance); + EXPECT_NEAR(x_next(5), x_curr(5), tolerance); } -TEST(CreateStateTransitionMatrix, NumericalApproximation) +TEST(create_state_transition_matrix, NumericalApproximation) { // The transition matrix A = df / dx // We check if df = A * dx approximates f(x + dx) - f(x) @@ -64,10 +65,10 @@ TEST(CreateStateTransitionMatrix, NumericalApproximation) const Vector6d dx = 0.1 * Vector6d::Ones(); const Vector6d x = Vector6d::Zero(); - const Matrix6d A = createStateTransitionMatrix(x, dt); - const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + const Matrix6d a = create_state_transition_matrix(x, dt); + const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt); - EXPECT_LT((df - A * dx).norm(), 2e-3); + EXPECT_LT((df - a * dx).norm(), 2e-3); } { @@ -76,20 +77,20 @@ TEST(CreateStateTransitionMatrix, NumericalApproximation) const Vector6d dx = 0.1 * Vector6d::Ones(); const Vector6d x = (Vector6d() << 0.1, 0.2, 0.1, 0.4, 0.1, 0.3).finished(); - const Matrix6d A = createStateTransitionMatrix(x, dt); - const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + const Matrix6d a = create_state_transition_matrix(x, dt); + const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt); - EXPECT_LT((df - A * dx).norm(), 5e-3); + EXPECT_LT((df - a * dx).norm(), 5e-3); } } -TEST(ProcessNoiseCovariance, ProcessNoiseCovariance) +TEST(process_noise_covariance, process_noise_covariance) { - const Matrix6d Q = processNoiseCovariance(1., 2., 3.); - EXPECT_EQ(Q(2, 2), 1.); // for yaw - EXPECT_EQ(Q(4, 4), 2.); // for vx - EXPECT_EQ(Q(5, 5), 3.); // for wz + const Matrix6d q = process_noise_covariance(1., 2., 3.); + EXPECT_EQ(q(2, 2), 1.); // for yaw + EXPECT_EQ(q(4, 4), 2.); // for vx + EXPECT_EQ(q(5, 5), 3.); // for wz // Make sure other elements are zero - EXPECT_EQ(processNoiseCovariance(0, 0, 0).norm(), 0.); + EXPECT_EQ(process_noise_covariance(0, 0, 0).norm(), 0.); } diff --git a/localization/ekf_localizer/test/test_string.cpp b/localization/ekf_localizer/test/test_string.cpp index 7b35a56d889e9..013072e5145c7 100644 --- a/localization/ekf_localizer/test/test_string.cpp +++ b/localization/ekf_localizer/test/test_string.cpp @@ -16,11 +16,11 @@ #include -TEST(EraseLeadingSlash, SmokeTest) +TEST(erase_leading_slash, SmokeTest) { - EXPECT_EQ(eraseLeadingSlash("/topic"), "topic"); - EXPECT_EQ(eraseLeadingSlash("topic"), "topic"); // do nothing + EXPECT_EQ(erase_leading_slash("/topic"), "topic"); + EXPECT_EQ(erase_leading_slash("topic"), "topic"); // do nothing - EXPECT_EQ(eraseLeadingSlash(""), ""); - EXPECT_EQ(eraseLeadingSlash("/"), ""); + EXPECT_EQ(erase_leading_slash(""), ""); + EXPECT_EQ(erase_leading_slash("/"), ""); } diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index 2069969e0ae5e..ec7d3420d7f79 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -18,45 +18,45 @@ #include -TEST(PoseDelayStepWarningMessage, SmokeTest) +TEST(pose_delay_step_warning_message, SmokeTest) { EXPECT_STREQ( - poseDelayStepWarningMessage(6.0, 4.0).c_str(), + pose_delay_step_warning_message(6.0, 4.0).c_str(), "Pose delay exceeds the compensation limit, ignored. " "delay: 6.000[s], limit: 4.000[s]"); } -TEST(TwistDelayStepWarningMessage, SmokeTest) +TEST(twist_delay_step_warning_message, SmokeTest) { EXPECT_STREQ( - twistDelayStepWarningMessage(10.0, 6.0).c_str(), + twist_delay_step_warning_message(10.0, 6.0).c_str(), "Twist delay exceeds the compensation limit, ignored. " "delay: 10.000[s], limit: 6.000[s]"); } -TEST(PoseDelayTimeWarningMessage, SmokeTest) +TEST(pose_delay_time_warning_message, SmokeTest) { EXPECT_STREQ( - poseDelayTimeWarningMessage(-1.0).c_str(), + pose_delay_time_warning_message(-1.0).c_str(), "Pose time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); EXPECT_STREQ( - poseDelayTimeWarningMessage(-0.4).c_str(), + pose_delay_time_warning_message(-0.4).c_str(), "Pose time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); } -TEST(TwistDelayTimeWarningMessage, SmokeTest) +TEST(twist_delay_time_warning_message, SmokeTest) { EXPECT_STREQ( - twistDelayTimeWarningMessage(-1.0).c_str(), + twist_delay_time_warning_message(-1.0).c_str(), "Twist time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); EXPECT_STREQ( - twistDelayTimeWarningMessage(-0.4).c_str(), + twist_delay_time_warning_message(-0.4).c_str(), "Twist time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); } -TEST(MahalanobisWarningMessage, SmokeTest) +TEST(mahalanobis_warning_message, SmokeTest) { EXPECT_STREQ( - mahalanobisWarningMessage(1.0, 0.5).c_str(), + mahalanobis_warning_message(1.0, 0.5).c_str(), "The Mahalanobis distance 1.0000 is over the limit 0.5000."); } diff --git a/localization/geo_pose_projector/CMakeLists.txt b/localization/geo_pose_projector/CMakeLists.txt index 6e2cdf32fb6c5..f274e7d23dbab 100644 --- a/localization/geo_pose_projector/CMakeLists.txt +++ b/localization/geo_pose_projector/CMakeLists.txt @@ -4,11 +4,15 @@ project(geo_pose_projector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(geo_pose_projector - src/geo_pose_projector_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/geo_pose_projector.cpp ) -ament_target_dependencies(geo_pose_projector) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "GeoPoseProjector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml index d840add1ed1c7..de6785a016858 100644 --- a/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml +++ b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml index c0e2db59deb64..36b2aec8384ac 100644 --- a/localization/geo_pose_projector/package.xml +++ b/localization/geo_pose_projector/package.xml @@ -24,6 +24,7 @@ geography_utils geometry_msgs rclcpp + rclcpp_components tf2_geometry_msgs tf2_ros diff --git a/localization/geo_pose_projector/src/geo_pose_projector.cpp b/localization/geo_pose_projector/src/geo_pose_projector.cpp index d147888bb743b..80bb407958de2 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector.cpp +++ b/localization/geo_pose_projector/src/geo_pose_projector.cpp @@ -25,8 +25,8 @@ #include -GeoPoseProjector::GeoPoseProjector() -: Node("geo_pose_projector"), publish_tf_(declare_parameter("publish_tf")) +GeoPoseProjector::GeoPoseProjector(const rclcpp::NodeOptions & options) +: rclcpp::Node("geo_pose_projector", options), publish_tf_(declare_parameter("publish_tf")) { // Subscribe to map_projector_info topic const auto adaptor = component_interface_utils::NodeAdaptor(this); @@ -102,3 +102,6 @@ void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr m tf_broadcaster_->sendTransform(transform_stamped); } } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(GeoPoseProjector) diff --git a/localization/geo_pose_projector/src/geo_pose_projector.hpp b/localization/geo_pose_projector/src/geo_pose_projector.hpp index b0611fec36984..738e805ef8101 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector.hpp +++ b/localization/geo_pose_projector/src/geo_pose_projector.hpp @@ -36,7 +36,7 @@ class GeoPoseProjector : public rclcpp::Node using MapProjectorInfo = map_interface::MapProjectorInfo; public: - GeoPoseProjector(); + explicit GeoPoseProjector(const rclcpp::NodeOptions & options); private: void on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg); diff --git a/localization/geo_pose_projector/src/geo_pose_projector_node.cpp b/localization/geo_pose_projector/src/geo_pose_projector_node.cpp deleted file mode 100644 index 97367d6b9f774..0000000000000 --- a/localization/geo_pose_projector/src/geo_pose_projector_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// 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 "geo_pose_projector.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index 57589837dd529..1b142b254d2e1 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -4,17 +4,13 @@ project(gyro_odometer) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(${PROJECT_NAME} - src/gyro_odometer_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp + src/diagnostics_module.cpp ) target_link_libraries(${PROJECT_NAME} fmt) -ament_auto_add_library(gyro_odometer_node SHARED - src/gyro_odometer_core.cpp -) - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_gyro_odometer test/test_main.cpp @@ -25,10 +21,15 @@ if(BUILD_TESTING) rclcpp ) target_link_libraries(test_gyro_odometer - gyro_odometer_node + ${PROJECT_NAME} ) endif() +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::gyro_odometer::GyroOdometerNode" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package(INSTALL_TO_SHARE launch diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index e9e390010f084..aa6f2a96f4838 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -34,3 +34,18 @@ - [Limitation] The frequency of the output messages depends on the frequency of the input IMU message. - [Limitation] We cannot produce reliable values for the lateral and vertical velocities. Therefore we assign large values to the corresponding elements in the output covariance matrix. + +## Diagnostics + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| -------------------------------- | ----------------------------------------------------------------------------------------- | ------------------------------- | ------------------------------------------------- | +| `topic_time_stamp` | the time stamp of service calling. [nano second] | none | none | +| `is_arrived_first_vehicle_twist` | whether the vehicle twist topic has been received even once. | not arrive yet | none | +| `is_arrived_first_imu` | whether the imu topic has been received even once. | not arrive yet | none | +| `vehicle_twist_time_stamp_dt` | the time difference between the current time and the latest vehicle twist topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `imu_time_stamp_dt` | the time difference between the current time and the latest imu topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `vehicle_twist_queue_size` | the size of vehicle_twist_queue. | none | none | +| `imu_queue_size` | the size of gyro_queue. | none | none | +| `is_succeed_transform_imu` | whether transform imu is succeed or not. | none | failed | diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp new file mode 100644 index 0000000000000..c5b7b7a592013 --- /dev/null +++ b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp @@ -0,0 +1,65 @@ +// Copyright 2023 Autoware Foundation +// +// 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 GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ +#define GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +class DiagnosticsModule +{ +public: + DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + void clear(); + void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); + template + void add_key_value(const std::string & key, const T & value); + void update_level_and_message(const int8_t level, const std::string & message); + void publish(const rclcpp::Time & publish_time_stamp); + +private: + [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array( + const rclcpp::Time & publish_time_stamp) const; + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_; +}; + +template +void DiagnosticsModule::add_key_value(const std::string & key, const T & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = std::to_string(value); + add_key_value(key_value); +} + +template <> +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value); +template <> +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); + +} // namespace autoware::gyro_odometer + +#endif // GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ 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 3a6358e62c21a..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,9 +15,10 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_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 "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 @@ -36,20 +37,23 @@ #include #include -class GyroOdometer : public rclcpp::Node +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 GyroOdometer(const rclcpp::NodeOptions & options); - ~GyroOdometer(); + explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); private: - void callbackVehicleTwist( + void callback_vehicle_twist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr); - void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); - void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); + void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + void concat_gyro_and_odometer(); + void publish_data(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); rclcpp::Subscription::SharedPtr vehicle_twist_sub_; @@ -63,16 +67,22 @@ class GyroOdometer : 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_; bool vehicle_twist_arrived_; bool imu_arrived_; + rclcpp::Time latest_vehicle_twist_ros_time_; + rclcpp::Time latest_imu_ros_time_; std::deque vehicle_twist_queue_; std::deque gyro_queue_; + + std::unique_ptr diagnostics_; }; +} // namespace autoware::gyro_odometer + #endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index 21aff3e10d26c..aed6050858fe1 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -11,7 +11,7 @@ - + diff --git a/localization/gyro_odometer/media/diagnostic.png b/localization/gyro_odometer/media/diagnostic.png new file mode 100644 index 0000000000000..f2324f4079d0d Binary files /dev/null and b/localization/gyro_odometer/media/diagnostic.png differ diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index a0a982ddedc16..faa64a1313ebb 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -17,15 +17,15 @@ ament_cmake_auto autoware_cmake + autoware_universe_utils + diagnostic_msgs fmt geometry_msgs + rclcpp + rclcpp_components sensor_msgs tf2 tf2_geometry_msgs - tier4_autoware_utils - - rclcpp - rclcpp_components ament_cmake_ros ament_lint_auto diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp new file mode 100644 index 0000000000000..5d687943cef53 --- /dev/null +++ b/localization/gyro_odometer/src/diagnostics_module.cpp @@ -0,0 +1,110 @@ +// Copyright 2023 Autoware Foundation +// +// 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 "gyro_odometer/diagnostics_module.hpp" + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +: clock_(node->get_clock()) +{ + diagnostics_pub_ = + node->create_publisher("/diagnostics", 10); + + diagnostics_status_msg_.name = + std::string(node->get_name()) + std::string(": ") + diagnostic_name; + diagnostics_status_msg_.hardware_id = node->get_name(); +} + +void DiagnosticsModule::clear() +{ + diagnostics_status_msg_.values.clear(); + diagnostics_status_msg_.values.shrink_to_fit(); + + diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_status_msg_.message = ""; +} + +void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) +{ + auto it = std::find_if( + std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), + [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; }); + + if (it != std::cend(diagnostics_status_msg_.values)) { + it->value = key_value_msg.value; + } else { + diagnostics_status_msg_.values.push_back(key_value_msg); + } +} + +template <> +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value; + add_key_value(key_value); +} + +template <> +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value ? "True" : "False"; + add_key_value(key_value); +} + +void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) +{ + if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!diagnostics_status_msg_.message.empty()) { + diagnostics_status_msg_.message += "; "; + } + diagnostics_status_msg_.message += message; + } + if (level > diagnostics_status_msg_.level) { + diagnostics_status_msg_.level = level; + } +} + +void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) +{ + diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); +} + +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( + const rclcpp::Time & publish_time_stamp) const +{ + diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; + diagnostics_msg.header.stamp = publish_time_stamp; + diagnostics_msg.status.push_back(diagnostics_status_msg_); + + if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + diagnostics_msg.status.at(0).message = "OK"; + } + + return diagnostics_msg; +} + +} // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 5de0ecd7cdc0a..c661f63d91535 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -14,6 +14,8 @@ #include "gyro_odometer/gyro_odometer_core.hpp" +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -23,15 +25,19 @@ #include #include +#include #include -std::array transformCovariance(const std::array & cov) +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]}); - std::array cov_transformed; + std::array cov_transformed = {}; cov_transformed.fill(0.); cov_transformed[COV_IDX::X_X] = max_cov; cov_transformed[COV_IDX::Y_Y] = max_cov; @@ -39,83 +45,23 @@ std::array transformCovariance(const std::array & cov) return cov_transformed; } -geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( - const std::deque & vehicle_twist_queue, - const std::deque & gyro_queue) -{ - 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; - - double vx_mean = 0; - geometry_msgs::msg::Vector3 gyro_mean{}; - double vx_covariance_original = 0; - geometry_msgs::msg::Vector3 gyro_covariance_original{}; - for (const auto & vehicle_twist : vehicle_twist_queue) { - vx_mean += vehicle_twist.twist.twist.linear.x; - vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; - } - vx_mean /= vehicle_twist_queue.size(); - vx_covariance_original /= vehicle_twist_queue.size(); - - for (const auto & gyro : gyro_queue) { - gyro_mean.x += gyro.angular_velocity.x; - gyro_mean.y += gyro.angular_velocity.y; - gyro_mean.z += gyro.angular_velocity.z; - gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; - gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; - gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; - } - gyro_mean.x /= gyro_queue.size(); - gyro_mean.y /= gyro_queue.size(); - gyro_mean.z /= gyro_queue.size(); - gyro_covariance_original.x /= gyro_queue.size(); - gyro_covariance_original.y /= gyro_queue.size(); - gyro_covariance_original.z /= gyro_queue.size(); - - geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; - const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue.back().header.stamp); - const auto latest_imu_stamp = rclcpp::Time(gyro_queue.back().header.stamp); - if (latest_vehicle_twist_stamp < latest_imu_stamp) { - twist_with_cov.header.stamp = latest_imu_stamp; - } else { - twist_with_cov.header.stamp = latest_vehicle_twist_stamp; - } - twist_with_cov.header.frame_id = gyro_queue.front().header.frame_id; - twist_with_cov.twist.twist.linear.x = vx_mean; - twist_with_cov.twist.twist.angular = gyro_mean; - - // From a statistical point of view, here we reduce the covariances according to the number of - // observed data - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = - vx_covariance_original / vehicle_twist_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = - gyro_covariance_original.x / gyro_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = - gyro_covariance_original.y / gyro_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = - gyro_covariance_original.z / gyro_queue.size(); - - return twist_with_cov; -} - -GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) -: Node("gyro_odometer", options), +GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) +: Node("gyro_odometer", node_options), output_frame_(declare_parameter("output_frame")), message_timeout_sec_(declare_parameter("message_timeout_sec")), 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}, - std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callback_vehicle_twist, this, std::placeholders::_1)); imu_sub_ = create_subscription( - "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1)); + "imu", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callback_imu, this, std::placeholders::_1)); twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); twist_with_covariance_raw_pub_ = create_publisher( @@ -125,123 +71,206 @@ GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) twist_with_covariance_pub_ = create_publisher( "twist_with_covariance", rclcpp::QoS{10}); + diagnostics_ = std::make_unique(this, "gyro_odometer_status"); + // TODO(YamatoAndo) createTimer } -GyroOdometer::~GyroOdometer() +void GyroOdometerNode::callback_vehicle_twist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr) { + diagnostics_->clear(); + diagnostics_->add_key_value( + "topic_time_stamp", + static_cast(vehicle_twist_msg_ptr->header.stamp).nanoseconds()); + + vehicle_twist_arrived_ = true; + latest_vehicle_twist_ros_time_ = vehicle_twist_msg_ptr->header.stamp; + vehicle_twist_queue_.push_back(*vehicle_twist_msg_ptr); + concat_gyro_and_odometer(); + + diagnostics_->publish(vehicle_twist_msg_ptr->header.stamp); } -void GyroOdometer::callbackVehicleTwist( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) +void GyroOdometerNode::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { - vehicle_twist_arrived_ = true; - if (!imu_arrived_) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed"); + diagnostics_->clear(); + diagnostics_->add_key_value( + "topic_time_stamp", static_cast(imu_msg_ptr->header.stamp).nanoseconds()); + + imu_arrived_ = true; + latest_imu_ros_time_ = imu_msg_ptr->header.stamp; + gyro_queue_.push_back(*imu_msg_ptr); + concat_gyro_and_odometer(); + + diagnostics_->publish(imu_msg_ptr->header.stamp); +} + +void GyroOdometerNode::concat_gyro_and_odometer() +{ + // check arrive first topic + diagnostics_->add_key_value("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); + diagnostics_->add_key_value("is_arrived_first_imu", imu_arrived_); + if (!vehicle_twist_arrived_) { + std::stringstream message; + message << "Twist msg is not subscribed"; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (!imu_arrived_) { + std::stringstream message; + message << "Imu msg is not subscribed"; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); - const double twist_dt = std::abs((this->now() - vehicle_twist_ptr->header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + // check timeout + const double vehicle_twist_dt = + std::abs((this->now() - latest_vehicle_twist_ros_time_).seconds()); + const double imu_dt = std::abs((this->now() - latest_imu_ros_time_).seconds()); + diagnostics_->add_key_value("vehicle_twist_time_stamp_dt", vehicle_twist_dt); + diagnostics_->add_key_value("imu_time_stamp_dt", imu_dt); + if (vehicle_twist_dt > message_timeout_sec_) { + const std::string message = fmt::format( + "Vehicle twist msg is timeout. vehicle_twist_dt: {}[sec], tolerance {}[sec]", + vehicle_twist_dt, message_timeout_sec_); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); - if (gyro_queue_.empty()) return; - const double imu_dt = std::abs((this->now() - gyro_queue_.back().header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (imu_dt > message_timeout_sec_) { + const std::string message = fmt::format( + "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); -} - -void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) -{ - imu_arrived_ = true; - if (!vehicle_twist_arrived_) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed"); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - const double imu_dt = std::abs((this->now() - imu_msg_ptr->header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); + // check queue size + diagnostics_->add_key_value("vehicle_twist_queue_size", vehicle_twist_queue_.size()); + diagnostics_->add_key_value("imu_queue_size", gyro_queue_.size()); + if (vehicle_twist_queue_.empty()) { + // not output error and clear queue + return; + } + if (gyro_queue_.empty()) { + // not output error and clear queue return; } + // get transformation geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = - transform_listener_->getLatestTransform(imu_msg_ptr->header.frame_id, output_frame_); - if (!tf_imu2base_ptr) { - RCLCPP_ERROR( - this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), - (imu_msg_ptr->header.frame_id).c_str()); + transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_); + + const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr); + diagnostics_->add_key_value("is_succeed_transform_imu", is_succeed_transform_imu); + if (!is_succeed_transform_imu) { + std::stringstream message; + message << "Please publish TF " << output_frame_ << " to " + << gyro_queue_.front().header.frame_id; + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.header = imu_msg_ptr->header; - angular_velocity.vector = imu_msg_ptr->angular_velocity; - - geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - transformed_angular_velocity.header = tf_imu2base_ptr->header; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); - - sensor_msgs::msg::Imu gyro_base_link; - gyro_base_link.header = imu_msg_ptr->header; - gyro_base_link.header.frame_id = output_frame_; - gyro_base_link.angular_velocity = transformed_angular_velocity.vector; - gyro_base_link.angular_velocity_covariance = - transformCovariance(imu_msg_ptr->angular_velocity_covariance); - - gyro_queue_.push_back(gyro_base_link); - - if (vehicle_twist_queue_.empty()) return; - const double twist_dt = - std::abs((this->now() - vehicle_twist_queue_.back().header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; + // transform gyro frame + for (auto & gyro : gyro_queue_) { + geometry_msgs::msg::Vector3Stamped angular_velocity; + angular_velocity.header = gyro.header; + angular_velocity.vector = gyro.angular_velocity; + + geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; + transformed_angular_velocity.header = tf_imu2base_ptr->header; + tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); + + gyro.header.frame_id = output_frame_; + gyro.angular_velocity = transformed_angular_velocity.vector; + gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance); + } + + 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; + geometry_msgs::msg::Vector3 gyro_mean{}; + double vx_covariance_original = 0; + geometry_msgs::msg::Vector3 gyro_covariance_original{}; + for (const auto & vehicle_twist : vehicle_twist_queue_) { + vx_mean += vehicle_twist.twist.twist.linear.x; + vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; } + vx_mean /= static_cast(vehicle_twist_queue_.size()); + vx_covariance_original /= static_cast(vehicle_twist_queue_.size()); + + for (const auto & gyro : gyro_queue_) { + gyro_mean.x += gyro.angular_velocity.x; + gyro_mean.y += gyro.angular_velocity.y; + gyro_mean.z += gyro.angular_velocity.z; + gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; + gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; + gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; + } + gyro_mean.x /= static_cast(gyro_queue_.size()); + gyro_mean.y /= static_cast(gyro_queue_.size()); + gyro_mean.z /= static_cast(gyro_queue_.size()); + gyro_covariance_original.x /= static_cast(gyro_queue_.size()); + gyro_covariance_original.y /= static_cast(gyro_queue_.size()); + gyro_covariance_original.z /= static_cast(gyro_queue_.size()); + + // concat + geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; + const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue_.back().header.stamp); + const auto latest_imu_stamp = rclcpp::Time(gyro_queue_.back().header.stamp); + if (latest_vehicle_twist_stamp < latest_imu_stamp) { + twist_with_cov.header.stamp = latest_imu_stamp; + } else { + twist_with_cov.header.stamp = latest_vehicle_twist_stamp; + } + twist_with_cov.header.frame_id = gyro_queue_.front().header.frame_id; + twist_with_cov.twist.twist.linear.x = vx_mean; + twist_with_cov.twist.twist.angular = gyro_mean; + + // From a statistical point of view, here we reduce the covariances according to the number of + // observed data + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = + vx_covariance_original / static_cast(vehicle_twist_queue_.size()); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = + gyro_covariance_original.x / static_cast(gyro_queue_.size()); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = + gyro_covariance_original.y / static_cast(gyro_queue_.size()); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = + gyro_covariance_original.z / static_cast(gyro_queue_.size()); + + publish_data(twist_with_cov); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); vehicle_twist_queue_.clear(); gyro_queue_.clear(); } -void GyroOdometer::publishData( +void GyroOdometerNode::publish_data( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; @@ -269,3 +298,8 @@ void GyroOdometer::publishData( twist_pub_->publish(twist); twist_with_covariance_pub_->publish(twist_with_covariance); } + +} // namespace autoware::gyro_odometer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode) diff --git a/localization/gyro_odometer/src/gyro_odometer_node.cpp b/localization/gyro_odometer/src/gyro_odometer_node.cpp deleted file mode 100644 index 5bb6241506fbe..0000000000000 --- a/localization/gyro_odometer/src/gyro_odometer_node.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 "gyro_odometer/gyro_odometer_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - auto node = std::make_shared(options); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp index 54e1d320319d3..f55278998f5f0 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp @@ -17,7 +17,7 @@ using geometry_msgs::msg::TwistWithCovarianceStamped; using sensor_msgs::msg::Imu; -Imu generateSampleImu() +Imu generate_sample_imu() { Imu imu; imu.header.frame_id = "base_link"; @@ -27,7 +27,7 @@ Imu generateSampleImu() return imu; } -TwistWithCovarianceStamped generateSampleVelocity() +TwistWithCovarianceStamped generate_sample_velocity() { TwistWithCovarianceStamped twist; twist.header.frame_id = "base_link"; @@ -35,7 +35,7 @@ TwistWithCovarianceStamped generateSampleVelocity() return twist; } -rclcpp::NodeOptions getNodeOptionsWithDefaultParams() +rclcpp::NodeOptions get_node_options_with_default_params() { rclcpp::NodeOptions node_options; diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp index 6e3aff0b841a9..9da344e410e61 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp @@ -20,11 +20,8 @@ #include #include -using geometry_msgs::msg::TwistWithCovarianceStamped; -using sensor_msgs::msg::Imu; - -Imu generateSampleImu(); -TwistWithCovarianceStamped generateSampleVelocity(); -rclcpp::NodeOptions getNodeOptionsWithDefaultParams(); +sensor_msgs::msg::Imu generate_sample_imu(); +geometry_msgs::msg::TwistWithCovarianceStamped generate_sample_velocity(); +rclcpp::NodeOptions get_node_options_with_default_params(); #endif // TEST_GYRO_ODOMETER_HELPER_HPP_ diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index 7f6416fbdda16..fc331a638a1dd 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -50,19 +50,22 @@ class VelocityGenerator : public rclcpp::Node class GyroOdometerValidator : public rclcpp::Node { public: - GyroOdometerValidator() : Node("gyro_odometer_validator"), received_latest_twist_ptr(nullptr) - { - twist_sub = create_subscription( - "/twist_with_covariance", 1, [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { + GyroOdometerValidator() + : Node("gyro_odometer_validator"), + twist_sub(create_subscription( + "/twist_with_covariance", 1, + [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { received_latest_twist_ptr = msg; - }); + })), + received_latest_twist_ptr(nullptr) + { } rclcpp::Subscription::SharedPtr twist_sub; TwistWithCovarianceStamped::ConstSharedPtr received_latest_twist_ptr; }; -void spinSome(rclcpp::Node::SharedPtr node_ptr) +void wait_spin_some(rclcpp::Node::SharedPtr node_ptr) { for (int i = 0; i < 50; ++i) { rclcpp::spin_some(node_ptr); @@ -70,7 +73,7 @@ void spinSome(rclcpp::Node::SharedPtr node_ptr) } } -bool isTwistValid( +bool is_twist_valid( const TwistWithCovarianceStamped & twist, const TwistWithCovarianceStamped & twist_ground_truth) { if (twist.twist.twist.linear.x != twist_ground_truth.twist.twist.linear.x) { @@ -99,8 +102,8 @@ bool isTwistValid( // velocity data are provided TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) { - Imu input_imu = generateSampleImu(); - TwistWithCovarianceStamped input_velocity = generateSampleVelocity(); + Imu input_imu = generate_sample_imu(); + TwistWithCovarianceStamped input_velocity = generate_sample_velocity(); TwistWithCovarianceStamped expected_output_twist; expected_output_twist.twist.twist.linear.x = input_velocity.twist.twist.linear.x; @@ -108,7 +111,8 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y; expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z; - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = std::make_shared( + get_node_options_with_default_params()); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -119,13 +123,13 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) velocity_generator->vehicle_velocity_pub->publish(input_velocity); // gyro_odometer receives IMU and velocity, and publishes the fused twist data. - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node receives the fused twist data and store in "received_latest_twist_ptr". - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_FALSE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); - EXPECT_TRUE(isTwistValid( + EXPECT_TRUE(is_twist_valid( *(gyro_odometer_validator_node->received_latest_twist_ptr), expected_output_twist)); } @@ -133,19 +137,20 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) // Verify that the gyro_odometer does NOT publish any outputs when only IMU is provided TEST(GyroOdometer, TestGyroOdometerImuOnly) { - Imu input_imu = generateSampleImu(); + Imu input_imu = generate_sample_imu(); - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = std::make_shared( + get_node_options_with_default_params()); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); imu_generator->imu_pub->publish(input_imu); // gyro_odometer receives IMU - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node waits for the output fused twist from gyro_odometer - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_TRUE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); } diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt deleted file mode 100644 index d625064b8f6cb..0000000000000 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(ar_tag_based_localizer) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -find_package(OpenCV REQUIRED) - -ament_auto_add_executable(ar_tag_based_localizer - src/main.cpp - src/ar_tag_based_localizer.cpp -) -target_include_directories(ar_tag_based_localizer - SYSTEM PUBLIC - ${OpenCV_INCLUDE_DIRS} -) -target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES}) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_ar_tag_based_localizer - test/test.cpp - src/ar_tag_based_localizer.cpp - ) - target_include_directories(test_ar_tag_based_localizer - SYSTEM PUBLIC - ${OpenCV_INCLUDE_DIRS} - ) - target_link_libraries(test_ar_tag_based_localizer ${OpenCV_LIBRARIES}) -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md deleted file mode 100644 index 1a776bd810fff..0000000000000 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md +++ /dev/null @@ -1,80 +0,0 @@ -# AR Tag Based Localizer - -**ArTagBasedLocalizer** is a vision-based localization node. - -ar_tag_image - -This node uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. -The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. - -## Inputs / Outputs - -### `ar_tag_based_localizer` node - -#### Input - -| Name | Type | Description | -| :--------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | -| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | -| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | -| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. | - -#### Output - -| Name | Type | Description | -| :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- | -| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose | -| `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | -| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] Loaded landmarks to visualize in Rviz as thin boards | -| `/tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | -| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | - -## Parameters - -{{ json_to_markdown("localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json") }} - -## How to launch - -When launching Autoware, set `artag` for `pose_source`. - -```bash -ros2 launch autoware_launch ... \ - pose_source:=artag \ - ... -``` - -### Rosbag - -#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1uMVwQQFcfs8JOqfoA1FqfH_fLPwQ71jK/view) - -This data is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). -Essentially, AR tag-based self-localization is not intended for such public road driving, but for driving in a smaller area, so the max driving speed is set at 15 km/h. - -It is a known problem that the timing of when each AR tag begins to be detected can cause significant changes in estimation. - -![sample_result_in_awsim](./doc_image/sample_result_in_awsim.png) - -#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) - -Please remap the topic names and play it. - -```bash -ros2 bag play /path/to/ar_tag_based_localizer_sample_bag/ -r 0.5 -s sqlite3 \ - --remap /sensing/camera/front/image:=/sensing/camera/traffic_light/image_raw \ - /sensing/camera/front/image/info:=/sensing/camera/traffic_light/camera_info -``` - -This dataset contains issues such as missing IMU data, and overall the accuracy is low. Even when running AR tag-based self-localization, significant difference from the true trajectory can be observed. - -The image below shows the trajectory when the sample is executed and plotted. - -![sample_result](./doc_image/sample_result.png) - -The pull request video below should also be helpful. - - - -## Principle - -![principle](../doc_image/principle.png) diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml deleted file mode 100644 index 072479cc7aaf5..0000000000000 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - ar_tag_based_localizer - 0.0.0 - The ar_tag_based_localizer package - Shintaro Sakoda - Masahiro Sakamoto - Yamato Ando - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Ryu Yamamoto - Apache License 2.0 - - ament_cmake - autoware_cmake - - ament_index_cpp - aruco - cv_bridge - diagnostic_msgs - landmark_manager - lanelet2_extension - localization_util - rclcpp - tf2_eigen - tf2_geometry_msgs - tf2_ros - visualization_msgs - - - ament_cmake - - diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp deleted file mode 100644 index 8ef1dd6195580..0000000000000 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// 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. - -// This source code is derived from the https://github.com/pal-robotics/aruco_ros. -// Here is the license statement. -/***************************** - Copyright 2011 Rafael Muñoz Salinas. All rights reserved. - - Redistribution and use in source and binary forms, with or without modification, are - permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, this list of - conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, this list - of conditions and the following disclaimer in the documentation and/or other materials - provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED - WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND - FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR - CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - The views and conclusions contained in the software and documentation are those of the - authors and should not be interpreted as representing official policies, either expressed - or implied, of Rafael Muñoz Salinas. - ********************************/ - -#include "ar_tag_based_localizer.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - std::shared_ptr ptr = std::make_shared(); - rclcpp::spin(ptr); - rclcpp::shutdown(); -} diff --git a/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt deleted file mode 100644 index 1b57e5cc89018..0000000000000 --- a/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(landmark_manager) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -ament_auto_add_library(landmark_manager SHARED - src/landmark_manager.cpp -) - -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/localization/landmark_based_localizer/landmark_manager/package.xml b/localization/landmark_based_localizer/landmark_manager/package.xml deleted file mode 100644 index be19de9334e84..0000000000000 --- a/localization/landmark_based_localizer/landmark_manager/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - landmark_manager - 0.0.0 - The landmark_manager package - Shintaro Sakoda - Masahiro Sakamoto - Yamato Ando - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Ryu Yamamoto - Apache License 2.0 - - ament_cmake - autoware_cmake - - eigen - - autoware_auto_mapping_msgs - geometry_msgs - lanelet2_extension - localization_util - rclcpp - tf2_eigen - - - ament_cmake - - diff --git a/localization/localization_error_monitor/CMakeLists.txt b/localization/localization_error_monitor/CMakeLists.txt index 3528367486350..c27e51e6e0359 100644 --- a/localization/localization_error_monitor/CMakeLists.txt +++ b/localization/localization_error_monitor/CMakeLists.txt @@ -4,15 +4,16 @@ project(localization_error_monitor) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(localization_error_monitor_module SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/diagnostics.cpp + src/localization_error_monitor.cpp ) -ament_auto_add_executable(localization_error_monitor - src/main.cpp - src/node.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "LocalizationErrorMonitor" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor ) -target_link_libraries(localization_error_monitor localization_error_monitor_module) if(BUILD_TESTING) function(add_testcase filepath) @@ -20,17 +21,17 @@ if(BUILD_TESTING) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - target_link_libraries(${test_name} localization_error_monitor_module) + target_link_libraries(${test_name} ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() - find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() add_testcase(test/test_diagnostics.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE config launch ) diff --git a/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp index 4fe4e5a5aac14..b1da87128bee5 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp @@ -20,12 +20,12 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy_lateral_direction( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array); #endif // LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ 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 new file mode 100644 index 0000000000000..a178a305f6d12 --- /dev/null +++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp @@ -0,0 +1,63 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ +#define LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include + +struct Ellipse +{ + double long_radius; + double short_radius; + double yaw; + Eigen::Matrix2d P; + double size_lateral_direction; +}; + +class LocalizationErrorMonitor : public rclcpp::Node +{ +private: + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr ellipse_marker_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; + + rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr logger_configure_; + + double scale_; + double error_ellipse_size_; + double warn_ellipse_size_; + double error_ellipse_size_lateral_direction_; + double warn_ellipse_size_lateral_direction_; + Ellipse ellipse_; + + void on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg); + visualization_msgs::msg::Marker create_ellipse_marker( + const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom); + static double measure_size_ellipse_along_body_frame(const Eigen::Matrix2d & Pinv, double theta); + +public: + explicit LocalizationErrorMonitor(const rclcpp::NodeOptions & options); +}; +#endif // LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ diff --git a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp deleted file mode 100644 index 296b278004333..0000000000000 --- a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef LOCALIZATION_ERROR_MONITOR__NODE_HPP_ -#define LOCALIZATION_ERROR_MONITOR__NODE_HPP_ - -#include -#include -#include - -#include -#include -#include - -#include - -struct Ellipse -{ - double long_radius; - double short_radius; - double yaw; - Eigen::Matrix2d P; - double size_lateral_direction; -}; - -class LocalizationErrorMonitor : public rclcpp::Node -{ -private: - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Publisher::SharedPtr ellipse_marker_pub_; - rclcpp::Publisher::SharedPtr diag_pub_; - - rclcpp::TimerBase::SharedPtr timer_; - - std::unique_ptr logger_configure_; - - double scale_; - double error_ellipse_size_; - double warn_ellipse_size_; - double error_ellipse_size_lateral_direction_; - double warn_ellipse_size_lateral_direction_; - Ellipse ellipse_; - - void onOdom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg); - visualization_msgs::msg::Marker createEllipseMarker( - const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom); - double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta); - -public: - LocalizationErrorMonitor(); - ~LocalizationErrorMonitor() = default; -}; -#endif // LOCALIZATION_ERROR_MONITOR__NODE_HPP_ diff --git a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml index eb2b5741b00fc..ad3e8beff92ab 100644 --- a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml +++ b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index a1b352d911a0d..681da7e57c6c5 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -20,12 +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/diagnostics.cpp b/localization/localization_error_monitor/src/diagnostics.cpp index 375fccfa06787..e6b9da8fc4a97 100644 --- a/localization/localization_error_monitor/src/diagnostics.cpp +++ b/localization/localization_error_monitor/src/diagnostics.cpp @@ -17,7 +17,7 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -41,7 +41,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy_lateral_direction( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -66,7 +66,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection } // The highest level within the stat_array will be reflected in the merged_stat. -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array) { diagnostic_msgs::msg::DiagnosticStatus merged_stat; diff --git a/localization/localization_error_monitor/src/localization_error_monitor.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp new file mode 100644 index 0000000000000..204afa7bdec2f --- /dev/null +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -0,0 +1,149 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "localization_error_monitor/localization_error_monitor.hpp" + +#include "localization_error_monitor/diagnostics.hpp" + +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include +#include +#include +#include + +LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & options) +: Node("localization_error_monitor", options) +{ + scale_ = this->declare_parameter("scale"); + error_ellipse_size_ = this->declare_parameter("error_ellipse_size"); + warn_ellipse_size_ = this->declare_parameter("warn_ellipse_size"); + + error_ellipse_size_lateral_direction_ = + this->declare_parameter("error_ellipse_size_lateral_direction"); + warn_ellipse_size_lateral_direction_ = + this->declare_parameter("warn_ellipse_size_lateral_direction"); + + odom_sub_ = this->create_subscription( + "input/odom", 1, std::bind(&LocalizationErrorMonitor::on_odom, this, std::placeholders::_1)); + + // QoS setup + rclcpp::QoS durable_qos(1); + durable_qos.transient_local(); // option for latching + ellipse_marker_pub_ = + this->create_publisher("debug/ellipse_marker", durable_qos); + + diag_pub_ = this->create_publisher("/diagnostics", 10); + + logger_configure_ = std::make_unique(this); +} + +visualization_msgs::msg::Marker LocalizationErrorMonitor::create_ellipse_marker( + const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom) +{ + tf2::Quaternion quat; + quat.setEuler(0, 0, ellipse.yaw); + + const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); + const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); + visualization_msgs::msg::Marker marker; + marker.header = odom->header; + marker.header.stamp = this->now(); + marker.ns = "error_ellipse"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = odom->pose.pose; + marker.pose.orientation = tf2::toMsg(quat); + marker.scale.x = ellipse_long_radius * 2; + marker.scale.y = ellipse_short_radius * 2; + marker.scale.z = 0.01; + marker.color.a = 0.1; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + return marker; +} + +void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) +{ + // create xy covariance (2x2 matrix) + // input geometry_msgs::PoseWithCovariance contain 6x6 matrix + Eigen::Matrix2d xy_covariance; + const auto cov = input_msg->pose.covariance; + xy_covariance(0, 0) = cov[0 * 6 + 0]; + xy_covariance(0, 1) = cov[0 * 6 + 1]; + xy_covariance(1, 0) = cov[1 * 6 + 0]; + xy_covariance(1, 1) = cov[1 * 6 + 1]; + + Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); + + // eigen values and vectors are sorted in ascending order + ellipse_.long_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(1)); + ellipse_.short_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(0)); + + // principal component vector + const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); + ellipse_.yaw = std::atan2(pc_vector.y(), pc_vector.x()); + + // ellipse size along lateral direction (body-frame) + ellipse_.P = xy_covariance; + const double yaw_vehicle = tf2::getYaw(input_msg->pose.pose.orientation); + ellipse_.size_lateral_direction = + scale_ * measure_size_ellipse_along_body_frame(ellipse_.P.inverse(), yaw_vehicle); + + const auto ellipse_marker = create_ellipse_marker(ellipse_, input_msg); + ellipse_marker_pub_->publish(ellipse_marker); + + // diagnostics + std::vector diag_status_array; + diag_status_array.push_back( + check_localization_accuracy(ellipse_.long_radius, warn_ellipse_size_, error_ellipse_size_)); + diag_status_array.push_back(check_localization_accuracy_lateral_direction( + ellipse_.size_lateral_direction, warn_ellipse_size_lateral_direction_, + error_ellipse_size_lateral_direction_)); + + diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; + diag_merged_status = merge_diagnostic_status(diag_status_array); + diag_merged_status.name = "localization: " + std::string(this->get_name()); + diag_merged_status.hardware_id = this->get_name(); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = input_msg->header.stamp; + diag_msg.status.push_back(diag_merged_status); + diag_pub_->publish(diag_msg); +} + +double LocalizationErrorMonitor::measure_size_ellipse_along_body_frame( + const Eigen::Matrix2d & Pinv, const double theta) +{ + Eigen::MatrixXd e(2, 1); + e(0, 0) = std::cos(theta); + e(1, 0) = std::sin(theta); + + double d = std::sqrt((e.transpose() * Pinv * e)(0, 0) / Pinv.determinant()); + return d; +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(LocalizationErrorMonitor) diff --git a/localization/localization_error_monitor/src/main.cpp b/localization/localization_error_monitor/src/main.cpp deleted file mode 100644 index f4fa5ab664005..0000000000000 --- a/localization/localization_error_monitor/src/main.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "localization_error_monitor/node.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/node.cpp deleted file mode 100644 index f47372a0158b2..0000000000000 --- a/localization/localization_error_monitor/src/node.cpp +++ /dev/null @@ -1,145 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "localization_error_monitor/node.hpp" - -#include "localization_error_monitor/diagnostics.hpp" - -#include - -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include -#include -#include -#include -#include - -LocalizationErrorMonitor::LocalizationErrorMonitor() : Node("localization_error_monitor") -{ - scale_ = this->declare_parameter("scale"); - error_ellipse_size_ = this->declare_parameter("error_ellipse_size"); - warn_ellipse_size_ = this->declare_parameter("warn_ellipse_size"); - - error_ellipse_size_lateral_direction_ = - this->declare_parameter("error_ellipse_size_lateral_direction"); - warn_ellipse_size_lateral_direction_ = - this->declare_parameter("warn_ellipse_size_lateral_direction"); - - odom_sub_ = this->create_subscription( - "input/odom", 1, std::bind(&LocalizationErrorMonitor::onOdom, this, std::placeholders::_1)); - - // QoS setup - rclcpp::QoS durable_qos(1); - durable_qos.transient_local(); // option for latching - ellipse_marker_pub_ = - this->create_publisher("debug/ellipse_marker", durable_qos); - - diag_pub_ = this->create_publisher("/diagnostics", 10); - - logger_configure_ = std::make_unique(this); -} - -visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker( - const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom) -{ - tf2::Quaternion quat; - quat.setEuler(0, 0, ellipse.yaw); - - const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); - const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); - visualization_msgs::msg::Marker marker; - marker.header = odom->header; - marker.header.stamp = this->now(); - marker.ns = "error_ellipse"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = odom->pose.pose; - marker.pose.orientation = tf2::toMsg(quat); - marker.scale.x = ellipse_long_radius * 2; - marker.scale.y = ellipse_short_radius * 2; - marker.scale.z = 0.01; - marker.color.a = 0.1; - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - return marker; -} - -void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) -{ - // create xy covariance (2x2 matrix) - // input geometry_msgs::PoseWithCovariance contain 6x6 matrix - Eigen::Matrix2d xy_covariance; - const auto cov = input_msg->pose.covariance; - xy_covariance(0, 0) = cov[0 * 6 + 0]; - xy_covariance(0, 1) = cov[0 * 6 + 1]; - xy_covariance(1, 0) = cov[1 * 6 + 0]; - xy_covariance(1, 1) = cov[1 * 6 + 1]; - - Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); - - // eigen values and vectors are sorted in ascending order - ellipse_.long_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(1)); - ellipse_.short_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(0)); - - // principal component vector - const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); - ellipse_.yaw = std::atan2(pc_vector.y(), pc_vector.x()); - - // ellipse size along lateral direction (body-frame) - ellipse_.P = xy_covariance; - const double yaw_vehicle = tf2::getYaw(input_msg->pose.pose.orientation); - ellipse_.size_lateral_direction = - scale_ * measureSizeEllipseAlongBodyFrame(ellipse_.P.inverse(), yaw_vehicle); - - const auto ellipse_marker = createEllipseMarker(ellipse_, input_msg); - ellipse_marker_pub_->publish(ellipse_marker); - - // diagnostics - std::vector diag_status_array; - diag_status_array.push_back( - checkLocalizationAccuracy(ellipse_.long_radius, warn_ellipse_size_, error_ellipse_size_)); - diag_status_array.push_back(checkLocalizationAccuracyLateralDirection( - ellipse_.size_lateral_direction, warn_ellipse_size_lateral_direction_, - error_ellipse_size_lateral_direction_)); - - diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; - diag_merged_status = mergeDiagnosticStatus(diag_status_array); - diag_merged_status.name = "localization: " + std::string(this->get_name()); - diag_merged_status.hardware_id = this->get_name(); - - diagnostic_msgs::msg::DiagnosticArray diag_msg; - diag_msg.header.stamp = this->now(); - diag_msg.status.push_back(diag_merged_status); - diag_pub_->publish(diag_msg); -} - -double LocalizationErrorMonitor::measureSizeEllipseAlongBodyFrame( - const Eigen::Matrix2d & Pinv, const double theta) -{ - Eigen::MatrixXd e(2, 1); - e(0, 0) = std::cos(theta); - e(1, 0) = std::sin(theta); - - double d = std::sqrt((e.transpose() * Pinv * e)(0, 0) / Pinv.determinant()); - return d; -} diff --git a/localization/localization_error_monitor/test/test_diagnostics.cpp b/localization/localization_error_monitor/test/test_diagnostics.cpp index 7b7e8aaab4ed9..12515687e7a98 100644 --- a/localization/localization_error_monitor/test/test_diagnostics.cpp +++ b/localization/localization_error_monitor/test/test_diagnostics.cpp @@ -24,23 +24,23 @@ TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracy) const double error_ellipse_size = 1.0; double ellipse_size = 0.0; - stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); ellipse_size = 0.7; - stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); ellipse_size = 0.8; - stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); ellipse_size = 0.9; - stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); ellipse_size = 1.0; - stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); } @@ -52,28 +52,28 @@ TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracyLateralDi const double error_ellipse_size = 0.3; double ellipse_size = 0.0; - stat = - checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy_lateral_direction( + ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); ellipse_size = 0.24; - stat = - checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy_lateral_direction( + ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); ellipse_size = 0.25; - stat = - checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy_lateral_direction( + ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); ellipse_size = 0.29; - stat = - checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy_lateral_direction( + ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); ellipse_size = 0.3; - stat = - checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy_lateral_direction( + ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); } @@ -86,7 +86,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat_array.at(1).message = "OK"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); EXPECT_EQ(merged_stat.message, "OK"); @@ -94,7 +94,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat_array.at(1).message = "OK"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0"); @@ -102,7 +102,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat_array.at(1).message = "WARN1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN1"); @@ -110,7 +110,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat_array.at(1).message = "WARN1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); @@ -118,7 +118,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR1"); @@ -126,7 +126,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); @@ -134,7 +134,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "ERROR0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); } 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/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp b/localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp new file mode 100644 index 0000000000000..ee25ac175c0b4 --- /dev/null +++ b/localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp @@ -0,0 +1,84 @@ +// Copyright 2023 Autoware Foundation +// +// 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 LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#define LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ + +/* +A implementation of tree-structured parzen estimator (TPE) +See below pdf for the TPE algorithm detail. +https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf + +Optuna is also used as a reference for implementation. +https://github.com/optuna/optuna +*/ + +#include +#include +#include + +class TreeStructuredParzenEstimator +{ +public: + using Input = std::vector; + using Score = double; + struct Trial + { + Input input; + Score score; + }; + + enum Direction { + MINIMIZE = 0, + MAXIMIZE = 1, + }; + + enum Index { + TRANS_X = 0, + TRANS_Y = 1, + TRANS_Z = 2, + ANGLE_X = 3, + ANGLE_Y = 4, + ANGLE_Z = 5, + INDEX_NUM = 6, + }; + + TreeStructuredParzenEstimator() = delete; + TreeStructuredParzenEstimator( + const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, + std::vector sample_stddev); + void add_trial(const Trial & trial); + [[nodiscard]] Input get_next_input() const; + +private: + static constexpr double max_good_rate = 0.10; + static constexpr int64_t n_ei_candidates = 100; + + static std::mt19937_64 engine; + + [[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_; + const Direction direction_; + const int64_t n_startup_trials_; + const int64_t input_dimension_; + const std::vector sample_mean_; + const std::vector sample_stddev_; + Input base_stddev_; +}; + +#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/localization_util/src/util_func.cpp b/localization/localization_util/src/util_func.cpp index ae6f0b61f063c..133442df68dcd 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -28,11 +28,11 @@ std_msgs::msg::ColorRGBA exchange_color_crc(double x) color.b = 1.0; color.g = static_cast(std::sin(x * 2.0 * M_PI)); color.r = 0; - } else if (x > 0.25 && x <= 0.5) { + } else if (x <= 0.5) { color.b = static_cast(std::sin(x * 2 * M_PI)); color.g = 1.0; color.r = 0; - } else if (x > 0.5 && x <= 0.75) { + } else if (x <= 0.75) { color.b = 0; color.g = 1.0; color.r = static_cast(-std::sin(x * 2.0 * M_PI)); diff --git a/localization/localization_util/test/test_smart_pose_buffer.cpp b/localization/localization_util/test/test_smart_pose_buffer.cpp index 2520e458f2d33..a8ed6d98b6064 100644 --- a/localization/localization_util/test/test_smart_pose_buffer.cpp +++ b/localization/localization_util/test/test_smart_pose_buffer.cpp @@ -42,15 +42,7 @@ bool compare_pose( pose_a.pose.pose.orientation.w == pose_b.pose.pose.orientation.w; } -class TestSmartPoseBuffer : public ::testing::Test -{ -protected: - void SetUp() override {} - - void TearDown() override {} -}; - -TEST_F(TestSmartPoseBuffer, interpolate_pose) // NOLINT +TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT { rclcpp::Logger logger = rclcpp::get_logger("test_logger"); const double pose_timeout_sec = 10.0; diff --git a/localization/localization_util/test/test_tpe.cpp b/localization/localization_util/test/test_tpe.cpp new file mode 100644 index 0000000000000..fd9afe8b2a75f --- /dev/null +++ b/localization/localization_util/test/test_tpe.cpp @@ -0,0 +1,67 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "localization_util/tree_structured_parzen_estimator.hpp" + +#include + +TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) +{ + auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { + double value = 0.0; + const auto n = static_cast(input.size()); + for (int64_t i = 0; i < n; i++) { + const double v = input[i] * 10; + value += v * v; + } + return value; + }; + + constexpr int64_t k_outer_trials_num = 20; + constexpr int64_t k_inner_trials_num = 200; + std::cout << std::fixed; + std::vector mean_scores; + std::vector sample_mean(5, 0.0); + std::vector sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1}; + + 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 < 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 < k_inner_trials_num; trial++) { + const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); + const double score = -sphere_function(input); + estimator.add_trial({input, score}); + best_score = std::max(best_score, score); + } + scores.push_back(best_score); + } + + const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); + const double mean = sum / static_cast(scores.size()); + mean_scores.push_back(mean); + 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; + } + ASSERT_LT(mean_scores[0], mean_scores[1]); +} diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 7ac78514c49fe..5e4db9571c404 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -22,20 +22,24 @@ else() endif() endif() -find_package(glog REQUIRED) find_package(PCL REQUIRED COMPONENTS common io registration) include_directories(${PCL_INCLUDE_DIRS}) -ament_auto_add_executable(ndt_scan_matcher +ament_auto_add_library(${PROJECT_NAME} SHARED src/diagnostics_module.cpp src/map_update_module.cpp src/ndt_scan_matcher_core.cpp - src/ndt_scan_matcher_node.cpp src/particle.cpp ) link_directories(${PCL_LIBRARY_DIRS}) -target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES} glog::glog) +target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "NDTScanMatcher" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) if(BUILD_TESTING) add_launch_test( @@ -46,19 +50,9 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_auto_add_gtest(standard_sequence_for_initial_pose_estimation test/test_cases/standard_sequence_for_initial_pose_estimation.cpp - src/diagnostics_module.cpp - src/map_update_module.cpp - src/ndt_scan_matcher_core.cpp -# src/ndt_scan_matcher_node.cpp - src/particle.cpp ) ament_auto_add_gtest(once_initialize_at_out_of_map_then_initialize_correctly test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp - src/diagnostics_module.cpp - src/map_update_module.cpp - src/ndt_scan_matcher_core.cpp -# src/ndt_scan_matcher_node.cpp - src/particle.cpp ) endif() diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index fbc2fa3c9a3f3..48ce49d2bcaa9 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -262,23 +262,27 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num drawing -| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_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 | -| `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 | yes | -| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | -| `is_set_map_points` | whether the map points is set or not | not set | none | yes | -| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | -| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | -| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | -| `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 | -| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | 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 | none | the number of times is 5 or more | - | +| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_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 `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. | +| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | +| `is_set_map_points` | whether the map points is set or not | not set | none | yes | +| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | +| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | +| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | +| `transform_probability_diff` | the tp score difference for the current ndt optimization | none | none | no | +| `transform_probability_before` | the tp score before the current ndt optimization | none | none | no | +| `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 `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 `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/diagnostics_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp index 828eb6bed346b..6dfea386abaf8 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp @@ -27,14 +27,15 @@ class DiagnosticsModule public: DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); void clear(); - void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); + void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); template - void addKeyValue(const std::string & key, const T & value); - void updateLevelAndMessage(const int8_t level, const std::string & message); - void publish(); + void add_key_value(const std::string & key, const T & value); + void update_level_and_message(const int8_t level, const std::string & message); + void publish(const rclcpp::Time & publish_time_stamp); private: - diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray() const; + [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array( + const rclcpp::Time & publish_time_stamp) const; rclcpp::Clock::SharedPtr clock_; rclcpp::Publisher::SharedPtr diagnostics_pub_; @@ -43,17 +44,17 @@ class DiagnosticsModule }; template -void DiagnosticsModule::addKeyValue(const std::string & key, const T & value) +void DiagnosticsModule::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = std::to_string(value); - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value); template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); #endif // NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_ 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 bc7bf1fe40d36..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 @@ -33,62 +33,64 @@ struct HyperParameters { struct Frame { - std::string base_frame; - std::string ndt_base_frame; - std::string map_frame; - } frame; + std::string base_frame{}; + std::string ndt_base_frame{}; + std::string map_frame{}; + } frame{}; struct SensorPoints { - double required_distance; - } sensor_points; + double timeout_sec{}; + double required_distance{}; + } sensor_points{}; - pclomp::NdtParams ndt; - bool ndt_regularization_enable; + pclomp::NdtParams ndt{}; + bool ndt_regularization_enable{}; struct InitialPoseEstimation { - int64_t particles_num; - int64_t n_startup_trials; - } initial_pose_estimation; + int64_t particles_num{}; + int64_t n_startup_trials{}; + } initial_pose_estimation{}; struct Validation { - double lidar_topic_timeout_sec; - double initial_pose_timeout_sec; - double initial_pose_distance_tolerance_m; - double critical_upper_bound_exe_time_ms; - } validation; + 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 { - ConvergedParamType converged_param_type; - double converged_param_transform_probability; - double converged_param_nearest_voxel_transformation_likelihood; + ConvergedParamType converged_param_type{}; + double converged_param_transform_probability{}; + double converged_param_nearest_voxel_transformation_likelihood{}; struct NoGroundPoints { - bool enable; - double z_margin_for_ground_removal; - } no_ground_points; - } score_estimation; + bool enable{}; + double z_margin_for_ground_removal{}; + } no_ground_points{}; + } score_estimation{}; struct Covariance { - std::array output_pose_covariance; + std::array output_pose_covariance{}; struct CovarianceEstimation { - bool enable; - std::vector initial_pose_offset_model; - } covariance_estimation; - } covariance; + bool enable{}; + std::vector initial_pose_offset_model{}; + } covariance_estimation{}; + } covariance{}; struct DynamicMapLoading { - double update_distance; - double map_radius; - double lidar_radius; - } dynamic_map_loading; + double update_distance{}; + double map_radius{}; + double lidar_radius{}; + } dynamic_map_loading{}; public: explicit HyperParameters(rclcpp::Node * node) @@ -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 4f8042d106c75..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 @@ -131,8 +131,6 @@ class NDTScanMatcher : public rclcpp::Node static int count_oscillation(const std::vector & result_pose_msg_array); - std::array rotate_covariance( - const std::array & src_covariance, const Eigen::Matrix3d & rotation) const; std::array estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time); @@ -205,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/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index cbcb0a9f74bc4..671e2ace56cad 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index b9a1f7dbad1bd..c69836d77c825 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -19,16 +19,17 @@ autoware_cmake autoware_map_msgs + autoware_universe_utils diagnostic_msgs fmt geometry_msgs - glog libpcl-all-dev localization_util nav_msgs ndt_omp pcl_conversions rclcpp + rclcpp_components sensor_msgs std_msgs std_srvs @@ -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/diagnostics_module.cpp b/localization/ndt_scan_matcher/src/diagnostics_module.cpp index 9d7eed46414ea..805ee676c5e04 100644 --- a/localization/ndt_scan_matcher/src/diagnostics_module.cpp +++ b/localization/ndt_scan_matcher/src/diagnostics_module.cpp @@ -41,7 +41,7 @@ void DiagnosticsModule::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) { auto it = std::find_if( std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), @@ -55,24 +55,24 @@ void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_v } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value) +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value; - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value) +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value ? "True" : "False"; - addKeyValue(key_value); + add_key_value(key_value); } -void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) { if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { if (!diagnostics_status_msg_.message.empty()) { @@ -85,15 +85,16 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str } } -void DiagnosticsModule::publish() +void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) { - diagnostics_pub_->publish(createDiagnosticsArray()); + diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray() const +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( + const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; - diagnostics_msg.header.stamp = clock_->now(); + diagnostics_msg.header.stamp = publish_time_stamp; diagnostics_msg.status.push_back(diagnostics_status_msg_); if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 43174a39b797e..32e5a0f2a7c3c 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -52,26 +52,24 @@ void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->addKeyValue("timer_callback_time_stamp", clock_->now().seconds()); - // check is_activated - diagnostics_ptr->addKeyValue("is_activated", is_activated); + diagnostics_ptr->add_key_value("is_activated", is_activated); if (!is_activated) { std::stringstream message; message << "Node is not activated."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } // check is_set_last_update_position const bool is_set_last_update_position = (position != std::nullopt); - diagnostics_ptr->addKeyValue("is_set_last_update_position", is_set_last_update_position); + diagnostics_ptr->add_key_value("is_set_last_update_position", is_set_last_update_position); if (!is_set_last_update_position) { std::stringstream message; message << "Cannot find the reference position for map update." << "Please check if the EKF odometry is provided to NDT."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -94,11 +92,11 @@ bool MapUpdateModule::should_update_map( const double distance = std::hypot(dx, dy); // check distance_last_update_position_to_current_position - diagnostics_ptr->addKeyValue("distance_last_update_position_to_current_position", distance); + diagnostics_ptr->add_key_value("distance_last_update_position_to_current_position", distance); if (distance + param_.lidar_radius > param_.map_radius) { std::stringstream message; message << "Dynamic map loading is not keeping up."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); // If the map does not keep up with the current position, @@ -112,7 +110,7 @@ bool MapUpdateModule::should_update_map( void MapUpdateModule::update_map( const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->addKeyValue("is_need_rebuild", need_rebuild_); + diagnostics_ptr->add_key_value("is_need_rebuild", need_rebuild_); // If the current position is super far from the previous loading position, // lock and rebuild ndt_ptr_ @@ -132,14 +130,14 @@ void MapUpdateModule::update_map( const bool updated = update_ndt(position, *ndt_ptr_, diagnostics_ptr); // check is_updated_map - diagnostics_ptr->addKeyValue("is_updated_map", updated); + diagnostics_ptr->add_key_value("is_updated_map", updated); if (!updated) { std::stringstream message; message << "update_ndt failed. If this happens with initial position estimation, make sure that" << "(1) the initial position matches the pcd map and (2) the map_loader is working " "properly."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, message.str()); last_update_position_ = position; @@ -159,7 +157,7 @@ void MapUpdateModule::update_map( const bool updated = update_ndt(position, *secondary_ndt_ptr_, diagnostics_ptr); // check is_updated_map - diagnostics_ptr->addKeyValue("is_updated_map", updated); + diagnostics_ptr->add_key_value("is_updated_map", updated); if (!updated) { last_update_position_ = position; return; @@ -191,7 +189,7 @@ bool MapUpdateModule::update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->addKeyValue("maps_size_before", ndt.getCurrentMapIDs().size()); + diagnostics_ptr->add_key_value("maps_size_before", ndt.getCurrentMapIDs().size()); auto request = std::make_shared(); @@ -201,11 +199,11 @@ bool MapUpdateModule::update_ndt( request->cached_ids = ndt.getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false); + diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", false); std::stringstream message; message << "Waiting for pcd loader service. Check the pointcloud_map_loader."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -219,23 +217,23 @@ bool MapUpdateModule::update_ndt( while (status != std::future_status::ready) { // check is_succeed_call_pcd_loader if (!rclcpp::ok()) { - diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false); + diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", false); std::stringstream message; message << "pcd_loader service is not working."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; // No update } status = result.wait_for(std::chrono::seconds(1)); } - diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", true); + diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true); auto & maps_to_add = result.get()->new_pointcloud_with_ids; auto & map_ids_to_remove = result.get()->ids_to_remove; - diagnostics_ptr->addKeyValue("maps_to_add_size", maps_to_add.size()); - diagnostics_ptr->addKeyValue("maps_to_remove_size", map_ids_to_remove.size()); + diagnostics_ptr->add_key_value("maps_to_add_size", maps_to_add.size()); + diagnostics_ptr->add_key_value("maps_to_remove_size", map_ids_to_remove.size()); if (maps_to_add.empty() && map_ids_to_remove.empty()) { return false; // No update @@ -263,9 +261,9 @@ bool MapUpdateModule::update_ndt( const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0; - diagnostics_ptr->addKeyValue("map_update_execution_time", exe_time); - diagnostics_ptr->addKeyValue("maps_size_after", ndt.getCurrentMapIDs().size()); - diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", true); + diagnostics_ptr->add_key_value("map_update_execution_time", exe_time); + diagnostics_ptr->add_key_value("maps_size_after", ndt.getCurrentMapIDs().size()); + diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true); return true; // Updated } 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 8e7685180c1f9..3bf6e355000bf 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 @@ -62,6 +62,28 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } +std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) +{ + std::array ret_covariance = src_covariance; + + Eigen::Matrix3d src_cov; + src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], + src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], + src_covariance[14]; + + Eigen::Matrix3d ret_cov; + ret_cov = rotation * src_cov * rotation.transpose(); + + for (Eigen::Index i = 0; i < 3; ++i) { + ret_covariance[i] = ret_cov(0, i); + ret_covariance[i + 6] = ret_cov(1, i); + ret_covariance[i + 12] = ret_cov(2, i); + } + + return ret_covariance; +} + NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) : Node("ndt_scan_matcher", options), tf2_broadcaster_(*this), @@ -188,16 +210,20 @@ 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() { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_map_update_->clear(); + diagnostics_map_update_->add_key_value("timer_callback_time_stamp", ros_time_now.nanoseconds()); + map_update_module_->callback_timer(is_activated_, latest_ekf_position_, diagnostics_map_update_); - diagnostics_map_update_->publish(); + diagnostics_map_update_->publish(ros_time_now); } void NDTScanMatcher::callback_initial_pose( @@ -207,21 +233,22 @@ void NDTScanMatcher::callback_initial_pose( callback_initial_pose_main(initial_pose_msg_ptr); - diagnostics_initial_pose_->publish(); + diagnostics_initial_pose_->publish(initial_pose_msg_ptr->header.stamp); } void NDTScanMatcher::callback_initial_pose_main( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { - diagnostics_initial_pose_->addKeyValue( - "topic_time_stamp", static_cast(initial_pose_msg_ptr->header.stamp).seconds()); + diagnostics_initial_pose_->add_key_value( + "topic_time_stamp", + static_cast(initial_pose_msg_ptr->header.stamp).nanoseconds()); // check is_activated - diagnostics_initial_pose_->addKeyValue("is_activated", static_cast(is_activated_)); + diagnostics_initial_pose_->add_key_value("is_activated", static_cast(is_activated_)); if (!is_activated_) { std::stringstream message; message << "Node is not activated."; - diagnostics_initial_pose_->updateLevelAndMessage( + diagnostics_initial_pose_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -229,13 +256,13 @@ void NDTScanMatcher::callback_initial_pose_main( // check is_expected_frame_id const bool is_expected_frame_id = (initial_pose_msg_ptr->header.frame_id == param_.frame.map_frame); - diagnostics_initial_pose_->addKeyValue("is_expected_frame_id", is_expected_frame_id); + diagnostics_initial_pose_->add_key_value("is_expected_frame_id", is_expected_frame_id); if (!is_expected_frame_id) { std::stringstream message; message << "Received initial pose message with frame_id " << initial_pose_msg_ptr->header.frame_id << ", but expected " << param_.frame.map_frame << ". Please check the frame_id in the input topic and ensure it is correct."; - diagnostics_initial_pose_->updateLevelAndMessage( + diagnostics_initial_pose_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); return; } @@ -254,12 +281,12 @@ void NDTScanMatcher::callback_regularization_pose( { diagnostics_regularization_pose_->clear(); - diagnostics_regularization_pose_->addKeyValue( - "topic_time_stamp", static_cast(pose_conv_msg_ptr->header.stamp).seconds()); + diagnostics_regularization_pose_->add_key_value( + "topic_time_stamp", static_cast(pose_conv_msg_ptr->header.stamp).nanoseconds()); regularization_pose_buffer_->push_back(pose_conv_msg_ptr); - diagnostics_regularization_pose_->publish(); + diagnostics_regularization_pose_->publish(pose_conv_msg_ptr->header.stamp); } void NDTScanMatcher::callback_sensor_points( @@ -273,18 +300,18 @@ 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; - skipping_publish_num = is_succeed_scan_matching ? 0 : (skipping_publish_num + 1); - diagnostics_scan_points_->addKeyValue("skipping_publish_num", skipping_publish_num); - if (skipping_publish_num >= error_skipping_publish_num) { + 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 >= param_.validation.skipping_publish_num) { std::stringstream message; message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times)."; - diagnostics_scan_points_->updateLevelAndMessage( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + diagnostics_scan_points_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } - diagnostics_scan_points_->publish(); + diagnostics_scan_points_->publish(sensor_points_msg_in_sensor_frame->header.stamp); } bool NDTScanMatcher::callback_sensor_points_main( @@ -294,15 +321,15 @@ bool NDTScanMatcher::callback_sensor_points_main( // check topic_time_stamp const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; - diagnostics_scan_points_->addKeyValue("topic_time_stamp", sensor_ros_time.seconds()); + diagnostics_scan_points_->add_key_value("topic_time_stamp", sensor_ros_time.nanoseconds()); // check sensor_points_size const size_t sensor_points_size = sensor_points_msg_in_sensor_frame->width; - diagnostics_scan_points_->addKeyValue("sensor_points_size", sensor_points_size); + diagnostics_scan_points_->add_key_value("sensor_points_size", sensor_points_size); if (sensor_points_size == 0) { std::stringstream message; message << "Sensor points is empty."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -310,14 +337,14 @@ bool NDTScanMatcher::callback_sensor_points_main( // check sensor_points_delay_time_sec const double sensor_points_delay_time_sec = (this->now() - sensor_points_msg_in_sensor_frame->header.stamp).seconds(); - diagnostics_scan_points_->addKeyValue( + 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])."; - diagnostics_scan_points_->updateLevelAndMessage( + << "(the tolerance is " << param_.sensor_points.timeout_sec << "[sec])."; + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer, @@ -346,13 +373,13 @@ bool NDTScanMatcher::callback_sensor_points_main( std::stringstream message; message << ex.what() << ". Please publish TF " << sensor_frame << " to " << param_.frame.base_frame; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_scan_points_->addKeyValue("is_succeed_transform_sensor_points", false); + diagnostics_scan_points_->add_key_value("is_succeed_transform_sensor_points", false); return false; } - diagnostics_scan_points_->addKeyValue("is_succeed_transform_sensor_points", true); + diagnostics_scan_points_->add_key_value("is_succeed_transform_sensor_points", true); // check sensor_points_max_distance double max_distance = 0.0; @@ -361,12 +388,12 @@ bool NDTScanMatcher::callback_sensor_points_main( max_distance = std::max(max_distance, distance); } - diagnostics_scan_points_->addKeyValue("sensor_points_max_distance", max_distance); + diagnostics_scan_points_->add_key_value("sensor_points_max_distance", max_distance); if (max_distance < param_.sensor_points.required_distance) { std::stringstream message; message << "Max distance of sensor points = " << std::fixed << std::setprecision(3) << max_distance << " [m] < " << param_.sensor_points.required_distance << " [m]"; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -378,11 +405,11 @@ bool NDTScanMatcher::callback_sensor_points_main( ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); // check is_activated - diagnostics_scan_points_->addKeyValue("is_activated", static_cast(is_activated_)); + diagnostics_scan_points_->add_key_value("is_activated", static_cast(is_activated_)); if (!is_activated_) { std::stringstream message; message << "Node is not activated."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -393,12 +420,12 @@ bool NDTScanMatcher::callback_sensor_points_main( // check is_succeed_interpolate_initial_pose const bool is_succeed_interpolate_initial_pose = (interpolation_result_opt != std::nullopt); - diagnostics_scan_points_->addKeyValue( + diagnostics_scan_points_->add_key_value( "is_succeed_interpolate_initial_pose", is_succeed_interpolate_initial_pose); if (!is_succeed_interpolate_initial_pose) { std::stringstream message; message << "Couldn't interpolate pose. Please check the initial pose topic"; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -414,11 +441,11 @@ bool NDTScanMatcher::callback_sensor_points_main( // check is_set_map_points const bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr); - diagnostics_scan_points_->addKeyValue("is_set_map_points", is_set_map_points); + diagnostics_scan_points_->add_key_value("is_set_map_points", is_set_map_points); if (!is_set_map_points) { std::stringstream message; message << "Map points is not set."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -438,60 +465,91 @@ bool NDTScanMatcher::callback_sensor_points_main( } // check iteration_num - diagnostics_scan_points_->addKeyValue("iteration_num", ndt_result.iteration_num); + diagnostics_scan_points_->add_key_value("iteration_num", ndt_result.iteration_num); const bool is_ok_iteration_num = (ndt_result.iteration_num < ndt_ptr_->getMaximumIterations()); if (!is_ok_iteration_num) { std::stringstream message; message << "The number of iterations has reached its upper limit. The number of iterations: " << ndt_result.iteration_num << ", Limit: " << ndt_ptr_->getMaximumIterations() << "."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } // check local_optimal_solution_oscillation_num constexpr int oscillation_num_threshold = 10; const int oscillation_num = count_oscillation(transformation_msg_array); - diagnostics_scan_points_->addKeyValue("local_optimal_solution_oscillation_num", oscillation_num); + diagnostics_scan_points_->add_key_value( + "local_optimal_solution_oscillation_num", oscillation_num); const bool is_local_optimal_solution_oscillation = (oscillation_num > oscillation_num_threshold); if (is_local_optimal_solution_oscillation) { std::stringstream message; message << "There is a possibility of oscillation in a local minimum"; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } // check score - diagnostics_scan_points_->addKeyValue("transform_probability", ndt_result.transform_probability); - diagnostics_scan_points_->addKeyValue( + diagnostics_scan_points_->add_key_value( + "transform_probability", ndt_result.transform_probability); + diagnostics_scan_points_->add_key_value( "nearest_voxel_transformation_likelihood", ndt_result.nearest_voxel_transformation_likelihood); - std::string score_name = ""; double score = 0.0; double score_threshold = 0.0; if (param_.score_estimation.converged_param_type == ConvergedParamType::TRANSFORM_PROBABILITY) { - score_name = "Transform Probability"; score = ndt_result.transform_probability; score_threshold = param_.score_estimation.converged_param_transform_probability; } else if ( param_.score_estimation.converged_param_type == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { - score_name = "Nearest Voxel Transformation Likelihood"; score = ndt_result.nearest_voxel_transformation_likelihood; score_threshold = param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood; } else { std::stringstream message; message << "Unknown converged param type. Please check `score_estimation.converged_param_type`"; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); return false; } + // check score diff + const std::vector & tp_array = ndt_result.transform_probability_array; + if (static_cast(tp_array.size()) != ndt_result.iteration_num + 1) { + // only publish warning to /diagnostics, not skip publishing pose + std::stringstream message; + message << "transform_probability_array size is not equal to iteration_num + 1." + << " transform_probability_array size: " << tp_array.size() + << ", iteration_num: " << ndt_result.iteration_num; + diagnostics_scan_points_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } else { + const float diff = tp_array.back() - tp_array.front(); + diagnostics_scan_points_->add_key_value("transform_probability_diff", diff); + diagnostics_scan_points_->add_key_value("transform_probability_before", tp_array.front()); + } + const std::vector & nvtl_array = ndt_result.nearest_voxel_transformation_likelihood_array; + if (static_cast(nvtl_array.size()) != ndt_result.iteration_num + 1) { + // only publish warning to /diagnostics, not skip publishing pose + std::stringstream message; + message + << "nearest_voxel_transformation_likelihood_array size is not equal to iteration_num + 1." + << " nearest_voxel_transformation_likelihood_array size: " << nvtl_array.size() + << ", iteration_num: " << ndt_result.iteration_num; + diagnostics_scan_points_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } else { + const float diff = nvtl_array.back() - nvtl_array.front(); + diagnostics_scan_points_->add_key_value("nearest_voxel_transformation_likelihood_diff", diff); + diagnostics_scan_points_->add_key_value( + "nearest_voxel_transformation_likelihood_before", nvtl_array.front()); + } + bool is_ok_score = (score > score_threshold); if (!is_ok_score) { std::stringstream message; message << "Score is below the threshold. Score: " << score << ", Threshold: " << score_threshold; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); RCLCPP_WARN_STREAM(this->get_logger(), message.str()); } @@ -518,13 +576,12 @@ bool NDTScanMatcher::callback_sensor_points_main( // check distance_initial_to_result const auto distance_initial_to_result = static_cast( norm(interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position)); - diagnostics_scan_points_->addKeyValue("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) { + diagnostics_scan_points_->add_key_value("distance_initial_to_result", 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])."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } @@ -533,11 +590,11 @@ bool NDTScanMatcher::callback_sensor_points_main( const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0f; - diagnostics_scan_points_->addKeyValue("execution_time", exe_time); + diagnostics_scan_points_->add_key_value("execution_time", exe_time); if (exe_time > param_.validation.critical_upper_bound_exe_time_ms) { std::stringstream message; message << "NDT exe time is too long (took " << exe_time << " [ms])."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } @@ -558,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); @@ -609,15 +666,15 @@ void NDTScanMatcher::transform_sensor_measurement( geometry_msgs::msg::TransformStamped transform; try { transform = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { + } catch (const tf2::TransformException & ex) { throw; } 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); } @@ -629,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( @@ -673,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; @@ -702,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); @@ -750,28 +807,6 @@ int NDTScanMatcher::count_oscillation( return max_oscillation_cnt; } -std::array NDTScanMatcher::rotate_covariance( - const std::array & src_covariance, const Eigen::Matrix3d & rotation) const -{ - std::array ret_covariance = src_covariance; - - Eigen::Matrix3d src_cov; - src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], - src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], - src_covariance[14]; - - Eigen::Matrix3d ret_cov; - ret_cov = rotation * src_cov * rotation.transpose(); - - for (Eigen::Index i = 0; i < 3; ++i) { - ret_covariance[i] = ret_cov(0, i); - ret_covariance[i + 6] = ret_cov(1, i); - ret_covariance[i + 12] = ret_cov(2, i); - } - - return ret_covariance; -} - std::array NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) @@ -868,8 +903,10 @@ void NDTScanMatcher::service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_trigger_node_->clear(); - diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", this->now().seconds()); + diagnostics_trigger_node_->add_key_value("service_call_time_stamp", ros_time_now.nanoseconds()); is_activated_ = req->data; if (is_activated_) { @@ -877,38 +914,40 @@ void NDTScanMatcher::service_trigger_node( } res->success = true; - diagnostics_trigger_node_->addKeyValue("is_activated", static_cast(is_activated_)); - diagnostics_trigger_node_->addKeyValue("is_succeed_service", res->success); - diagnostics_trigger_node_->publish(); + diagnostics_trigger_node_->add_key_value("is_activated", static_cast(is_activated_)); + diagnostics_trigger_node_->add_key_value("is_succeed_service", res->success); + diagnostics_trigger_node_->publish(ros_time_now); } void NDTScanMatcher::service_ndt_align( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_ndt_align_->clear(); + diagnostics_ndt_align_->add_key_value("service_call_time_stamp", ros_time_now.nanoseconds()); + service_ndt_align_main(req, res); // check is_succeed_service bool is_succeed_service = res->success; - diagnostics_ndt_align_->addKeyValue("is_succeed_service", is_succeed_service); + diagnostics_ndt_align_->add_key_value("is_succeed_service", is_succeed_service); if (!is_succeed_service) { std::stringstream message; message << "ndt_align_service is failed."; - diagnostics_ndt_align_->updateLevelAndMessage( + diagnostics_ndt_align_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } - diagnostics_ndt_align_->publish(); + diagnostics_ndt_align_->publish(ros_time_now); } void NDTScanMatcher::service_ndt_align_main( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { - diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", this->now().seconds()); - // get TF from pose_frame to map_frame const std::string & target_frame = param_.frame.map_frame; const std::string & source_frame = req->pose_with_covariance.header.frame_id; @@ -921,17 +960,17 @@ void NDTScanMatcher::service_ndt_align_main( // "gnss_link" instead of "map". The ndt_align is designed to return identity when this issue // occurs. However, in the future, converting to a non-existent frame_id should be prohibited. - diagnostics_ndt_align_->addKeyValue("is_succeed_transform_initial_pose", false); + diagnostics_ndt_align_->add_key_value("is_succeed_transform_initial_pose", false); std::stringstream message; message << "Please publish TF " << target_frame.c_str() << " to " << source_frame.c_str(); - diagnostics_ndt_align_->updateLevelAndMessage( + diagnostics_ndt_align_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); res->success = false; return; } - diagnostics_ndt_align_->addKeyValue("is_succeed_transform_initial_pose", true); + diagnostics_ndt_align_->add_key_value("is_succeed_transform_initial_pose", true); // transform pose_frame to map_frame const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); @@ -943,11 +982,11 @@ void NDTScanMatcher::service_ndt_align_main( // check is_set_map_points bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr); - diagnostics_ndt_align_->addKeyValue("is_set_map_points", is_set_map_points); + diagnostics_ndt_align_->add_key_value("is_set_map_points", is_set_map_points); if (!is_set_map_points) { std::stringstream message; message << "No InputTarget. Please check the map file and the map_loader service"; - diagnostics_ndt_align_->updateLevelAndMessage( + diagnostics_ndt_align_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); res->success = false; @@ -956,11 +995,11 @@ void NDTScanMatcher::service_ndt_align_main( // check is_set_sensor_points bool is_set_sensor_points = (ndt_ptr_->getInputSource() != nullptr); - diagnostics_ndt_align_->addKeyValue("is_set_sensor_points", is_set_sensor_points); + diagnostics_ndt_align_->add_key_value("is_set_sensor_points", is_set_sensor_points); if (!is_set_sensor_points) { std::stringstream message; message << "No InputSource. Please check the input lidar topic"; - diagnostics_ndt_align_->updateLevelAndMessage( + diagnostics_ndt_align_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); res->success = false; @@ -1052,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); @@ -1068,7 +1107,10 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); - diagnostics_ndt_align_->addKeyValue("best_particle_score", best_particle_ptr->score); + diagnostics_ndt_align_->add_key_value("best_particle_score", best_particle_ptr->score); return result_pose_with_cov_msg; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(NDTScanMatcher) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp deleted file mode 100644 index 785055eef3700..0000000000000 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 "ndt_scan_matcher/ndt_scan_matcher_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); // NOLINT - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - auto ndt_scan_matcher = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor exec; - exec.add_node(ndt_scan_matcher); - exec.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/ndt_scan_matcher/test/test_util.hpp b/localization/ndt_scan_matcher/test/test_util.hpp index 2f78827abf891..dbf055d3c95d3 100644 --- a/localization/ndt_scan_matcher/test/test_util.hpp +++ b/localization/ndt_scan_matcher/test/test_util.hpp @@ -15,8 +15,13 @@ #ifndef TEST_UTIL_HPP_ #define TEST_UTIL_HPP_ +#include +#include + +#include #include #include +#include inline pcl::PointCloud make_sample_half_cubic_pcd() { diff --git a/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt index a2fbbddf7d120..ee63d9f43559a 100644 --- a/localization/pose2twist/CMakeLists.txt +++ b/localization/pose2twist/CMakeLists.txt @@ -4,11 +4,23 @@ project(pose2twist) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose2twist - src/pose2twist_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose2twist_core.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "Pose2Twist" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_angular_velocity + test/test_angular_velocity.cpp + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/pose2twist/README.md b/localization/pose2twist/README.md index 07d9c37b710fc..f1f7d6408fafb 100644 --- a/localization/pose2twist/README.md +++ b/localization/pose2twist/README.md @@ -5,7 +5,7 @@ This `pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging. The `twist.linear.x` is calculated as `sqrt(dx * dx + dy * dy + dz * dz) / dt`, and the values in the `y` and `z` fields are zero. -The `twist.angular` is calculated as `d_roll / dt`, `d_pitch / dt` and `d_yaw / dt` for each field. +The `twist.angular` is calculated as `relative_rotation_vector / dt` for each field. ## Inputs / Outputs diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp index c2a39f7e2ed3d..d1ff6ee5ff8b6 100644 --- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -21,14 +21,23 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +// Compute the relative rotation of q2 from q1 as a rotation vector +geometry_msgs::msg::Vector3 compute_relative_rotation_vector( + const tf2::Quaternion & q1, const tf2::Quaternion & q2); + class Pose2Twist : public rclcpp::Node { public: - Pose2Twist(); - ~Pose2Twist() = default; + explicit Pose2Twist(const rclcpp::NodeOptions & options); private: - void callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr); + void callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr); rclcpp::Subscription::SharedPtr pose_sub_; diff --git a/localization/pose2twist/launch/pose2twist.launch.xml b/localization/pose2twist/launch/pose2twist.launch.xml index a0fae57a163e6..57a41dbfcf017 100644 --- a/localization/pose2twist/launch/pose2twist.launch.xml +++ b/localization/pose2twist/launch/pose2twist.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index 16c49bb51c218..07e445c72978c 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -19,6 +19,7 @@ geometry_msgs rclcpp + rclcpp_components tf2_geometry_msgs tier4_debug_msgs diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index a321a06122816..cdde78ed7e357 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -14,17 +14,11 @@ #include "pose2twist/pose2twist_core.hpp" -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include -Pose2Twist::Pose2Twist() : Node("pose2twist_core") +Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose2twist", options) { using std::placeholders::_1; @@ -38,35 +32,27 @@ Pose2Twist::Pose2Twist() : Node("pose2twist_core") create_publisher("angular_z", durable_qos); // Note: this callback publishes topics above pose_sub_ = create_subscription( - "pose", queue_size, std::bind(&Pose2Twist::callbackPose, this, _1)); -} - -double calcDiffForRadian(const double lhs_rad, const double rhs_rad) -{ - double diff_rad = lhs_rad - rhs_rad; - if (diff_rad > M_PI) { - diff_rad = diff_rad - 2 * M_PI; - } else if (diff_rad < -M_PI) { - diff_rad = diff_rad + 2 * M_PI; - } - return diff_rad; + "pose", queue_size, std::bind(&Pose2Twist::callback_pose, this, _1)); } -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) +tf2::Quaternion get_quaternion(const geometry_msgs::msg::PoseStamped::SharedPtr & pose_stamped_ptr) { - geometry_msgs::msg::Vector3 rpy; - tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); - tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); - return rpy; + const auto & orientation = pose_stamped_ptr->pose.orientation; + return tf2::Quaternion{orientation.x, orientation.y, orientation.z, orientation.w}; } -geometry_msgs::msg::Vector3 getRPY(geometry_msgs::msg::PoseStamped::SharedPtr pose) +geometry_msgs::msg::Vector3 compute_relative_rotation_vector( + const tf2::Quaternion & q1, const tf2::Quaternion & q2) { - return getRPY(pose->pose); + // If we define q2 as the rotation obtained by applying dq after applying q1, + // then q2 = q1 * dq . + // Therefore, dq = q1.inverse() * q2 . + const tf2::Quaternion diff_quaternion = q1.inverse() * q2; + const tf2::Vector3 axis = diff_quaternion.getAxis() * diff_quaternion.getAngle(); + return geometry_msgs::msg::Vector3{}.set__x(axis.x()).set__y(axis.y()).set__z(axis.z()); } -geometry_msgs::msg::TwistStamped calcTwist( +geometry_msgs::msg::TwistStamped calc_twist( geometry_msgs::msg::PoseStamped::SharedPtr pose_a, geometry_msgs::msg::PoseStamped::SharedPtr pose_b) { @@ -79,18 +65,16 @@ geometry_msgs::msg::TwistStamped calcTwist( return twist; } - const auto pose_a_rpy = getRPY(pose_a); - const auto pose_b_rpy = getRPY(pose_b); + const auto pose_a_quaternion = get_quaternion(pose_a); + const auto pose_b_quaternion = get_quaternion(pose_b); geometry_msgs::msg::Vector3 diff_xyz; - geometry_msgs::msg::Vector3 diff_rpy; + const geometry_msgs::msg::Vector3 relative_rotation_vector = + compute_relative_rotation_vector(pose_a_quaternion, pose_b_quaternion); diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x; diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y; diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z; - diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x); - diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y); - diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z); geometry_msgs::msg::TwistStamped twist; twist.header = pose_b->header; @@ -99,33 +83,36 @@ geometry_msgs::msg::TwistStamped calcTwist( dt; twist.twist.linear.y = 0; twist.twist.linear.z = 0; - twist.twist.angular.x = diff_rpy.x / dt; - twist.twist.angular.y = diff_rpy.y / dt; - twist.twist.angular.z = diff_rpy.z / dt; + twist.twist.angular.x = relative_rotation_vector.x / dt; + twist.twist.angular.y = relative_rotation_vector.y / dt; + twist.twist.angular.z = relative_rotation_vector.z / dt; return twist; } -void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr) +void Pose2Twist::callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr) { // TODO(YamatoAndo) check time stamp diff // TODO(YamatoAndo) check suddenly move // TODO(YamatoAndo) apply low pass filter - geometry_msgs::msg::PoseStamped::SharedPtr current_pose_msg = pose_msg_ptr; + const geometry_msgs::msg::PoseStamped::SharedPtr & current_pose_msg = pose_msg_ptr; static geometry_msgs::msg::PoseStamped::SharedPtr prev_pose_msg = current_pose_msg; - geometry_msgs::msg::TwistStamped twist_msg = calcTwist(prev_pose_msg, current_pose_msg); + geometry_msgs::msg::TwistStamped twist_msg = calc_twist(prev_pose_msg, current_pose_msg); prev_pose_msg = current_pose_msg; twist_msg.header.frame_id = "base_link"; twist_pub_->publish(twist_msg); tier4_debug_msgs::msg::Float32Stamped linear_x_msg; linear_x_msg.stamp = this->now(); - linear_x_msg.data = twist_msg.twist.linear.x; + linear_x_msg.data = static_cast(twist_msg.twist.linear.x); linear_x_pub_->publish(linear_x_msg); tier4_debug_msgs::msg::Float32Stamped angular_z_msg; angular_z_msg.stamp = this->now(); - angular_z_msg.data = twist_msg.twist.angular.z; + angular_z_msg.data = static_cast(twist_msg.twist.angular.z); angular_z_pub_->publish(angular_z_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Pose2Twist) diff --git a/localization/pose2twist/src/pose2twist_node.cpp b/localization/pose2twist/src/pose2twist_node.cpp deleted file mode 100644 index 81f98461ecffd..0000000000000 --- a/localization/pose2twist/src/pose2twist_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 "pose2twist/pose2twist_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} diff --git a/localization/pose2twist/test/test_angular_velocity.cpp b/localization/pose2twist/test/test_angular_velocity.cpp new file mode 100644 index 0000000000000..bf2ca0a3ba5c2 --- /dev/null +++ b/localization/pose2twist/test/test_angular_velocity.cpp @@ -0,0 +1,115 @@ +// Copyright 2022 The Autoware Contributors +// +// 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 "pose2twist/pose2twist_core.hpp" + +#include + +// 1e-3 radian = 0.057 degrees +constexpr double acceptable_error = 1e-3; + +TEST(AngularVelocityFromQuaternion, CheckMultiplicationOrder) +{ + // If we define q2 as the rotation obtained by applying dq after applying q1, then q2 = q1 * dq . + // + // IT IS NOT q2 = dq * q1 . + // + // This test checks that the multiplication order is correct. + + const tf2::Vector3 target_vector(1, 2, 3); + // initial state + // Now, car is facing to the +x direction + // z + // ^ y + // | ^ + // | / + // | / + // car -> x + // + // + // + + tf2::Quaternion q1; + q1.setRPY(0., 0., M_PI / 2.); // yaw = 90 degrees + const tf2::Vector3 initially_rotated_vector = tf2::quatRotate(q1, target_vector); + // after applying q1 + // Now, car is facing to the +y direction + // z + // ^ + // | y + // | ^ + // | / + // <--car x + // + // + // + EXPECT_NEAR(initially_rotated_vector.x(), -2., acceptable_error); + EXPECT_NEAR(initially_rotated_vector.y(), 1., acceptable_error); + EXPECT_NEAR(initially_rotated_vector.z(), 3., acceptable_error); + + tf2::Quaternion dq; + dq.setRPY(0., M_PI / 2., 0.); // pitch = 90 degrees + const tf2::Vector3 finally_rotated_vector = tf2::quatRotate(q1 * dq, target_vector); + // after applying dq + // Now, car is facing to the -z direction + // z y + // ^ + // / + // / + // / + // <--car x + // | + // v + // + EXPECT_NEAR(finally_rotated_vector.x(), -2., acceptable_error); + EXPECT_NEAR(finally_rotated_vector.y(), 3., acceptable_error); + EXPECT_NEAR(finally_rotated_vector.z(), -1., acceptable_error); + + // Failure case + { + const tf2::Vector3 false_rotated_vector = tf2::quatRotate(dq * q1, target_vector); + + EXPECT_FALSE(std::abs(false_rotated_vector.x() - (-2)) < acceptable_error); + EXPECT_FALSE(std::abs(false_rotated_vector.y() - (3)) < acceptable_error); + EXPECT_FALSE(std::abs(false_rotated_vector.z() - (-1)) < acceptable_error); + } +} + +TEST(AngularVelocityFromQuaternion, CheckNumericalValidity) +{ + auto test = [](const tf2::Vector3 & expected_axis, const double expected_angle) -> void { + tf2::Quaternion expected_q; + expected_q.setRotation(expected_axis, expected_angle); + + // Create a random initial quaternion + tf2::Quaternion initial_q; + initial_q.setRPY(0.2, 0.3, 0.4); + + // Calculate the final quaternion by rotating the initial quaternion by the expected + // quaternion + const tf2::Quaternion final_q = initial_q * expected_q; + + // Calculate the relative rotation between the initial and final quaternion + const geometry_msgs::msg::Vector3 rotation_vector = + compute_relative_rotation_vector(initial_q, final_q); + + EXPECT_NEAR(rotation_vector.x, expected_axis.x() * expected_angle, acceptable_error); + EXPECT_NEAR(rotation_vector.y, expected_axis.y() * expected_angle, acceptable_error); + EXPECT_NEAR(rotation_vector.z, expected_axis.z() * expected_angle, acceptable_error); + }; + + test(tf2::Vector3(1.0, 0.0, 0.0).normalized(), 0.1); // 0.1 radian = 5.7 degrees + test(tf2::Vector3(1.0, 1.0, 0.0).normalized(), -0.2); // 0.2 radian = 11.4 degrees + test(tf2::Vector3(1.0, 2.0, 3.0).normalized(), 0.3); // 0.3 radian = 17.2 degrees +} diff --git a/localization/pose_estimator_arbiter/CMakeLists.txt b/localization/pose_estimator_arbiter/CMakeLists.txt index 9a47b654a6ab4..eefb7fc9a6879 100644 --- a/localization/pose_estimator_arbiter/CMakeLists.txt +++ b/localization/pose_estimator_arbiter/CMakeLists.txt @@ -4,29 +4,23 @@ project(pose_estimator_arbiter) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(glog REQUIRED) - find_package(PCL REQUIRED COMPONENTS common) include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) -# ============================== -# switch rule library -ament_auto_add_library(switch_rule - SHARED - src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp -) -target_include_directories(switch_rule PUBLIC src) - # ============================== # pose estimator arbiter node -ament_auto_add_executable(${PROJECT_NAME} +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp - src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp + src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC src) -target_link_libraries(${PROJECT_NAME} switch_rule glog::glog) -# ============================== +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "pose_estimator_arbiter::PoseEstimatorArbiter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -53,6 +47,7 @@ endif() add_subdirectory(example_rule) # ============================== -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch ) diff --git a/localization/pose_estimator_arbiter/README.md b/localization/pose_estimator_arbiter/README.md index 7e1ad2b9ec6eb..a28da699ad926 100644 --- a/localization/pose_estimator_arbiter/README.md +++ b/localization/pose_estimator_arbiter/README.md @@ -93,7 +93,7 @@ For switching rule: | Name | Type | Description | | ----------------------------- | ------------------------------------------------------------ | --------------------------------- | -| `/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | | `/input/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | localization final output | | `/input/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | localization initialization state | diff --git a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt index 333f92842b860..b2b5a828e42e7 100644 --- a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt +++ b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt @@ -7,7 +7,7 @@ ament_auto_add_library(example_rule src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp ) target_include_directories(example_rule PUBLIC src example_rule/src) -target_link_libraries(example_rule switch_rule) +target_link_libraries(example_rule pose_estimator_arbiter) # ============================== # define test definition macro @@ -16,7 +16,7 @@ function(add_testcase filepath) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" switch_rule example_rule) + target_link_libraries("${test_name}" pose_estimator_arbiter example_rule) target_include_directories(${test_name} PUBLIC src) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() 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 d6901f9be2dbf..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 @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -29,7 +29,7 @@ namespace pose_estimator_arbiter::rule_helper { class PoseEstimatorArea { - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; @@ -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 094434c62ac9c..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,18 +16,22 @@ #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()); // Register callback shared_data_->vector_map.register_callback( - [this](autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) -> void { + [this](autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) -> void { pose_estimator_area_->init(msg); }); 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 35ed8b04bfcad..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 @@ -18,7 +18,7 @@ #include -#include +#include #include @@ -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; @@ -59,7 +59,7 @@ TEST_F(MockNode, poseEstimatorArea) lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet_map->add(create_polygon3d()); - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using Point = geometry_msgs::msg::Point; HADMapBin msg; lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); @@ -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 a0a983e2ad3b7..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 @@ -16,7 +16,7 @@ #include -#include +#include #include #include @@ -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 { @@ -71,7 +71,7 @@ TEST_F(MockNode, vectorMapBasedRule) lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet_map->add(create_polygon3d()); - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; HADMapBin msg; lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); diff --git a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml index 0a708e3f48988..b5be96fc3ce44 100644 --- a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml +++ b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml index 480b323f3031d..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 @@ -12,26 +18,23 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_mapping_msgs + autoware_map_msgs + autoware_universe_utils diagnostic_msgs geometry_msgs - glog lanelet2_extension magic_enum pcl_conversions pcl_ros pluginlib + rclcpp_components sensor_msgs std_msgs std_srvs - tier4_autoware_utils ublox_msgs visualization_msgs yabloc_particle_filter - rclcpp - rclcpp_components - ament_cmake_ros ament_lint_auto autoware_lint_common 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 9e67dfc063964..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 @@ -42,18 +42,18 @@ class PoseEstimatorArbiter : public rclcpp::Node using Image = sensor_msgs::msg::Image; using PointCloud2 = sensor_msgs::msg::PointCloud2; using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; public: - PoseEstimatorArbiter(); + explicit PoseEstimatorArbiter(const rclcpp::NodeOptions & options); private: // 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 4fc3fd9b914a6..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 @@ -42,11 +42,11 @@ static std::unordered_set parse_estimator_name_args( return running_estimator_list; } -PoseEstimatorArbiter::PoseEstimatorArbiter() -: Node("pose_estimator_arbiter"), +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(); @@ -211,3 +211,6 @@ void PoseEstimatorArbiter::on_timer() } } // namespace pose_estimator_arbiter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pose_estimator_arbiter::PoseEstimatorArbiter) diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp deleted file mode 100644 index 20aaaf10abaab..0000000000000 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// 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 "pose_estimator_arbiter/pose_estimator_arbiter.hpp" - -#include - -int main(int argc, char * argv[]) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - } - - 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/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 b9c3bd45c9e24..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 @@ -16,7 +16,7 @@ #define POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ #include -#include +#include #include #include #include @@ -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. @@ -77,10 +77,10 @@ struct SharedData using Image = sensor_msgs::msg::Image; using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using PointCloud2 = sensor_msgs::msg::PointCloud2; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + 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/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt index 81a60a1d6b717..08d5bb47fca09 100644 --- a/localization/pose_initializer/CMakeLists.txt +++ b/localization/pose_initializer/CMakeLists.txt @@ -4,8 +4,7 @@ project(pose_initializer) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose_initializer_node - src/pose_initializer/pose_initializer_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_initializer/pose_initializer_core.cpp src/pose_initializer/gnss_module.cpp src/pose_initializer/ndt_module.cpp @@ -15,6 +14,12 @@ ament_auto_add_executable(pose_initializer_node src/pose_initializer/ndt_localization_trigger_module.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "PoseInitializer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + if(BUILD_TESTING) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) @@ -30,7 +35,8 @@ if(BUILD_TESTING) add_testcase(test/test_copy_vector_to_array.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch config ) diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 7006becf00c2f..36cc92e1d235b 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -17,9 +17,9 @@ This node depends on the map height fitter library. ### Services -| Name | Type | Description | -| -------------------------- | --------------------------------------------------- | --------------------- | -| `/localization/initialize` | autoware_adapi_v1_msgs::srv::InitializeLocalization | initial pose from api | +| Name | Type | Description | +| -------------------------- | ---------------------------------------------------- | --------------------- | +| `/localization/initialize` | tier4_localization_msgs::srv::InitializeLocalization | initial pose from api | ### Clients @@ -46,3 +46,86 @@ This node depends on the map height fitter library. This `pose_initializer` is used via default AD API. For detailed description of the API description, please refer to [the description of `default_ad_api`](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/localization.md). drawing + +## Initialize pose via CLI + +### Using the GNSS estimated position + +```bash +ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization +``` + +The GNSS estimated position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position. + +### Using the input position + +```bash +ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization " +pose_with_covariance: + - header: + frame_id: map + pose: + pose: + position: + x: 89571.1640625 + y: 42301.1875 + z: -3.1565165519714355 + orientation: + x: 0.0 + y: 0.0 + z: 0.28072773940524687 + w: 0.9597874433062874 + covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467] +method: 0 +" +``` + +The input position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position. + +### Direct initial position set + +```bash +ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization " +pose_with_covariance: + - header: + frame_id: map + pose: + pose: + position: + x: 89571.1640625 + y: 42301.1875 + z: -3.1565165519714355 + orientation: + x: 0.0 + y: 0.0 + z: 0.28072773940524687 + w: 0.9597874433062874 + covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467] +method: 1 +" +``` + +The initial position is set directly by the input position without going through localization algorithm. + +### Via ros2 topic pub + +```bash +ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped " +header: + frame_id: map +pose: + pose: + position: + x: 89571.1640625 + y: 42301.1875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.28072773940524687 + w: 0.9597874433062874 +" +``` + +It behaves the same as "initialpose (from rviz)". +The position.z and the covariance will be overwritten by [ad_api_adaptors](https://github.com/autowarefoundation/autoware.universe/tree/main/system/default_ad_api_helpers/ad_api_adaptors), so there is no need to input them. diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 3e4911e2c2936..2ffdebf39c474 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -9,7 +9,7 @@ - + diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 85c9c26bd6c8c..5f3ea72f7d400 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -19,14 +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/copy_vector_to_array.hpp b/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp index 1274092ae4993..33d95c58a726b 100644 --- a/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp +++ b/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp @@ -40,7 +40,7 @@ template std::array get_covariance_parameter(NodeT * node, const std::string & name) { const auto parameter = node->template declare_parameter>(name); - std::array covariance; + std::array covariance{}; copy_vector_to_array(parameter, covariance); return covariance; } diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.cpp b/localization/pose_initializer/src/pose_initializer/gnss_module.cpp index e8762c8c6f493..b746828e33b1c 100644 --- a/localization/pose_initializer/src/pose_initializer/gnss_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/gnss_module.cpp @@ -19,13 +19,18 @@ #include -GnssModule::GnssModule(rclcpp::Node * node) : fitter_(node) +GnssModule::GnssModule(rclcpp::Node * node) +: fitter_(node), + clock_(node->get_clock()), + timeout_(node->declare_parameter("gnss_pose_timeout")) { sub_gnss_pose_ = node->create_subscription( - "gnss_pose_cov", 1, [this](PoseWithCovarianceStamped::ConstSharedPtr msg) { pose_ = msg; }); + "gnss_pose_cov", 1, std::bind(&GnssModule::on_pose, this, std::placeholders::_1)); +} - clock_ = node->get_clock(); - timeout_ = node->declare_parameter("gnss_pose_timeout"); +void GnssModule::on_pose(PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + pose_ = msg; } geometry_msgs::msg::PoseWithCovarianceStamped GnssModule::get_pose() diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp index 8c3bc658e7e7a..fd490b00d0f70 100644 --- a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp @@ -30,6 +30,8 @@ class GnssModule PoseWithCovarianceStamped get_pose(); private: + void on_pose(PoseWithCovarianceStamped::ConstSharedPtr msg); + map_height_fitter::MapHeightFitter fitter_; rclcpp::Clock::SharedPtr clock_; rclcpp::Subscription::SharedPtr sub_gnss_pose_; diff --git a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp index 6e11f8fe74212..1f657545e687c 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp @@ -23,9 +23,9 @@ using ServiceException = component_interface_utils::ServiceException; using Initialize = localization_interface::Initialize; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -NdtModule::NdtModule(rclcpp::Node * node) : logger_(node->get_logger()) +NdtModule::NdtModule(rclcpp::Node * node) +: logger_(node->get_logger()), cli_align_(node->create_client("ndt_align")) { - cli_align_ = node->create_client("ndt_align"); } PoseWithCovarianceStamped NdtModule::align_pose(const PoseWithCovarianceStamped & pose) 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 bc86b5476dcca..7d75a1fc80f2e 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -23,9 +23,11 @@ #include "yabloc_module.hpp" #include +#include #include -PoseInitializer::PoseInitializer() : Node("pose_initializer") +PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) +: rclcpp::Node("pose_initializer", options) { const auto node = component_interface_utils::NodeAdaptor(this); group_srv_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -54,7 +56,7 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer") 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); @@ -65,28 +67,24 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer") throw std::invalid_argument( "Could not set user defined initial pose. The size of initial_pose is " + std::to_string(initial_pose_array.size()) + ". It must be 7."); - } else if ( + } + if ( std::abs(initial_pose_array[3]) < 1e-6 && std::abs(initial_pose_array[4]) < 1e-6 && std::abs(initial_pose_array[5]) < 1e-6 && std::abs(initial_pose_array[6]) < 1e-6) { throw std::invalid_argument("Input quaternion is invalid. All elements are close to zero."); - } else { - geometry_msgs::msg::Pose initial_pose; - initial_pose.position.x = initial_pose_array[0]; - initial_pose.position.y = initial_pose_array[1]; - initial_pose.position.z = initial_pose_array[2]; - initial_pose.orientation.x = initial_pose_array[3]; - initial_pose.orientation.y = initial_pose_array[4]; - initial_pose.orientation.z = initial_pose_array[5]; - initial_pose.orientation.w = initial_pose_array[6]; - - set_user_defined_initial_pose(initial_pose); } - } -} -PoseInitializer::~PoseInitializer() -{ - // to delete gnss module + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = initial_pose_array[0]; + initial_pose.position.y = initial_pose_array[1]; + initial_pose.position.z = initial_pose_array[2]; + initial_pose.orientation.x = initial_pose_array[3]; + initial_pose.orientation.y = initial_pose_array[4]; + initial_pose.orientation.z = initial_pose_array[5]; + initial_pose.orientation.w = initial_pose_array[6]; + + set_user_defined_initial_pose(initial_pose, true); + } } void PoseInitializer::change_state(State::Message::_state_type state) @@ -114,11 +112,12 @@ void PoseInitializer::change_node_trigger(bool flag, bool need_spin) } } -void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose) +void PoseInitializer::set_user_defined_initial_pose( + const geometry_msgs::msg::Pose initial_pose, bool need_spin) { try { change_state(State::Message::INITIALIZING); - change_node_trigger(false, true); + change_node_trigger(false, need_spin); PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; @@ -127,7 +126,7 @@ void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Po pose.pose.covariance = output_pose_covariance_; pub_reset_->publish(pose); - change_node_trigger(true, true); + change_node_trigger(true, need_spin); change_state(State::Message::INITIALIZED); RCLCPP_INFO(get_logger(), "Set user defined initial pose"); @@ -147,25 +146,52 @@ void PoseInitializer::on_initialize( Initialize::Service::Response::ERROR_UNSAFE, "The vehicle is not stopped."); } try { - change_state(State::Message::INITIALIZING); - change_node_trigger(false, false); - - auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front(); - if (ndt_) { - pose = ndt_->align_pose(pose); - } else if (yabloc_) { - // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more - // accuracy pose. - pose = yabloc_->align_pose(pose); - } - pose.pose.covariance = output_pose_covariance_; - pub_reset_->publish(pose); + if (req->method == Initialize::Service::Request::AUTO) { + change_state(State::Message::INITIALIZING); + change_node_trigger(false, false); + + auto pose = + req->pose_with_covariance.empty() ? get_gnss_pose() : req->pose_with_covariance.front(); + if (ndt_) { + pose = ndt_->align_pose(pose); + } else if (yabloc_) { + // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more + // accuracy pose. + pose = yabloc_->align_pose(pose); + } + pose.pose.covariance = output_pose_covariance_; + pub_reset_->publish(pose); + + change_node_trigger(true, false); + res->status.success = true; + change_state(State::Message::INITIALIZED); + + } else if (req->method == Initialize::Service::Request::DIRECT) { + if (req->pose_with_covariance.empty()) { + std::stringstream message; + message << "No input pose_with_covariance. If you want to use DIRECT method, please input " + "pose_with_covariance."; + RCLCPP_ERROR_STREAM(get_logger(), message.str()); + throw ServiceException( + autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); + } + auto pose = req->pose_with_covariance.front().pose.pose; + set_user_defined_initial_pose(pose, false); + res->status.success = true; - change_node_trigger(true, false); - res->status.success = true; - change_state(State::Message::INITIALIZED); + } else { + std::stringstream message; + message << "Unknown method type (=" << std::to_string(req->method) << ")"; + RCLCPP_ERROR_STREAM(get_logger(), message.str()); + throw ServiceException( + autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); + } } catch (const ServiceException & error) { - res->status = error.status(); + autoware_adapi_v1_msgs::msg::ResponseStatus respose_status; + respose_status = error.status(); + res->status.success = respose_status.success; + res->status.code = respose_status.code; + res->status.message = respose_status.message; change_state(State::Message::UNINITIALIZED); } } @@ -180,3 +206,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseInitializer::get_gnss_pose() throw ServiceException( Initialize::Service::Response::ERROR_GNSS_SUPPORT, "GNSS is not supported."); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(PoseInitializer) 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 623cfe50307f5..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 @@ -34,8 +34,7 @@ class NdtLocalizationTriggerModule; class PoseInitializer : public rclcpp::Node { public: - PoseInitializer(); - ~PoseInitializer(); + explicit PoseInitializer(const rclcpp::NodeOptions & options); private: using ServiceException = component_interface_utils::ServiceException; @@ -48,19 +47,20 @@ class PoseInitializer : public rclcpp::Node component_interface_utils::Publisher::SharedPtr pub_state_; component_interface_utils::Service::SharedPtr srv_initialize_; State::Message state_; - std::array output_pose_covariance_; - std::array gnss_particle_covariance_; + std::array output_pose_covariance_{}; + std::array gnss_particle_covariance_{}; std::unique_ptr gnss_; std::unique_ptr ndt_; std::unique_ptr yabloc_; 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); - void set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose); + void set_user_defined_initial_pose( + const geometry_msgs::msg::Pose initial_pose, bool need_spin = false); void change_state(State::Message::_state_type state); void on_initialize( const Initialize::Service::Request::SharedPtr req, diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp deleted file mode 100644 index c5b31c4e37ccd..0000000000000 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2020 Autoware Foundation -// -// 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 "pose_initializer_core.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); - return 0; -} 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_initializer/src/pose_initializer/yabloc_module.cpp b/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp index f79f1e58e8d62..204f2289adbf4 100644 --- a/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp @@ -23,9 +23,9 @@ using ServiceException = component_interface_utils::ServiceException; using Initialize = localization_interface::Initialize; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -YabLocModule::YabLocModule(rclcpp::Node * node) : logger_(node->get_logger()) +YabLocModule::YabLocModule(rclcpp::Node * node) +: logger_(node->get_logger()), cli_align_(node->create_client("yabloc_align")) { - cli_align_ = node->create_client("yabloc_align"); } PoseWithCovarianceStamped YabLocModule::align_pose(const PoseWithCovarianceStamped & pose) diff --git a/localization/pose_initializer/test/test_copy_vector_to_array.cpp b/localization/pose_initializer/test/test_copy_vector_to_array.cpp index 65b1d7e7d2711..f372a40076ffb 100644 --- a/localization/pose_initializer/test/test_copy_vector_to_array.cpp +++ b/localization/pose_initializer/test/test_copy_vector_to_array.cpp @@ -19,7 +19,7 @@ TEST(CopyVectorToArray, CopyAllElements) { const std::vector vector{0, 1, 2, 3, 4}; - std::array array; + std::array array{}; copy_vector_to_array(vector, array); EXPECT_THAT(array, testing::ElementsAre(0, 1, 2, 3, 4)); } @@ -28,7 +28,7 @@ TEST(CopyVectorToArray, CopyZeroElements) { const std::vector vector{}; // just confirm that this works - std::array array; + std::array array{}; copy_vector_to_array(vector, array); } @@ -36,7 +36,7 @@ TEST(CopyVectorToArray, ThrowsInvalidArgumentIfMoreElementsExpected) { auto f = [] { const std::vector vector{0, 1, 2, 3, 4}; - std::array array; + std::array array{}; copy_vector_to_array(vector, array); }; diff --git a/localization/pose_instability_detector/CMakeLists.txt b/localization/pose_instability_detector/CMakeLists.txt index 5270df636d791..c6f94ab7df16e 100644 --- a/localization/pose_instability_detector/CMakeLists.txt +++ b/localization/pose_instability_detector/CMakeLists.txt @@ -4,14 +4,19 @@ project(pose_instability_detector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose_instability_detector - src/main.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_instability_detector.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "PoseInstabilityDetector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_pose_instability_detector + ament_auto_add_gtest(test_${PROJECT_NAME} test/test.cpp src/pose_instability_detector.cpp ) @@ -19,6 +24,6 @@ endif() ament_auto_package( INSTALL_TO_SHARE - launch - config + launch + config ) diff --git a/localization/pose_instability_detector/README.md b/localization/pose_instability_detector/README.md index 89cf6ca3be684..4ced0fa8eb97b 100644 --- a/localization/pose_instability_detector/README.md +++ b/localization/pose_instability_detector/README.md @@ -1,20 +1,111 @@ # pose_instability_detector -The `pose_instability_detector` package includes a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). +The `pose_instability_detector` is a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). This node triggers periodic timer callbacks to compare two poses: -- The pose obtained by integrating the twist values from the last received message on `/localization/kinematic_state` over a duration specified by `interval_sec`. +- The pose calculated by dead reckoning starting from the pose of `/localization/kinematic_state` obtained `timer_period` seconds ago. - The latest pose from `/localization/kinematic_state`. The results of this comparison are then output to the `/diagnostics` topic. +![overview](./media/pose_instability_detector_overview.png) + +![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) + If this node outputs WARN messages to `/diagnostics`, it means that the EKF output is significantly different from the integrated twist values. +In other words, WARN outputs indicate that the vehicle has moved to a place outside the expected range based on the twist values. This discrepancy suggests that there may be an issue with either the estimated pose or the input twist. -The following diagram provides an overview of what the timeline of this process looks like: +The following diagram provides an overview of how the procedure looks like: + +![procedure](./media/pose_instabilty_detector_procedure.svg) + +## Dead reckoning algorithm + +Dead reckoning is a method of estimating the position of a vehicle based on its previous position and velocity. +The procedure for dead reckoning is as follows: + +1. Capture the necessary twist values from the `/input/twist` topic. +2. Integrate the twist values to calculate the pose transition. +3. Apply the pose transition to the previous pose to obtain the current pose. + +### Collecting twist values + +The `pose_instability_detector` node collects the twist values from the `~/input/twist` topic to perform dead reckoning. +Ideally, `pose_instability_detector` needs the twist values between the previous pose and the current pose. +Therefore, `pose_instability_detector` snips the twist buffer and apply interpolations and extrapolations to obtain the twist values at the desired time. + +![how_to_snip_necessary_twist](./media/how_to_snip_twist.png) + +### Linear transition and angular transition + +After the twist values are collected, the node calculates the linear transition and angular transition based on the twist values and add them to the previous pose. + +## Threshold definition + +The `pose_instability_detector` node compares the pose calculated by dead reckoning with the latest pose from the EKF output. +These two pose are ideally the same, but in reality, they are not due to the error in the twist values the pose observation. +If these two poses are significantly different so that the absolute value exceeds the threshold, the node outputs a WARN message to the `/diagnostics` topic. +There are six thresholds (x, y, z, roll, pitch, and yaw) to determine whether the poses are significantly different, and these thresholds are determined by the following subsections. + +### `diff_position_x` + +This threshold examines the difference in the longitudinal axis between the two poses, and check whether the vehicle goes beyond the expected error. +This threshold is a sum of "maximum longitudinal error due to velocity scale factor error" and "pose estimation error tolerance". + +$$ +\tau_x = v_{\rm max}\frac{\beta_v}{100} \Delta t + \epsilon_x\\ +$$ -![timeline](./media/timeline.drawio.svg) +| Symbol | Description | Unit | +| ------------- | -------------------------------------------------------------------------------- | ----- | +| $\tau_x$ | Threshold for the difference in the longitudinal axis | $m$ | +| $v_{\rm max}$ | Maximum velocity | $m/s$ | +| $\beta_v$ | Scale factor tolerance for the maximum velocity | $\%$ | +| $\Delta t$ | Time interval | $s$ | +| $\epsilon_x$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the longitudinal axis | $m$ | + +### `diff_position_y` and `diff_position_z` + +These thresholds examine the difference in the lateral and vertical axes between the two poses, and check whether the vehicle goes beyond the expected error. +The `pose_instability_detector` calculates the possible range where the vehicle goes, and get the maximum difference between the nominal dead reckoning pose and the maximum limit pose. + +![lateral_threshold_calculation](./media/lateral_threshold_calculation.png) + +Addition to this, the `pose_instability_detector` node considers the pose estimation error tolerance to determine the threshold. + +$$ +\tau_y = l + \epsilon_y +$$ + +| Symbol | Description | Unit | +| ------------ | ----------------------------------------------------------------------------------------------- | ---- | +| $\tau_y$ | Threshold for the difference in the lateral axis | $m$ | +| $l$ | Maximum lateral distance described in the image above (See the appendix how this is calculated) | $m$ | +| $\epsilon_y$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the lateral axis | $m$ | + +Note that `pose_instability_detector` sets the threshold for the vertical axis as the same as the lateral axis. Only the pose estimator error tolerance is different. + +### `diff_angle_x`, `diff_angle_y`, and `diff_angle_z` + +These thresholds examine the difference in the roll, pitch, and yaw angles between the two poses. +This threshold is a sum of "maximum angular error due to velocity scale factor error and bias error" and "pose estimation error tolerance". + +$$ +\tau_\phi = \tau_\theta = \tau_\psi = \left(\omega_{\rm max}\frac{\beta_\omega}{100} + b \right) \Delta t + \epsilon_\psi +$$ + +| Symbol | Description | Unit | +| ------------------ | ------------------------------------------------------------------------ | ------------- | +| $\tau_\phi$ | Threshold for the difference in the roll angle | ${\rm rad}$ | +| $\tau_\theta$ | Threshold for the difference in the pitch angle | ${\rm rad}$ | +| $\tau_\psi$ | Threshold for the difference in the yaw angle | ${\rm rad}$ | +| $\omega_{\rm max}$ | Maximum angular velocity | ${\rm rad}/s$ | +| $\beta_\omega$ | Scale factor tolerance for the maximum angular velocity | $\%$ | +| $b$ | Bias tolerance of the angular velocity | ${\rm rad}/s$ | +| $\Delta t$ | Time interval | $s$ | +| $\epsilon_\psi$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the yaw angle | ${\rm rad}$ | ## Parameters @@ -34,4 +125,44 @@ The following diagram provides an overview of what the timeline of this process | `~/debug/diff_pose` | geometry_msgs::msg::PoseStamped | diff_pose | | `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Diagnostics | -![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) +## Appendix + +On calculating the maximum lateral distance $l$, the `pose_instability_detector` node will estimate the following poses. + +| Pose | heading velocity $v$ | angular velocity $\omega$ | +| ------------------------------- | ------------------------------------------------ | -------------------------------------------------------------- | +| Nominal dead reckoning pose | $v_{\rm max}$ | $\omega_{\rm max}$ | +| Dead reckoning pose of corner A | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ | +| Dead reckoning pose of corner B | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ | +| Dead reckoning pose of corner C | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ | +| Dead reckoning pose of corner D | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ | + +Given a heading velocity $v$ and $\omega$, the 2D theoretical variation seen from the previous pose is calculated as follows: + +$$ +\begin{align*} +\left[ + \begin{matrix} + \Delta x\\ + \Delta y + \end{matrix} +\right] +&= +\left[ + \begin{matrix} + \int_{0}^{\Delta t} v \cos(\omega t) dt\\ + \int_{0}^{\Delta t} v \sin(\omega t) dt + \end{matrix} +\right] +\\ +&= +\left[ + \begin{matrix} + \frac{v}{\omega} \sin(\omega \Delta t)\\ + \frac{v}{\omega} \left(1 - \cos(\omega \Delta t)\right) + \end{matrix} +\right] +\end{align*} +$$ + +We calculate this variation for each corner and get the maximum value of the lateral distance $l$ by comparing the distance between the nominal dead reckoning pose and the corner poses. diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml index d94de020a4a12..d9b11b78885c9 100644 --- a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml +++ b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml @@ -1,9 +1,15 @@ /**: ros__parameters: - interval_sec: 0.5 # [sec] - threshold_diff_position_x: 1.0 # [m] - threshold_diff_position_y: 1.0 # [m] - threshold_diff_position_z: 1.0 # [m] - threshold_diff_angle_x: 1.0 # [rad] - threshold_diff_angle_y: 1.0 # [rad] - threshold_diff_angle_z: 1.0 # [rad] + timer_period: 0.5 # [sec] + + heading_velocity_maximum: 16.667 # [m/s] + heading_velocity_scale_factor_tolerance: 3.0 # [%] + + angular_velocity_maximum: 0.523 # [rad/s] + angular_velocity_scale_factor_tolerance: 0.2 # [%] + angular_velocity_bias_tolerance: 0.00698 # [rad/s] + + pose_estimator_longitudinal_tolerance: 0.11 # [m] + pose_estimator_lateral_tolerance: 0.11 # [m] + pose_estimator_vertical_tolerance: 0.11 # [m] + pose_estimator_angular_tolerance: 0.0175 # [rad] 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 new file mode 100644 index 0000000000000..febff8c2ee244 --- /dev/null +++ b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp @@ -0,0 +1,97 @@ +// Copyright 2023- Autoware Foundation +// +// 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__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ +#define AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +class PoseInstabilityDetector : public rclcpp::Node +{ + using Quaternion = geometry_msgs::msg::Quaternion; + using Twist = geometry_msgs::msg::Twist; + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using Odometry = nav_msgs::msg::Odometry; + using KeyValue = diagnostic_msgs::msg::KeyValue; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +public: + struct ThresholdValues + { + double position_x; + double position_y; + double position_z; + double angle_x; + double angle_y; + double angle_z; + }; + + explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + 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); + +private: + void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr); + void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); + void callback_timer(); + + static std::deque clip_out_necessary_twist( + const std::deque & twist_buffer, const rclcpp::Time & start_time, + const rclcpp::Time & end_time); + + // subscribers and timer + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::TimerBase::SharedPtr timer_; + + // publisher + rclcpp::Publisher::SharedPtr diff_pose_pub_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + // parameters + const double timer_period_; // [sec] + + const double heading_velocity_maximum_; // [m/s] + const double heading_velocity_scale_factor_tolerance_; // [%] + + const double angular_velocity_maximum_; // [rad/s] + const double angular_velocity_scale_factor_tolerance_; // [%] + const double angular_velocity_bias_tolerance_; // [rad/s] + + const double pose_estimator_longitudinal_tolerance_; // [m] + const double pose_estimator_lateral_tolerance_; // [m] + const double pose_estimator_vertical_tolerance_; // [m] + const double pose_estimator_angular_tolerance_; // [rad] + + // variables + std::optional latest_odometry_ = std::nullopt; + std::optional prev_odometry_ = std::nullopt; + std::deque twist_buffer_; +}; + +#endif // AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml index 4a390ee2854d7..5ebe7d7e429e0 100644 --- a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml +++ b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/localization/pose_instability_detector/media/how_to_snip_twist.png b/localization/pose_instability_detector/media/how_to_snip_twist.png new file mode 100644 index 0000000000000..6dc66a480e769 Binary files /dev/null and b/localization/pose_instability_detector/media/how_to_snip_twist.png differ diff --git a/localization/pose_instability_detector/media/lateral_threshold_calculation.png b/localization/pose_instability_detector/media/lateral_threshold_calculation.png new file mode 100644 index 0000000000000..cd919cdcb0383 Binary files /dev/null and b/localization/pose_instability_detector/media/lateral_threshold_calculation.png differ diff --git a/localization/pose_instability_detector/media/pose_instability_detector_overview.png b/localization/pose_instability_detector/media/pose_instability_detector_overview.png new file mode 100644 index 0000000000000..ea8f7a81700ae Binary files /dev/null and b/localization/pose_instability_detector/media/pose_instability_detector_overview.png differ diff --git a/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg new file mode 100644 index 0000000000000..ba45b1f52600b --- /dev/null +++ b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg @@ -0,0 +1,316 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Get necessary subsequence +
+ from twist buffer +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ Update +
+ + latest pose +
+ +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ Calculate integration of +
+ the twist subsequence +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
+ Calculate relative difference between +
+
+
+ + latest pose +
+ +
+
and
+
+ previous pose + twist integration +
+
+
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
+ Compare pose difference with thresholds +
+
+
+ Output results as + /diagnostics +
+
+
+
+
+ + + + + + + + + + + + +
+
+
+
+ + previous pose ← latest pose +
+ +
+
+
+
+ + + + + + + + + + + + +
+
+
+ Timer Callback +
+
+
+ + + + + + + + + + + + +
+
+
+ Odometry Subscription Callback +
+
+
+ + + + + + + diff --git a/localization/pose_instability_detector/media/rqt_runtime_monitor.png b/localization/pose_instability_detector/media/rqt_runtime_monitor.png index b3ad402e48ba7..9bad665db17e5 100644 Binary files a/localization/pose_instability_detector/media/rqt_runtime_monitor.png and b/localization/pose_instability_detector/media/rqt_runtime_monitor.png differ diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml index bf57e5589b747..04e9747d3723b 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/pose_instability_detector/package.xml @@ -18,13 +18,14 @@ autoware_cmake ament_index_cpp + autoware_universe_utils diagnostic_msgs geometry_msgs nav_msgs rclcpp + rclcpp_components tf2 tf2_geometry_msgs - tier4_autoware_utils tier4_debug_msgs ament_cmake_cppcheck diff --git a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json index 560d39a2d5bff..53380f8e7f252 100644 --- a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json +++ b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json @@ -1,62 +1,83 @@ { "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for Pose Instability Detector Nodes", + "title": "Parameters for Pose Instability Detector Node", "type": "object", "definitions": { "pose_instability_detector_node": { "type": "object", "properties": { - "interval_sec": { + "timer_period": { "type": "number", "default": 0.5, "exclusiveMinimum": 0, - "description": "The interval of timer_callback in seconds." + "description": "The period of timer_callback (sec)." }, - "threshold_diff_position_x": { + "heading_velocity_maximum": { "type": "number", - "default": 1.0, + "default": 16.667, "minimum": 0.0, - "description": "The threshold of diff_position x (m)." + "description": "The maximum of heading velocity (m/s)." }, - "threshold_diff_position_y": { + "heading_velocity_scale_factor_tolerance": { "type": "number", - "default": 1.0, + "default": 3.0, "minimum": 0.0, - "description": "The threshold of diff_position y (m)." + "description": "The tolerance of heading velocity scale factor (%)." }, - "threshold_diff_position_z": { + "angular_velocity_maximum": { "type": "number", - "default": 1.0, + "default": 0.523, "minimum": 0.0, - "description": "The threshold of diff_position z (m)." + "description": "The maximum of angular velocity (rad/s)." }, - "threshold_diff_angle_x": { + "angular_velocity_scale_factor_tolerance": { "type": "number", - "default": 1.0, + "default": 0.2, "minimum": 0.0, - "description": "The threshold of diff_angle x (rad)." + "description": "The tolerance of angular velocity scale factor (%)." }, - "threshold_diff_angle_y": { + "angular_velocity_bias_tolerance": { "type": "number", - "default": 1.0, + "default": 0.00698, "minimum": 0.0, - "description": "The threshold of diff_angle y (rad)." + "description": "The tolerance of angular velocity bias (rad/s)." }, - "threshold_diff_angle_z": { + "pose_estimator_longitudinal_tolerance": { "type": "number", - "default": 1.0, + "default": 0.11, "minimum": 0.0, - "description": "The threshold of diff_angle z (rad)." + "description": "The tolerance of longitudinal position of pose estimator (m)." + }, + "pose_estimator_lateral_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of lateral position of pose estimator (m)." + }, + "pose_estimator_vertical_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of vertical position of pose estimator (m)." + }, + "pose_estimator_angular_tolerance": { + "type": "number", + "default": 0.0175, + "minimum": 0.0, + "description": "The tolerance of roll angle of pose estimator (rad)." } }, "required": [ - "interval_sec", - "threshold_diff_position_x", - "threshold_diff_position_y", - "threshold_diff_position_z", - "threshold_diff_angle_x", - "threshold_diff_angle_y", - "threshold_diff_angle_z" + "timer_period", + "heading_velocity_maximum", + "heading_velocity_scale_factor_tolerance", + "angular_velocity_maximum", + "angular_velocity_scale_factor_tolerance", + "angular_velocity_bias_tolerance", + "pose_estimator_longitudinal_tolerance", + "pose_estimator_lateral_tolerance", + "pose_estimator_vertical_tolerance", + "pose_estimator_angular_tolerance" ] } }, diff --git a/localization/pose_instability_detector/src/main.cpp b/localization/pose_instability_detector/src/main.cpp deleted file mode 100644 index 34e679e86730f..0000000000000 --- a/localization/pose_instability_detector/src/main.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023- Autoware Foundation -// -// 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 "pose_instability_detector.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto pose_instability_detector = std::make_shared(); - rclcpp::spin(pose_instability_detector); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index afb7d6e007db2..28398588809eb 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -12,25 +12,40 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_instability_detector.hpp" +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include +#include +#include +#include #include PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) -: Node("pose_instability_detector", options), - threshold_diff_position_x_(this->declare_parameter("threshold_diff_position_x")), - threshold_diff_position_y_(this->declare_parameter("threshold_diff_position_y")), - threshold_diff_position_z_(this->declare_parameter("threshold_diff_position_z")), - threshold_diff_angle_x_(this->declare_parameter("threshold_diff_angle_x")), - threshold_diff_angle_y_(this->declare_parameter("threshold_diff_angle_y")), - threshold_diff_angle_z_(this->declare_parameter("threshold_diff_angle_z")) +: rclcpp::Node("pose_instability_detector", options), + timer_period_(this->declare_parameter("timer_period")), + heading_velocity_maximum_(this->declare_parameter("heading_velocity_maximum")), + heading_velocity_scale_factor_tolerance_( + this->declare_parameter("heading_velocity_scale_factor_tolerance")), + angular_velocity_maximum_(this->declare_parameter("angular_velocity_maximum")), + angular_velocity_scale_factor_tolerance_( + this->declare_parameter("angular_velocity_scale_factor_tolerance")), + angular_velocity_bias_tolerance_( + this->declare_parameter("angular_velocity_bias_tolerance")), + pose_estimator_longitudinal_tolerance_( + this->declare_parameter("pose_estimator_longitudinal_tolerance")), + pose_estimator_lateral_tolerance_( + this->declare_parameter("pose_estimator_lateral_tolerance")), + pose_estimator_vertical_tolerance_( + this->declare_parameter("pose_estimator_vertical_tolerance")), + pose_estimator_angular_tolerance_( + this->declare_parameter("pose_estimator_angular_tolerance")) { + // Define subscribers, publishers and a timer. odometry_sub_ = this->create_subscription( "~/input/odometry", 10, std::bind(&PoseInstabilityDetector::callback_odometry, this, std::placeholders::_1)); @@ -39,9 +54,8 @@ PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & opt "~/input/twist", 10, std::bind(&PoseInstabilityDetector::callback_twist, this, std::placeholders::_1)); - const double interval_sec = this->declare_parameter("interval_sec"); timer_ = rclcpp::create_timer( - this, this->get_clock(), std::chrono::duration(interval_sec), + this, this->get_clock(), std::chrono::duration(timer_period_), std::bind(&PoseInstabilityDetector::callback_timer, this)); diff_pose_pub_ = this->create_publisher("~/debug/diff_pose", 10); @@ -61,6 +75,7 @@ void PoseInstabilityDetector::callback_twist( void PoseInstabilityDetector::callback_timer() { + // odometry callback and timer callback has to be called at least once if (latest_odometry_ == std::nullopt) { return; } @@ -69,6 +84,16 @@ void PoseInstabilityDetector::callback_timer() return; } + // twist callback has to be called at least once + if (twist_buffer_.empty()) { + return; + } + + // time variables + const rclcpp::Time latest_odometry_time = rclcpp::Time(latest_odometry_->header.stamp); + const rclcpp::Time prev_odometry_time = rclcpp::Time(prev_odometry_->header.stamp); + + // define lambda function to convert quaternion to rpy auto quat_to_rpy = [](const Quaternion & quat) { tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); tf2::Matrix3x3 mat(tf2_quat); @@ -79,70 +104,48 @@ void PoseInstabilityDetector::callback_timer() return std::make_tuple(roll, pitch, yaw); }; - Pose pose = prev_odometry_->pose.pose; - rclcpp::Time prev_time = rclcpp::Time(prev_odometry_->header.stamp); - for (const TwistWithCovarianceStamped & twist_with_cov : twist_buffer_) { - const Twist twist = twist_with_cov.twist.twist; - - const rclcpp::Time curr_time = rclcpp::Time(twist_with_cov.header.stamp); - if (curr_time > latest_odometry_->header.stamp) { + // delete twist data older than prev_odometry_ (but preserve the one right before prev_odometry_) + while (twist_buffer_.size() > 1) { + if (rclcpp::Time(twist_buffer_[1].header.stamp) < prev_odometry_time) { + twist_buffer_.pop_front(); + } else { break; } + } - const rclcpp::Duration time_diff = curr_time - prev_time; - const double time_diff_sec = time_diff.seconds(); - if (time_diff_sec < 0.0) { - continue; - } - - // quat to rpy - auto [ang_x, ang_y, ang_z] = quat_to_rpy(pose.orientation); - - // rpy update - ang_x += twist.angular.x * time_diff_sec; - ang_y += twist.angular.y * time_diff_sec; - ang_z += twist.angular.z * time_diff_sec; - tf2::Quaternion quat; - quat.setRPY(ang_x, ang_y, ang_z); + // dead reckoning from prev_odometry_ to latest_odometry_ + PoseStamped::SharedPtr prev_pose = std::make_shared(); + prev_pose->header = prev_odometry_->header; + prev_pose->pose = prev_odometry_->pose.pose; - // Convert twist to world frame - tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); - linear_velocity = tf2::quatRotate(quat, linear_velocity); - - // update - pose.position.x += linear_velocity.x() * time_diff_sec; - pose.position.y += linear_velocity.y() * time_diff_sec; - pose.position.z += linear_velocity.z() * time_diff_sec; - pose.orientation.x = quat.x(); - pose.orientation.y = quat.y(); - pose.orientation.z = quat.z(); - pose.orientation.w = quat.w(); - prev_time = curr_time; - } + Pose::SharedPtr dr_pose = std::make_shared(); + dead_reckon(prev_pose, latest_odometry_time, twist_buffer_, dr_pose); - // compare pose and latest_odometry_ + // compare dead reckoning pose and latest_odometry_ const Pose latest_ekf_pose = latest_odometry_->pose.pose; - const Pose ekf_to_odom = tier4_autoware_utils::inverseTransformPose(pose, latest_ekf_pose); - const geometry_msgs::msg::Point pos = ekf_to_odom.position; - const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_odom.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}; - const rclcpp::Time stamp = latest_odometry_->header.stamp; - // publish diff_pose for debug PoseStamped diff_pose; - diff_pose.header = latest_odometry_->header; - diff_pose.pose = ekf_to_odom; + diff_pose.header.stamp = latest_odometry_time; + diff_pose.header.frame_id = "base_link"; + diff_pose.pose = ekf_to_dr; diff_pose_pub_->publish(diff_pose); - const std::vector thresholds = {threshold_diff_position_x_, threshold_diff_position_y_, - threshold_diff_position_z_, threshold_diff_angle_x_, - threshold_diff_angle_y_, threshold_diff_angle_z_}; + // publish diagnostics + ThresholdValues threshold_values = + calculate_threshold((latest_odometry_time - prev_odometry_time).seconds()); + + const std::vector thresholds = {threshold_values.position_x, threshold_values.position_y, + threshold_values.position_z, threshold_values.angle_x, + threshold_values.angle_y, threshold_values.angle_z}; const std::vector labels = {"diff_position_x", "diff_position_y", "diff_position_z", "diff_angle_x", "diff_angle_y", "diff_angle_z"}; - // publish diagnostics DiagnosticStatus status; status.name = "localization: pose_instability_detector"; status.hardware_id = this->get_name(); @@ -166,11 +169,240 @@ void PoseInstabilityDetector::callback_timer() status.message = (all_ok ? "OK" : "WARN"); DiagnosticArray diagnostics; - diagnostics.header.stamp = stamp; + diagnostics.header.stamp = latest_odometry_time; diagnostics.status.emplace_back(status); diagnostics_pub_->publish(diagnostics); // prepare for next loop prev_odometry_ = latest_odometry_; - twist_buffer_.clear(); } + +PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_threshold( + double interval_sec) const +{ + // Calculate maximum longitudinal difference + const double longitudinal_difference = + heading_velocity_maximum_ * heading_velocity_scale_factor_tolerance_ * 0.01 * interval_sec; + + // Calculate maximum lateral and vertical difference + double lateral_difference = 0.0; + + const std::vector heading_velocity_signs = {1.0, -1.0, -1.0, 1.0}; + const std::vector angular_velocity_signs = {1.0, 1.0, -1.0, -1.0}; + + const double nominal_variation_x = heading_velocity_maximum_ / angular_velocity_maximum_ * + sin(angular_velocity_maximum_ * interval_sec); + const double nominal_variation_y = heading_velocity_maximum_ / angular_velocity_maximum_ * + (1 - cos(angular_velocity_maximum_ * interval_sec)); + + for (int i = 0; i < 4; i++) { + const double edge_heading_velocity = + heading_velocity_maximum_ * + (1 + heading_velocity_signs[i] * heading_velocity_scale_factor_tolerance_ * 0.01); + const double edge_angular_velocity = + angular_velocity_maximum_ * + (1 + angular_velocity_signs[i] * angular_velocity_scale_factor_tolerance_ * 0.01) + + angular_velocity_signs[i] * angular_velocity_bias_tolerance_; + + const double edge_variation_x = + edge_heading_velocity / edge_angular_velocity * sin(edge_angular_velocity * interval_sec); + const double edge_variation_y = edge_heading_velocity / edge_angular_velocity * + (1 - cos(edge_angular_velocity * interval_sec)); + + const double diff_variation_x = edge_variation_x - nominal_variation_x; + const double diff_variation_y = edge_variation_y - nominal_variation_y; + + const double lateral_difference_candidate = abs( + diff_variation_x * sin(angular_velocity_maximum_ * interval_sec) - + diff_variation_y * cos(angular_velocity_maximum_ * interval_sec)); + lateral_difference = std::max(lateral_difference, lateral_difference_candidate); + } + + const double vertical_difference = lateral_difference; + + // Calculate maximum angular difference + const double roll_difference = + (angular_velocity_maximum_ * angular_velocity_scale_factor_tolerance_ * 0.01 + + angular_velocity_bias_tolerance_) * + interval_sec; + const double pitch_difference = roll_difference; + const double yaw_difference = roll_difference; + + // Set thresholds + 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_; + result_values.angle_x = roll_difference + pose_estimator_angular_tolerance_; + result_values.angle_y = pitch_difference + pose_estimator_angular_tolerance_; + result_values.angle_z = yaw_difference + pose_estimator_angular_tolerance_; + + return result_values; +} + +void PoseInstabilityDetector::dead_reckon( + PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, + const std::deque & twist_deque, Pose::SharedPtr & estimated_pose) +{ + // get start time + rclcpp::Time start_time = rclcpp::Time(initial_pose->header.stamp); + + // initialize estimated_pose + estimated_pose->position = initial_pose->pose.position; + estimated_pose->orientation = initial_pose->pose.orientation; + + // cut out necessary twist data + std::deque sliced_twist_deque = + clip_out_necessary_twist(twist_deque, start_time, end_time); + + // dead reckoning + rclcpp::Time prev_odometry_time = rclcpp::Time(sliced_twist_deque.front().header.stamp); + tf2::Quaternion prev_orientation; + tf2::fromMsg(estimated_pose->orientation, prev_orientation); + + for (size_t i = 1; i < sliced_twist_deque.size(); ++i) { + const rclcpp::Time curr_time = rclcpp::Time(sliced_twist_deque[i].header.stamp); + const double time_diff_sec = (curr_time - prev_odometry_time).seconds(); + + const Twist twist = sliced_twist_deque[i - 1].twist.twist; + + // variation of orientation (rpy update) + tf2::Quaternion delta_orientation; + tf2::Vector3 rotation_axis(twist.angular.x, twist.angular.y, twist.angular.z); + if (rotation_axis.length() > 0.0) { + delta_orientation.setRotation( + rotation_axis.normalized(), rotation_axis.length() * time_diff_sec); + } else { + delta_orientation.setValue(0.0, 0.0, 0.0, 1.0); + } + + tf2::Quaternion curr_orientation; + curr_orientation = prev_orientation * delta_orientation; + curr_orientation.normalize(); + + // average quaternion of two frames + tf2::Quaternion average_quat = prev_orientation.slerp(curr_orientation, 0.5); + + // Convert twist to world frame (take average of two frames) + tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); + linear_velocity = tf2::quatRotate(average_quat, linear_velocity); + + // xyz update + estimated_pose->position.x += linear_velocity.x() * time_diff_sec; + estimated_pose->position.y += linear_velocity.y() * time_diff_sec; + estimated_pose->position.z += linear_velocity.z() * time_diff_sec; + + // update previous variables + prev_odometry_time = curr_time; + prev_orientation = curr_orientation; + } + estimated_pose->orientation.x = prev_orientation.x(); + estimated_pose->orientation.y = prev_orientation.y(); + estimated_pose->orientation.z = prev_orientation.z(); + estimated_pose->orientation.w = prev_orientation.w(); +} + +std::deque +PoseInstabilityDetector::clip_out_necessary_twist( + const std::deque & twist_buffer, const rclcpp::Time & start_time, + const rclcpp::Time & end_time) +{ + // If there is only one element in the twist_buffer, return a deque that has the same twist + // from the start till the end + if (twist_buffer.size() == 1) { + TwistWithCovarianceStamped twist = twist_buffer.front(); + std::deque simple_twist_deque; + + twist.header.stamp = start_time; + simple_twist_deque.push_back(twist); + + twist.header.stamp = end_time; + simple_twist_deque.push_back(twist); + + return simple_twist_deque; + } + + // get iterator to the element that is right before start_time (if it does not exist, start_it = + // twist_buffer.begin()) + auto start_it = twist_buffer.begin(); + + for (auto it = twist_buffer.begin(); it != twist_buffer.end(); ++it) { + if (rclcpp::Time(it->header.stamp) > start_time) { + break; + } + start_it = it; + } + + // get iterator to the element that is right after end_time (if it does not exist, end_it = + // twist_buffer.end()) + auto end_it = twist_buffer.end(); + end_it--; + for (auto it = end_it; it != twist_buffer.begin(); --it) { + if (rclcpp::Time(it->header.stamp) < end_time) { + break; + } + end_it = it; + } + + // Create result deque + std::deque result_deque(start_it, end_it); + + // If the first element is later than start_time, add the first element to the front of the + // result_deque + if (rclcpp::Time(result_deque.front().header.stamp) > start_time) { + TwistWithCovarianceStamped start_twist = *start_it; + start_twist.header.stamp = start_time; + result_deque.push_front(start_twist); + } else { + // If the first element is earlier than start_time, interpolate the first element + rclcpp::Time time0 = rclcpp::Time(result_deque[0].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[1].header.stamp); + double ratio = (start_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[0].twist.twist; + Twist twist1 = result_deque[1].twist.twist; + result_deque[0].twist.twist.linear.x = twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[0].twist.twist.linear.y = twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[0].twist.twist.linear.z = twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[0].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[0].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[0].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[0].header.stamp = start_time; + } + + // If the last element is earlier than end_time, add the last element to the back of the + // result_deque + if (rclcpp::Time(result_deque.back().header.stamp) < end_time) { + TwistWithCovarianceStamped end_twist = *end_it; + end_twist.header.stamp = end_time; + result_deque.push_back(end_twist); + } else { + // If the last element is later than end_time, interpolate the last element + rclcpp::Time time0 = rclcpp::Time(result_deque[result_deque.size() - 2].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[result_deque.size() - 1].header.stamp); + double ratio = (end_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[result_deque.size() - 2].twist.twist; + Twist twist1 = result_deque[result_deque.size() - 1].twist.twist; + result_deque[result_deque.size() - 1].twist.twist.linear.x = + twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.y = + twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.z = + twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[result_deque.size() - 1].header.stamp = end_time; + } + return result_deque; +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(PoseInstabilityDetector) diff --git a/localization/pose_instability_detector/src/pose_instability_detector.hpp b/localization/pose_instability_detector/src/pose_instability_detector.hpp deleted file mode 100644 index 761a10b7a6bf7..0000000000000 --- a/localization/pose_instability_detector/src/pose_instability_detector.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2023- Autoware Foundation -// -// 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 POSE_INSTABILITY_DETECTOR_HPP_ -#define POSE_INSTABILITY_DETECTOR_HPP_ - -#include - -#include -#include -#include -#include - -#include - -class PoseInstabilityDetector : public rclcpp::Node -{ - using Quaternion = geometry_msgs::msg::Quaternion; - using Twist = geometry_msgs::msg::Twist; - using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; - using Pose = geometry_msgs::msg::Pose; - using PoseStamped = geometry_msgs::msg::PoseStamped; - using Odometry = nav_msgs::msg::Odometry; - using KeyValue = diagnostic_msgs::msg::KeyValue; - using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; - using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; - -public: - explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - -private: - void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr); - void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); - void callback_timer(); - - // subscribers and timer - rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Subscription::SharedPtr twist_sub_; - rclcpp::TimerBase::SharedPtr timer_; - - // publisher - rclcpp::Publisher::SharedPtr diff_pose_pub_; - rclcpp::Publisher::SharedPtr diagnostics_pub_; - - // parameters - const double threshold_diff_position_x_; - const double threshold_diff_position_y_; - const double threshold_diff_position_z_; - const double threshold_diff_angle_x_; - const double threshold_diff_angle_y_; - const double threshold_diff_angle_z_; - - // variables - std::optional latest_odometry_ = std::nullopt; - std::optional prev_odometry_ = std::nullopt; - std::vector twist_buffer_; -}; - -#endif // POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp index 5ea03859d7731..9300984967d4b 100644 --- a/localization/pose_instability_detector/test/test.cpp +++ b/localization/pose_instability_detector/test/test.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "../src/pose_instability_detector.hpp" +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" #include "test_message_helper_node.hpp" #include @@ -81,6 +81,11 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N timestamp.nanosec = 0; helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + // send the twist message1 (move 1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + // process the above message (by timer_callback) helper_->received_diagnostic_array_flag = false; while (!helper_->received_diagnostic_array_flag) { @@ -88,11 +93,6 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - // send the twist message1 (move 1m in x direction) - timestamp.sec = 0; - timestamp.nanosec = 5e8; - helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); - // send the twist message2 (move 1m in x direction) timestamp.sec = 1; timestamp.nanosec = 0; @@ -101,7 +101,9 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N // send the second odometry message (finish x = 12) timestamp.sec = 2; timestamp.nanosec = 0; - helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); // process the above messages (by timer_callback) helper_->received_diagnostic_array_flag = false; @@ -124,6 +126,11 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL timestamp.nanosec = 0; helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + // send the twist message1 (move 0.1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + // process the above message (by timer_callback) helper_->received_diagnostic_array_flag = false; while (!helper_->received_diagnostic_array_flag) { @@ -131,11 +138,6 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - // send the twist message1 (move 0.1m in x direction) - timestamp.sec = 0; - timestamp.nanosec = 5e8; - helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); - // send the twist message2 (move 0.1m in x direction) timestamp.sec = 1; timestamp.nanosec = 0; @@ -144,7 +146,9 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL // send the second odometry message (finish x = 12) timestamp.sec = 2; timestamp.nanosec = 0; - helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); // process the above messages (by timer_callback) helper_->received_diagnostic_array_flag = false; diff --git a/localization/stop_filter/CMakeLists.txt b/localization/stop_filter/CMakeLists.txt index 97a0443195ae5..4776a1f4967b2 100644 --- a/localization/stop_filter/CMakeLists.txt +++ b/localization/stop_filter/CMakeLists.txt @@ -4,11 +4,15 @@ project(stop_filter) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(stop_filter - src/stop_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/stop_filter.cpp ) -ament_target_dependencies(stop_filter) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "StopFilter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/stop_filter/include/stop_filter/stop_filter.hpp b/localization/stop_filter/include/stop_filter/stop_filter.hpp index 3c2b91590234f..e8428788820b2 100644 --- a/localization/stop_filter/include/stop_filter/stop_filter.hpp +++ b/localization/stop_filter/include/stop_filter/stop_filter.hpp @@ -37,7 +37,7 @@ class StopFilter : public rclcpp::Node { public: - StopFilter(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit StopFilter(const rclcpp::NodeOptions & options); private: rclcpp::Publisher::SharedPtr pub_odom_; //!< @brief odom publisher @@ -52,6 +52,6 @@ class StopFilter : public rclcpp::Node /** * @brief set odometry measurement */ - void callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); + void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg); }; #endif // STOP_FILTER__STOP_FILTER_HPP_ diff --git a/localization/stop_filter/launch/stop_filter.launch.xml b/localization/stop_filter/launch/stop_filter.launch.xml index 0ea92d26c9677..c53b37efc9954 100644 --- a/localization/stop_filter/launch/stop_filter.launch.xml +++ b/localization/stop_filter/launch/stop_filter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index 83eaf1b9fa821..2e6d5c4f8bbe1 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -21,6 +21,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components tf2 tier4_debug_msgs diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index ac0960b8cb67b..c1c4de2fb6b6e 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -24,20 +24,20 @@ using std::placeholders::_1; -StopFilter::StopFilter(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options) +StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("stop_filter", node_options) { vx_threshold_ = declare_parameter("vx_threshold"); wz_threshold_ = declare_parameter("wz_threshold"); sub_odom_ = create_subscription( - "input/odom", 1, std::bind(&StopFilter::callbackOdometry, this, _1)); + "input/odom", 1, std::bind(&StopFilter::callback_odometry, this, _1)); pub_odom_ = create_publisher("output/odom", 1); pub_stop_flag_ = create_publisher("debug/stop_flag", 1); } -void StopFilter::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) +void StopFilter::callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) { tier4_debug_msgs::msg::BoolStamped stop_flag_msg; stop_flag_msg.stamp = msg->header.stamp; @@ -57,3 +57,6 @@ void StopFilter::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) pub_stop_flag_->publish(stop_flag_msg); pub_odom_->publish(odom_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(StopFilter) diff --git a/localization/stop_filter/src/stop_filter_node.cpp b/localization/stop_filter/src/stop_filter_node.cpp deleted file mode 100644 index 9748214828de2..0000000000000 --- a/localization/stop_filter/src/stop_filter_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2021 TierIV -// -// 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 "stop_filter/stop_filter.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("stop_filter", node_options); - - rclcpp::spin(node); - - return 0; -} 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/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp b/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp deleted file mode 100644 index 30d36e7150113..0000000000000 --- a/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// 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 TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ -#define TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ - -/* -A implementation of tree-structured parzen estimator (TPE) -See below pdf for the TPE algorithm detail. -https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf - -Optuna is also used as a reference for implementation. -https://github.com/optuna/optuna -*/ - -#include -#include -#include - -class TreeStructuredParzenEstimator -{ -public: - using Input = std::vector; - using Score = double; - struct Trial - { - Input input; - Score score; - }; - - enum Direction { - MINIMIZE = 0, - MAXIMIZE = 1, - }; - - enum Index { - TRANS_X = 0, - TRANS_Y = 1, - TRANS_Z = 2, - ANGLE_X = 3, - ANGLE_Y = 4, - ANGLE_Z = 5, - INDEX_NUM = 6, - }; - - TreeStructuredParzenEstimator() = delete; - TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, - const std::vector & sample_mean, const std::vector & sample_stddev); - void add_trial(const Trial & trial); - Input get_next_input() const; - -private: - 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; - - std::vector trials_; - int64_t above_num_; - const Direction direction_; - const int64_t n_startup_trials_; - const int64_t input_dimension_; - const std::vector sample_mean_; - const std::vector sample_stddev_; - Input base_stddev_; -}; - -#endif // TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ 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/tree_structured_parzen_estimator/test/test_tpe.cpp b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp deleted file mode 100644 index f8a697878d6a3..0000000000000 --- a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// 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 "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" - -#include - -TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) -{ - auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { - double value = 0.0; - const int64_t n = input.size(); - for (int64_t i = 0; i < n; i++) { - const double v = input[i] * 10; - value += v * v; - } - return value; - }; - - constexpr int64_t kOuterTrialsNum = 20; - constexpr int64_t kInnerTrialsNum = 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"); - - 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"); - - std::vector scores; - for (int64_t i = 0; i < kOuterTrialsNum; 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++) { - const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); - const double score = -sphere_function(input); - estimator.add_trial({input, score}); - best_score = std::max(best_score, score); - } - scores.push_back(best_score); - } - - const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); - const double mean = sum / 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()); - - std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; - } - ASSERT_LT(mean_scores[0], mean_scores[1]); -} diff --git a/localization/twist2accel/CMakeLists.txt b/localization/twist2accel/CMakeLists.txt index 59f23eacb2fb3..9178bf02288a8 100644 --- a/localization/twist2accel/CMakeLists.txt +++ b/localization/twist2accel/CMakeLists.txt @@ -4,11 +4,15 @@ project(twist2accel) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(twist2accel - src/twist2accel_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/twist2accel.cpp ) -ament_target_dependencies(twist2accel) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "Twist2Accel" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/twist2accel/include/twist2accel/twist2accel.hpp b/localization/twist2accel/include/twist2accel/twist2accel.hpp index da5f2b4a3b228..e2fab219b11b4 100644 --- a/localization/twist2accel/include/twist2accel/twist2accel.hpp +++ b/localization/twist2accel/include/twist2accel/twist2accel.hpp @@ -40,7 +40,7 @@ class Twist2Accel : public rclcpp::Node { public: - Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit Twist2Accel(const rclcpp::NodeOptions & options); private: rclcpp::Publisher::SharedPtr @@ -63,9 +63,9 @@ class Twist2Accel : public rclcpp::Node /** * @brief set odometry measurement */ - void callbackTwistWithCovariance( + void callback_twist_with_covariance( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - void callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); - void estimateAccel(const geometry_msgs::msg::TwistStamped::SharedPtr msg); + void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg); + void estimate_accel(const geometry_msgs::msg::TwistStamped::SharedPtr msg); }; #endif // TWIST2ACCEL__TWIST2ACCEL_HPP_ diff --git a/localization/twist2accel/launch/twist2accel.launch.xml b/localization/twist2accel/launch/twist2accel.launch.xml index c4c9a3ed50bfc..c534a288aac3e 100644 --- a/localization/twist2accel/launch/twist2accel.launch.xml +++ b/localization/twist2accel/launch/twist2accel.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index 08dacf9185769..0dbce08f309ac 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -21,6 +21,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components signal_processing tf2 tier4_debug_msgs diff --git a/localization/twist2accel/src/twist2accel.cpp b/localization/twist2accel/src/twist2accel.cpp index 68019f27e95fe..800d696b1dea8 100644 --- a/localization/twist2accel/src/twist2accel.cpp +++ b/localization/twist2accel/src/twist2accel.cpp @@ -24,13 +24,13 @@ using std::placeholders::_1; -Twist2Accel::Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options) +Twist2Accel::Twist2Accel(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("twist2accel", node_options) { sub_odom_ = create_subscription( - "input/odom", 1, std::bind(&Twist2Accel::callbackOdometry, this, _1)); + "input/odom", 1, std::bind(&Twist2Accel::callback_odometry, this, _1)); sub_twist_ = create_subscription( - "input/twist", 1, std::bind(&Twist2Accel::callbackTwistWithCovariance, this, _1)); + "input/twist", 1, std::bind(&Twist2Accel::callback_twist_with_covariance, this, _1)); pub_accel_ = create_publisher("output/accel", 1); @@ -46,17 +46,17 @@ Twist2Accel::Twist2Accel(const std::string & node_name, const rclcpp::NodeOption lpf_aaz_ptr_ = std::make_shared(accel_lowpass_gain_); } -void Twist2Accel::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) +void Twist2Accel::callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) { if (!use_odom_) return; geometry_msgs::msg::TwistStamped twist; twist.header = msg->header; twist.twist = msg->twist.twist; - estimateAccel(std::make_shared(twist)); + estimate_accel(std::make_shared(twist)); } -void Twist2Accel::callbackTwistWithCovariance( +void Twist2Accel::callback_twist_with_covariance( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { if (use_odom_) return; @@ -64,10 +64,10 @@ void Twist2Accel::callbackTwistWithCovariance( geometry_msgs::msg::TwistStamped twist; twist.header = msg->header; twist.twist = msg->twist.twist; - estimateAccel(std::make_shared(twist)); + estimate_accel(std::make_shared(twist)); } -void Twist2Accel::estimateAccel(const geometry_msgs::msg::TwistStamped::SharedPtr msg) +void Twist2Accel::estimate_accel(const geometry_msgs::msg::TwistStamped::SharedPtr msg) { geometry_msgs::msg::AccelWithCovarianceStamped accel_msg; accel_msg.header = msg->header; @@ -103,3 +103,6 @@ void Twist2Accel::estimateAccel(const geometry_msgs::msg::TwistStamped::SharedPt pub_accel_->publish(accel_msg); prev_twist_ptr_ = msg; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Twist2Accel) diff --git a/localization/twist2accel/src/twist2accel_node.cpp b/localization/twist2accel/src/twist2accel_node.cpp deleted file mode 100644 index ce8ed31db5c32..0000000000000 --- a/localization/twist2accel/src/twist2accel_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2022 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 "twist2accel/twist2accel.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("twist2accel", node_options); - - rclcpp::spin(node); - - return 0; -} diff --git a/localization/yabloc/yabloc_common/CMakeLists.txt b/localization/yabloc/yabloc_common/CMakeLists.txt index c8474a85ad963..ca2f28bae7cda 100644 --- a/localization/yabloc/yabloc_common/CMakeLists.txt +++ b/localization/yabloc/yabloc_common/CMakeLists.txt @@ -23,9 +23,6 @@ find_package(Sophus REQUIRED) # because it rewrite CMAKE_NO_SYSTEM_FROM_IMPORTED to TRUE. set(CMAKE_NO_SYSTEM_FROM_IMPORTED FALSE) -# glog -find_package(glog REQUIRED) - # =================================================== # GeographicLib find_package(PkgConfig) @@ -45,7 +42,10 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/static_tf_subscriber.cpp src/extract_line_segments.cpp src/transform_line_segments.cpp - src/color.cpp) + src/color.cpp + src/ground_server/ground_server_core.cpp + src/ground_server/polygon_operation.cpp + src/ll2_decomposer/ll2_decomposer_core.cpp) target_include_directories( ${PROJECT_NAME} PUBLIC include ) @@ -63,23 +63,18 @@ target_link_libraries(${PROJECT_NAME} Geographic ${PCL_LIBRARIES} Sophus::Sophus # =================================================== # Executables # ground_server -set(TARGET ground_server_node) -ament_auto_add_executable(${TARGET} - src/ground_server/ground_server_core.cpp - src/ground_server/ground_server_node.cpp - src/ground_server/polygon_operation.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 glog::glog) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::ground_server::GroundServer" + EXECUTABLE yabloc_ground_server_node + EXECUTOR SingleThreadedExecutor +) # ll2_decomposer -set(TARGET ll2_decomposer_node) -ament_auto_add_executable(${TARGET} - src/ll2_decomposer/ll2_decomposer_core.cpp - src/ll2_decomposer/ll2_decomposer_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}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::ll2_decomposer::Ll2Decomposer" + EXECUTABLE yabloc_ll2_decomposer_node + EXECUTOR SingleThreadedExecutor +) # =================================================== ament_export_dependencies(PCL Sophus) diff --git a/localization/yabloc/yabloc_common/README.md b/localization/yabloc/yabloc_common/README.md index 6368305bdbfad..cb1799ce21e61 100644 --- a/localization/yabloc/yabloc_common/README.md +++ b/localization/yabloc/yabloc_common/README.md @@ -15,10 +15,10 @@ It estimates the height and tilt of the ground from lanelet2. #### Input -| Name | Type | Description | -| ------------------ | -------------------------------------------- | ------------------- | -| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | -| `input/pose` | `geometry_msgs::msg::PoseStamped` | estimated self pose | +| Name | Type | Description | +| ------------------ | --------------------------------------- | ------------------- | +| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | +| `input/pose` | `geometry_msgs::msg::PoseStamped` | estimated self pose | #### Output @@ -44,9 +44,9 @@ This node extracts the elements related to the road surface markings and yabloc #### Input -| Name | Type | Description | -| ------------------ | -------------------------------------------- | ----------- | -| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| Name | Type | Description | +| ------------------ | --------------------------------------- | ----------- | +| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | #### Output 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 154507f8195d9..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 @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include @@ -44,7 +44,7 @@ class GroundServer : public rclcpp::Node { public: using GroundPlane = common::GroundPlane; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; @@ -57,15 +57,15 @@ class GroundServer : public rclcpp::Node using String = std_msgs::msg::String; using PointCloud2 = sensor_msgs::msg::PointCloud2; using Point = geometry_msgs::msg::Point; - GroundServer(); + explicit GroundServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const bool force_zero_tilt_; - const float R; - const int K; + const float R_; + const int K_; // Subscriber - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_pose_stamped_; rclcpp::Subscription::SharedPtr sub_initial_pose_; // Publisher @@ -86,7 +86,7 @@ class GroundServer : public rclcpp::Node std::vector last_indices_; // Callback - void on_map(const HADMapBin & msg); + void on_map(const LaneletMapBin & msg); void on_initial_pose(const PoseCovStamped & msg); void on_pose_stamped(const PoseStamped & msg); 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 f6e6de6b38fdd..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 @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -35,11 +35,11 @@ class Ll2Decomposer : public rclcpp::Node { public: using Cloud2 = sensor_msgs::msg::PointCloud2; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; - Ll2Decomposer(); + explicit Ll2Decomposer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: rclcpp::Publisher::SharedPtr pub_road_marking_; @@ -47,24 +47,25 @@ class Ll2Decomposer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_bounding_box_; rclcpp::Publisher::SharedPtr pub_marker_; - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_map_; std::set road_marking_labels_; std::set sign_board_labels_; std::set bounding_box_labels_; - void on_map(const HADMapBin & msg); + 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/launch/yabloc_common.launch.xml b/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml index 48299e880cc9c..29d3da71bce7f 100644 --- a/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml +++ b/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml @@ -1,5 +1,7 @@ + + @@ -7,7 +9,7 @@ - + @@ -25,7 +27,7 @@ - + diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index 213057f428c38..18038d80fbfca 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -16,14 +16,15 @@ autoware_cmake rosidl_default_generators - autoware_auto_mapping_msgs + autoware_map_msgs + autoware_universe_utils cv_bridge geometry_msgs - glog lanelet2_core lanelet2_extension pcl_conversions rclcpp + rclcpp_components sensor_msgs signal_processing sophus 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 9987c4fbb5e72..f440c5da683a3 100644 --- a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp +++ b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp @@ -20,12 +20,13 @@ pcl::PointCloud extract_near_line_segments( const pcl::PointCloud & line_segments, const Sophus::SE3f & transform, const float max_range) { - constexpr double sqrt_two = std::sqrt(2); - 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. - auto check_intersection = [max_range, pose_vector](const pcl::PointNormal & pn) -> bool { + auto check_intersection = [sqrt_two, max_range, + pose_vector](const pcl::PointNormal & pn) -> bool { const Eigen::Vector3f from = pn.getVector3fMap() - pose_vector; const Eigen::Vector3f to = pn.getNormalVector3fMap() - pose_vector; @@ -41,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 749aa39318923..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() -: Node("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; @@ -43,7 +45,7 @@ GroundServer::GroundServer() auto on_pose = std::bind(&GroundServer::on_pose_stamped, this, _1); auto on_map = std::bind(&GroundServer::on_map, this, _1); - sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); + sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); sub_pose_stamped_ = create_subscription("~/input/pose", 10, on_pose); pub_ground_height_ = create_publisher("~/output/height", 10); @@ -100,7 +102,7 @@ void GroundServer::on_pose_stamped(const PoseStamped & msg) } } -void GroundServer::on_map(const HADMapBin & msg) +void GroundServer::on_map(const LaneletMapBin & msg) { lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(msg, lanelet_map); @@ -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(); } @@ -248,3 +251,6 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point) } } // namespace yabloc::ground_server + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ground_server::GroundServer) diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp deleted file mode 100644 index 4e032ff72c998..0000000000000 --- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp +++ /dev/null @@ -1,29 +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_common/ground_server/ground_server.hpp" - -#include - -int main(int argc, char ** argv) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); -} 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 5cf3e905e447d..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 @@ -24,7 +24,7 @@ namespace yabloc::ll2_decomposer { -Ll2Decomposer::Ll2Decomposer() : Node("ll2_to_image") +Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to_image", options) { using std::placeholders::_1; const rclcpp::QoS latch_qos = rclcpp::QoS(10).transient_local(); @@ -38,13 +38,13 @@ Ll2Decomposer::Ll2Decomposer() : Node("ll2_to_image") // Subscriber auto cb_map = std::bind(&Ll2Decomposer::on_map, this, _1); - sub_map_ = create_subscription("~/input/vector_map", map_qos, cb_map); + sub_map_ = create_subscription("~/input/vector_map", map_qos, cb_map); auto load_lanelet2_labels = [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); } @@ -102,7 +102,7 @@ pcl::PointCloud Ll2Decomposer::load_bounding_boxes( return cloud; } -void Ll2Decomposer::on_map(const HADMapBin & msg) +void Ll2Decomposer::on_map(const LaneletMapBin & msg) { RCLCPP_INFO_STREAM(get_logger(), "subscribed binary vector map"); lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); @@ -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 = common::Color(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 = common::Color(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++; @@ -263,3 +263,6 @@ void Ll2Decomposer::publish_additional_marker(const lanelet::LaneletMapPtr & lan } } // namespace yabloc::ll2_decomposer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ll2_decomposer::Ll2Decomposer) diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp deleted file mode 100644 index 692f6a5da0913..0000000000000 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp +++ /dev/null @@ -1,23 +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_common/ll2_decomposer/ll2_decomposer.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} 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/CMakeLists.txt b/localization/yabloc/yabloc_image_processing/CMakeLists.txt index 58437c085c4b2..2af75ab237585 100644 --- a/localization/yabloc/yabloc_image_processing/CMakeLists.txt +++ b/localization/yabloc/yabloc_image_processing/CMakeLists.txt @@ -14,56 +14,55 @@ find_package(OpenCV REQUIRED) # PCL find_package(PCL REQUIRED COMPONENTS common) +ament_auto_add_library(${PROJECT_NAME} SHARED + src/line_segment_detector/line_segment_detector_core.cpp + src/graph_segment/graph_segment_core.cpp + src/graph_segment/similar_area_searcher.cpp + src/segment_filter/segment_filter_core.cpp + src/undistort/undistort_node.cpp + src/line_segments_overlay/line_segments_overlay_core.cpp + src/lanelet2_overlay/lanelet2_overlay_core.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} ${OpenCV_LIBS}) + # =================================================== # Executable -# line segment detector -set(TARGET line_segment_detector_node) -ament_auto_add_executable(${TARGET} - src/line_segment_detector/line_segment_detector_node.cpp - src/line_segment_detector/line_segment_detector_core.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::graph_segment::GraphSegment" + EXECUTABLE yabloc_graph_segment_node + EXECUTOR SingleThreadedExecutor +) -# graph based segmentation -set(TARGET graph_segment_node) -ament_auto_add_executable(${TARGET} - src/graph_segment/graph_segment_node.cpp - src/graph_segment/graph_segment_core.cpp - src/graph_segment/similar_area_searcher.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::lanelet2_overlay::Lanelet2Overlay" + EXECUTABLE yabloc_lanelet2_overlay_node + EXECUTOR SingleThreadedExecutor +) -# segment filter -set(TARGET segment_filter_node) -ament_auto_add_executable(${TARGET} - src/segment_filter/segment_filter_node.cpp - src/segment_filter/segment_filter_core.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::line_segment_detector::LineSegmentDetector" + EXECUTABLE yabloc_line_segment_detector_node + EXECUTOR SingleThreadedExecutor +) -# undistort -set(TARGET undistort_node) -ament_auto_add_executable(${TARGET} - src/undistort/undistort_node.cpp) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::line_segments_overlay::LineSegmentsOverlay" + EXECUTABLE yabloc_line_segments_overlay_node + EXECUTOR SingleThreadedExecutor +) -# line_segments_overlay -set(TARGET line_segments_overlay_node) -ament_auto_add_executable(${TARGET} - src/line_segments_overlay/line_segments_overlay_core.cpp - src/line_segments_overlay/line_segments_overlay_node.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::segment_filter::SegmentFilter" + EXECUTABLE yabloc_segment_filter_node + EXECUTOR SingleThreadedExecutor +) -# lanelet2_overlay -set(TARGET lanelet2_overlay_node) -ament_auto_add_executable(${TARGET} - src/lanelet2_overlay/lanelet2_overlay_core.cpp - src/lanelet2_overlay/lanelet2_overlay_node.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::undistort::UndistortNode" + EXECUTABLE yabloc_undistort_node + EXECUTOR SingleThreadedExecutor +) # =================================================== ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp index a1e9c90eaa362..6c2e670a7a72e 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp @@ -32,7 +32,7 @@ class GraphSegment : public rclcpp::Node public: using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - GraphSegment(); + explicit GraphSegment(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const float target_height_ratio_; 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 468e765b74175..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 @@ -47,7 +47,7 @@ class Lanelet2Overlay : public rclcpp::Node using Image = sensor_msgs::msg::Image; using Float32Array = std_msgs::msg::Float32MultiArray; - Lanelet2Overlay(); + explicit Lanelet2Overlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: common::StaticTfSubscriber tf_subscriber_; @@ -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 74ef82dd94f59..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 @@ -42,7 +42,7 @@ class LineSegmentDetector : public rclcpp::Node using Image = sensor_msgs::msg::Image; using PointCloud2 = sensor_msgs::msg::PointCloud2; - LineSegmentDetector(); + explicit LineSegmentDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: rclcpp::Subscription::SharedPtr sub_image_; @@ -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/line_segments_overlay/line_segments_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp index abbc2f75725ed..00f01be5984e5 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp @@ -34,7 +34,7 @@ class LineSegmentsOverlay : public rclcpp::Node using Image = sensor_msgs::msg::Image; using LineSegment = pcl::PointXYZLNormal; using LineSegments = pcl::PointCloud; - LineSegmentsOverlay(); + explicit LineSegmentsOverlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: void on_image(const Image::ConstSharedPtr & img_msg); 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 766bb77d4da85..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 @@ -39,7 +39,7 @@ class SegmentFilter : public rclcpp::Node using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - SegmentFilter(); + explicit SegmentFilter(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: using ProjectFunc = std::function(const Eigen::Vector3f &)>; @@ -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/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index a0cad16302c2b..214772751f931 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -5,7 +5,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -27,7 +27,7 @@ - + @@ -40,7 +40,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml index 150ffed58138d..134f3ee765476 100644 --- a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml @@ -10,7 +10,7 @@ - + @@ -23,7 +23,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 209f09fdaa7ac..453ae58b9b33b 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -16,12 +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 75e14b5a4cd4b..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,18 +15,19 @@ #include "yabloc_image_processing/graph_segment/graph_segment.hpp" #include "yabloc_image_processing/graph_segment/histogram.hpp" +#include #include #include -#include #include #include namespace yabloc::graph_segment { -GraphSegment::GraphSegment() -: Node("graph_segment"), - target_height_ratio_(declare_parameter("target_height_ratio")), - target_candidate_box_width_(declare_parameter("target_candidate_box_width")) +GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) +: Node("graph_segment", options), + 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() 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() 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); } @@ -159,3 +164,6 @@ void GraphSegment::draw_and_publish_image( } } // namespace yabloc::graph_segment + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::graph_segment::GraphSegment) diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp deleted file mode 100644 index d11701e115eff..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp +++ /dev/null @@ -1,23 +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_image_processing/graph_segment/graph_segment.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} 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 367ff51567147..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 @@ -28,8 +28,8 @@ namespace yabloc::lanelet2_overlay { -Lanelet2Overlay::Lanelet2Overlay() -: Node("lanelet2_overlay"), tf_subscriber_(get_clock()), pose_buffer_{40} +Lanelet2Overlay::Lanelet2Overlay(const rclcpp::NodeOptions & options) +: Node("lanelet2_overlay", options), tf_subscriber_(get_clock()), pose_buffer_{40} { using std::placeholders::_1; @@ -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; @@ -211,3 +215,6 @@ void Lanelet2Overlay::make_vis_marker( } } // namespace yabloc::lanelet2_overlay + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::lanelet2_overlay::Lanelet2Overlay) diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp deleted file mode 100644 index 941ac9d84ab16..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp +++ /dev/null @@ -1,23 +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_image_processing/lanelet2_overlay/lanelet2_overlay.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} 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 57f2302fd5c5a..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 @@ -23,7 +23,8 @@ namespace yabloc::line_segment_detector { -LineSegmentDetector::LineSegmentDetector() : Node("line_detector") +LineSegmentDetector::LineSegmentDetector(const rclcpp::NodeOptions & options) +: Node("line_detector", options) { using std::placeholders::_1; @@ -52,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); @@ -66,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; @@ -78,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); @@ -106,3 +108,6 @@ std::vector LineSegmentDetector::remove_too_outer_elements( } } // namespace yabloc::line_segment_detector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segment_detector::LineSegmentDetector) diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp deleted file mode 100644 index 42965ae853ea7..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp +++ /dev/null @@ -1,23 +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_image_processing/line_segment_detector/line_segment_detector.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} 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 0ee4115a39760..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 @@ -23,18 +23,18 @@ namespace yabloc::line_segments_overlay { -LineSegmentsOverlay::LineSegmentsOverlay() -: Node("overlay_lanelet2"), +LineSegmentsOverlay::LineSegmentsOverlay(const rclcpp::NodeOptions & options) +: Node("line_segments_overlay", options), max_buffer_size_(static_cast(declare_parameter("max_buffer_size", 5))) { 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,10 +82,15 @@ 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); } } // namespace yabloc::line_segments_overlay + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segments_overlay::LineSegmentsOverlay) diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp deleted file mode 100644 index cac6a6a0c5a66..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp +++ /dev/null @@ -1,23 +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_image_processing/line_segments_overlay/line_segments_overlay.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} 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 cb1505903b210..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 @@ -23,13 +23,13 @@ namespace yabloc::segment_filter { -SegmentFilter::SegmentFilter() -: Node("segment_filter"), - 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")), +SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) +: Node("segment_filter", options), + 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() 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,10 +279,13 @@ 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; } } // namespace yabloc::segment_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::segment_filter::SegmentFilter) diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp deleted file mode 100644 index ea51babceee60..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp +++ /dev/null @@ -1,23 +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_image_processing/segment_filter/segment_filter.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} 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 7fc9ad785dbe2..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 @@ -37,10 +37,10 @@ class UndistortNode : public rclcpp::Node using CameraInfo = sensor_msgs::msg::CameraInfo; using Image = sensor_msgs::msg::Image; - UndistortNode() - : Node("undistort"), - OUTPUT_WIDTH(declare_parameter("width")), - OVERRIDE_FRAME_ID(declare_parameter("override_frame_id")) + explicit UndistortNode(const rclcpp::NodeOptions & options) + : Node("undistort", options), + 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]"); } @@ -166,10 +170,5 @@ class UndistortNode : public rclcpp::Node }; } // namespace yabloc::undistort -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::undistort::UndistortNode) 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..582abb03901a1 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 @@ -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/correction/abstract_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abstract_corrector.hpp index 31363742576fd..4deaeec0810ae 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 @@ -36,7 +36,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] 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/prediction/predictor.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp index e02393ee14db6..e4fb58f22e17f 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 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 5db4aa0c29e88..8e2b98d10931b 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -17,8 +17,10 @@ autoware_cmake rosidl_default_generators + autoware_universe_utils geometry_msgs rclcpp + rclcpp_components sensor_msgs sophus std_msgs @@ -26,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..c894cd999fb14 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 @@ -28,8 +28,8 @@ namespace yabloc::modularized_particle_filter { -CameraParticleCorrector::CameraParticleCorrector() -: AbstractCorrector("camera_particle_corrector"), +CameraParticleCorrector::CameraParticleCorrector(const rclcpp::NodeOptions & options) +: AbstractCorrector("camera_particle_corrector", options), min_prob_(declare_parameter("min_prob")), far_weight_gain_(declare_parameter("far_weight_gain")), cost_map_(this) @@ -130,7 +130,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()) { @@ -206,13 +206,13 @@ void CameraParticleCorrector::on_line_segments(const PointCloud2 & line_segments 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); } @@ -258,7 +258,7 @@ float abs_cos(const Eigen::Vector3f & t, float deg) { const float radian = 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)); } @@ -321,3 +321,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/common/particle_visualize_node.cpp b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp index 90cd62883e339..b63e87b9462f1 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 @@ -93,7 +94,8 @@ class ParticleVisualize : public rclcpp::Node marker.scale.y = 0.1; marker.scale.z = 0.1; - marker.color = common::color_scale::rainbow(bound_weight(p.weight)); + marker.color = + 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; @@ -105,11 +107,5 @@ class ParticleVisualize : public rclcpp::Node }; } // 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 955ed84fdea2d..888baf0bea0ac 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp @@ -45,7 +45,8 @@ void ParticleVisualizer::publish(const ParticleArray & msg) marker.scale.x = 0.3; marker.scale.y = 0.1; marker.scale.z = 0.1; - marker.color = common::color_scale::rainbow(boundWeight(p.weight)); + marker.color = + static_cast(common::color_scale::rainbow(boundWeight(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..5221256c9e392 100644 --- a/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/correction/abstract_corrector.cpp @@ -16,8 +16,9 @@ namespace yabloc::modularized_particle_filter { -AbstractCorrector::AbstractCorrector(const std::string & node_name) -: Node(node_name), +AbstractCorrector::AbstractCorrector( + const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options), acceptable_max_delay_(declare_parameter("acceptable_max_delay")), visualize_(declare_parameter("visualize")), logger_(rclcpp::get_logger("abstract_corrector")) 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 b321986240b69..f1dd22426a07d 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,8 +19,8 @@ namespace yabloc::modularized_particle_filter { -GnssParticleCorrector::GnssParticleCorrector() -: AbstractCorrector("gnss_particle_corrector"), +GnssParticleCorrector::GnssParticleCorrector(const rclcpp::NodeOptions & options) +: AbstractCorrector("gnss_particle_corrector", options), mahalanobis_distance_threshold_(declare_parameter("mahalanobis_distance_threshold")), weight_manager_(this) { @@ -136,7 +136,7 @@ void GnssParticleCorrector::publish_marker(const Eigen::Vector3f & position, boo marker.pose.position.z = latest_height_.data; float prob = i / 4.0f; - marker.color = common::color_scale::rainbow(prob); + 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)); @@ -160,3 +160,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/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp index ebd9c424edf5e..6255236f2802e 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,9 +16,9 @@ #include "yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp" +#include #include #include -#include #include @@ -169,7 +169,7 @@ HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const marker.header.frame_id = "map"; marker.id = id++; marker.type = Marker::LINE_STRIP; - marker.color = common::Color(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())); diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp index 764ef88bbde18..8046d43dc34a3 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp @@ -31,8 +31,8 @@ namespace yabloc::modularized_particle_filter { -Predictor::Predictor() -: Node("predictor"), +Predictor::Predictor(const rclcpp::NodeOptions & options) +: Node("predictor", options), number_of_particles_(declare_parameter("num_of_particles")), resampling_interval_seconds_(declare_parameter("resampling_interval_seconds")), static_linear_covariance_(declare_parameter("static_linear_covariance")), @@ -411,3 +411,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_pose_initializer/README.md b/localization/yabloc/yabloc_pose_initializer/README.md index 33b9788cca3d3..9ab5dd570510d 100644 --- a/localization/yabloc/yabloc_pose_initializer/README.md +++ b/localization/yabloc/yabloc_pose_initializer/README.md @@ -45,11 +45,11 @@ Converted model URL #### Input -| Name | Type | Description | -| ------------------- | -------------------------------------------- | ------------------------ | -| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | undistorted camera info | -| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted camera image | -| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| Name | Type | Description | +| ------------------- | --------------------------------------- | ------------------------ | +| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | undistorted camera info | +| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted camera image | +| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | #### Output 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 917a6ecaaf291..979d1b370699e 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 @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include @@ -40,7 +40,7 @@ class CameraPoseInitializer : public rclcpp::Node using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped; CameraPoseInitializer(); @@ -52,7 +52,7 @@ class CameraPoseInitializer : public rclcpp::Node std::unique_ptr projector_module_{nullptr}; rclcpp::Subscription::SharedPtr sub_initialpose_; - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_image_; rclcpp::Service::SharedPtr align_server_; @@ -63,7 +63,7 @@ class CameraPoseInitializer : public rclcpp::Node std::unique_ptr semantic_segmentation_{nullptr}; - void on_map(const HADMapBin & msg); + void on_map(const LaneletMapBin & msg); void on_service( const RequestPoseAlignment::Request::SharedPtr, RequestPoseAlignment::Response::SharedPtr request); diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index afd0d4aa86bcf..7ed16c9a8b82d 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -16,7 +16,7 @@ autoware_cmake rosidl_default_generators - autoware_auto_mapping_msgs + autoware_map_msgs cv_bridge geometry_msgs lanelet2_extension 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 19d3c8d88f260..083e7dd5bcd43 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 @@ -40,7 +40,7 @@ CameraPoseInitializer::CameraPoseInitializer() // Subscriber auto on_map = std::bind(&CameraPoseInitializer::on_map, this, _1); auto on_image = [this](Image::ConstSharedPtr msg) -> void { latest_image_msg_ = msg; }; - sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); + sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); sub_image_ = create_subscription("~/input/image_raw", 10, on_image); // Server @@ -164,7 +164,7 @@ std::optional CameraPoseInitializer::estimate_pose( return angles_rad.at(max_index); } -void CameraPoseInitializer::on_map(const HADMapBin & msg) +void CameraPoseInitializer::on_map(const LaneletMapBin & msg) { lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(msg, lanelet_map); diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp index c6c34f0e28a42..8a5fc65e2fac3 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp @@ -42,7 +42,8 @@ void MarkerModule::publish_marker( marker.type = Marker::ARROW; marker.id = i; marker.ns = "arrow"; - marker.color = common::color_scale::rainbow(normalize(scores.at(i))); + marker.color = + static_cast(common::color_scale::rainbow(normalize(scores.at(i)))); marker.color.a = 0.5; marker.pose.position.x = position.x(); diff --git a/map/map_height_fitter/CMakeLists.txt b/map/map_height_fitter/CMakeLists.txt index 8821158c54757..0dec2f6a1663a 100644 --- a/map/map_height_fitter/CMakeLists.txt +++ b/map/map_height_fitter/CMakeLists.txt @@ -5,8 +5,9 @@ find_package(autoware_cmake REQUIRED) find_package(PCL REQUIRED COMPONENTS common) autoware_package() -ament_auto_add_library(map_height_fitter SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/map_height_fitter.cpp + src/map_height_fitter_node.cpp ) target_link_libraries(map_height_fitter ${PCL_LIBRARIES}) @@ -14,11 +15,14 @@ target_link_libraries(map_height_fitter ${PCL_LIBRARIES}) # These are treated as errors in compile, so pedantic warnings are disabled for this package. target_compile_options(map_height_fitter PRIVATE -Wno-pedantic) -ament_auto_add_executable(node - src/node.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "MapHeightFitterNode" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor ) ament_auto_package( INSTALL_TO_SHARE launch - config) + config +) diff --git a/map/map_height_fitter/launch/map_height_fitter.launch.xml b/map/map_height_fitter/launch/map_height_fitter.launch.xml index 353f2151ee172..3e01a35a8e519 100644 --- a/map/map_height_fitter/launch/map_height_fitter.launch.xml +++ b/map/map_height_fitter/launch/map_height_fitter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index 7f384aa43ec7b..c10e65cdfaab1 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -18,13 +18,13 @@ ament_cmake autoware_cmake - autoware_auto_mapping_msgs autoware_map_msgs geometry_msgs lanelet2_extension libpcl-common pcl_conversions rclcpp + rclcpp_components sensor_msgs tf2_geometry_msgs tf2_ros diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 095574125d9a0..e01201f90f7e3 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -38,7 +38,7 @@ struct MapHeightFitter::Impl explicit Impl(rclcpp::Node * node); void on_pcd_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void on_vector_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void on_vector_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); bool get_partial_point_cloud_map(const Point & point); double get_ground_height(const Point & point) const; std::optional fit(const Point & position, const std::string & frame); @@ -59,7 +59,7 @@ struct MapHeightFitter::Impl // for fitting by vector_map_loader lanelet::LaneletMapPtr vector_map_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; }; MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), node_(node) @@ -95,7 +95,7 @@ MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), n } else if (fit_target_ == "vector_map") { const auto durable_qos = rclcpp::QoS(1).transient_local(); - sub_vector_map_ = node_->create_subscription( + sub_vector_map_ = node_->create_subscription( "~/vector_map", durable_qos, std::bind(&MapHeightFitter::Impl::on_vector_map, this, std::placeholders::_1)); @@ -163,7 +163,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } void MapHeightFitter::Impl::on_vector_map( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { vector_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, vector_map_); @@ -200,20 +200,12 @@ double MapHeightFitter::Impl::get_ground_height(const Point & point) const } } } else if (fit_target_ == "vector_map") { - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(vector_map_); - - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - pose.position.z = 0.0; - lanelet::ConstLanelet closest_lanelet; - const bool result = - lanelet::utils::query::getClosestLanelet(all_lanelets, pose, &closest_lanelet); - if (!result) { + const auto closest_points = vector_map_->pointLayer.nearest(lanelet::BasicPoint2d{x, y}, 1); + if (closest_points.empty()) { RCLCPP_WARN_STREAM(logger, "failed to get closest lanelet"); return point.z; } - height = closest_lanelet.centerline().back().z(); + height = closest_points.front().z(); } return std::isfinite(height) ? height : point.z; diff --git a/map/map_height_fitter/src/map_height_fitter_node.cpp b/map/map_height_fitter/src/map_height_fitter_node.cpp new file mode 100644 index 0000000000000..fdc7604b68855 --- /dev/null +++ b/map/map_height_fitter/src/map_height_fitter_node.cpp @@ -0,0 +1,51 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "map_height_fitter/map_height_fitter.hpp" + +#include + +#include + +using tier4_localization_msgs::srv::PoseWithCovarianceStamped; + +class MapHeightFitterNode : public rclcpp::Node +{ +public: + explicit MapHeightFitterNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("map_height_fitter", options), fitter_(this) + { + const auto on_service = [this]( + const PoseWithCovarianceStamped::Request::SharedPtr req, + const PoseWithCovarianceStamped::Response::SharedPtr res) { + const auto & pose = req->pose_with_covariance; + const auto fitted = fitter_.fit(pose.pose.pose.position, pose.header.frame_id); + if (fitted) { + res->pose_with_covariance = req->pose_with_covariance; + res->pose_with_covariance.pose.pose.position = fitted.value(); + res->success = true; + } else { + res->success = false; + } + }; + srv_ = create_service("~/service", on_service); + } + +private: + map_height_fitter::MapHeightFitter fitter_; + rclcpp::Service::SharedPtr srv_; +}; + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MapHeightFitterNode) diff --git a/map/map_height_fitter/src/node.cpp b/map/map_height_fitter/src/node.cpp deleted file mode 100644 index 55702dc08450d..0000000000000 --- a/map/map_height_fitter/src/node.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// 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 "map_height_fitter/map_height_fitter.hpp" - -#include - -#include - -using tier4_localization_msgs::srv::PoseWithCovarianceStamped; - -class MapHeightFitterNode : public rclcpp::Node -{ -public: - MapHeightFitterNode() : Node("map_height_fitter"), fitter_(this) - { - const auto on_service = [this]( - const PoseWithCovarianceStamped::Request::SharedPtr req, - const PoseWithCovarianceStamped::Response::SharedPtr res) { - const auto & pose = req->pose_with_covariance; - const auto fitted = fitter_.fit(pose.pose.pose.position, pose.header.frame_id); - if (fitted) { - res->pose_with_covariance = req->pose_with_covariance; - res->pose_with_covariance.pose.pose.position = fitted.value(); - res->success = true; - } else { - res->success = false; - } - }; - srv_ = create_service("~/service", on_service); - } - -private: - map_height_fitter::MapHeightFitter fitter_; - rclcpp::Service::SharedPtr srv_; -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 8a31ecee50be5..a30bfa1a8f633 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -130,7 +130,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Feature -lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. +lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_map_msgs/LaneletMapBin message. The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_map_msgs/msg/MapProjectorInfo.msg) for supported projector types. @@ -144,19 +144,22 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Published Topics -- ~output/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : Binary data of loaded Lanelet2 Map +- ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map ### Parameters {{ 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 ### Feature -lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. +lanelet2_map_visualization visualizes autoware_map_msgs/LaneletMapBin messages into visualization_msgs/MarkerArray. ### How to Run @@ -164,7 +167,7 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa ### Subscribed Topics -- ~input/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : binary data of Lanelet2 Map +- ~input/lanelet2_map (autoware_map_msgs/LaneletMapBin) : binary data of Lanelet2 Map ### Published Topics 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/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 0adc7612e9f61..4e85ddec056c1 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; using tier4_map_msgs::msg::MapProjectorInfo; class Lanelet2MapLoaderNode : public rclcpp::Node @@ -38,7 +38,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info); - static HADMapBin create_map_bin_msg( + static LaneletMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); @@ -48,7 +48,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; - rclcpp::Publisher::SharedPtr pub_map_bin_; + rclcpp::Publisher::SharedPtr pub_map_bin_; }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp index e5a5fe3e3a6fa..cb640e4dc83d5 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -29,12 +29,12 @@ class Lanelet2MapVisualizationNode : public rclcpp::Node explicit Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_map_bin_; + rclcpp::Subscription::SharedPtr sub_map_bin_; rclcpp::Publisher::SharedPtr pub_marker_; bool viz_lanelets_centerline_; - void onMapBin(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); }; #endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/map/map_loader/launch/lanelet2_map_loader.launch.xml b/map/map_loader/launch/lanelet2_map_loader.launch.xml index ea0157620ce43..5baee911d6572 100644 --- a/map/map_loader/launch/lanelet2_map_loader.launch.xml +++ b/map/map_loader/launch/lanelet2_map_loader.launch.xml @@ -4,16 +4,16 @@ - + - + - + diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index 3e447456bb623..3633a8fa39a6b 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index a9b657f843447..a6f4ac7f81275 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -4,7 +4,6 @@ map_loader 0.1.0 The map_loader package - Ryohsuke Mitsudome Yamato Ando Ryu Yamamoto Koji Minoda @@ -15,11 +14,12 @@ Shintaro Sakoda Apache License 2.0 + Ryohsuke Mitsudome + Koji Minoda ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs autoware_map_msgs component_interface_specs component_interface_utils 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 617f3dd503ce0..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,14 +81,18 @@ 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()); // create publisher and publish pub_map_bin_ = - create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); + create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); pub_map_bin_->publish(map_bin_msg); RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published."); } @@ -141,18 +147,18 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return nullptr; } -HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg( +LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { std::string format_version{}, map_version{}; lanelet::io_handlers::AutowareOsmParser::parseVersions( lanelet2_filename, &format_version, &map_version); - HADMapBin map_bin_msg; + LaneletMapBin map_bin_msg; map_bin_msg.header.stamp = now; map_bin_msg.header.frame_id = "map"; - map_bin_msg.format_version = format_version; - map_bin_msg.map_version = map_version; + map_bin_msg.version_map_format = format_version; + map_bin_msg.version_map = map_version; lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); return map_bin_msg; diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index bdfbcf904cf36..c81236bec86c0 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include @@ -73,7 +73,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt viz_lanelets_centerline_ = true; - sub_map_bin_ = this->create_subscription( + sub_map_bin_ = this->create_subscription( "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), std::bind(&Lanelet2MapVisualizationNode::onMapBin, this, _1)); @@ -82,7 +82,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt } void Lanelet2MapVisualizationNode::onMapBin( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); 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/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index f6102a1efa795..87f519ab22572 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -6,25 +6,24 @@ autoware_package() ament_auto_find_build_dependencies() -ament_auto_add_library(map_projection_loader_lib SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/map_projection_loader.cpp src/load_info_from_lanelet2_map.cpp ) -target_link_libraries(map_projection_loader_lib yaml-cpp) - -ament_auto_add_executable(map_projection_loader src/map_projection_loader_node.cpp) - -target_compile_options(map_projection_loader PUBLIC -g -Wall -Wextra -Wpedantic -Werror) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "MapProjectionLoader" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) -target_link_libraries(map_projection_loader map_projection_loader_lib) -target_include_directories(map_projection_loader PUBLIC include) +target_link_libraries(${PROJECT_NAME} yaml-cpp) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gmock(${test_name} ${filepath}) - target_link_libraries("${test_name}" map_projection_loader_lib) + target_link_libraries("${test_name}" ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() @@ -57,7 +56,8 @@ if(BUILD_TESTING) add_testcase(test/test_load_info_from_lanelet2_map.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch config ) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 848fcfba95f14..aa620256d977b 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -21,6 +21,10 @@ sample-map-rosbag └── pointcloud_map_metadata.yaml ``` +There are three types of transformations from latitude and longitude to XYZ coordinate system as shown in the figure below. Please refer to the following details for the necessary parameters for each projector type. + +![node_diagram](docs/map_projector_type.svg) + ### Using local coordinate ```yaml diff --git a/map/map_projection_loader/docs/map_projector_type.svg b/map/map_projection_loader/docs/map_projector_type.svg new file mode 100644 index 0000000000000..e1c8c2ac68987 --- /dev/null +++ b/map/map_projection_loader/docs/map_projector_type.svg @@ -0,0 +1,3010 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + MGRS + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + MGRS + + + X(east) + + + O + + + Y(north) + + + + Strict Boundary. + + Outside of the mgrs grid is + projected to undefined xy + + + mgrs_grid=54SUE + + + 54SUE + + + 54S(UTM grid) + + + meridian + + + LocalCartesianUTM + + + TransverseMercator + + + + + map_origin.latitude=35.6 + map_origin.longitude=139.5 + + + 3+6*floor(map_origin.longitude/6)[deg] + + + 3+6n[deg] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + X(east) + + + O + + + Y(north) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + map_origin.latitude=35.6 + map_origin.longitude=139.5 + + + map_origin.longitude[deg] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + X(east) + + + O + + + Y(north) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + No boundary, + but farther away from the meridian, + the greater the projection error becomes + + + + No boundary, + but farther away from the meridian, + the greater the projection error becomes + + + diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 54e794e2742bf..05bc6e64e1675 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -29,7 +29,7 @@ tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( class MapProjectionLoader : public rclcpp::Node { public: - MapProjectionLoader(); + explicit MapProjectionLoader(const rclcpp::NodeOptions & options); private: using MapProjectorInfo = map_interface::MapProjectorInfo; diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index a6570b69d3498..13418a7e97423 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 475881577bd58..7a930085cd7b1 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -21,6 +21,7 @@ component_interface_utils lanelet2_extension rclcpp + rclcpp_components tier4_map_msgs yaml-cpp diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 5966baaed8383..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(); @@ -82,7 +78,8 @@ tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( return msg; } -MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +MapProjectionLoader::MapProjectionLoader(const rclcpp::NodeOptions & options) +: rclcpp::Node("map_projection_loader", options) { const std::string yaml_filename = this->declare_parameter("map_projector_info_path"); const std::string lanelet2_map_filename = @@ -96,3 +93,6 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") adaptor.init_pub(publisher_); publisher_->publish(msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MapProjectionLoader) diff --git a/map/map_projection_loader/src/map_projection_loader_node.cpp b/map/map_projection_loader/src/map_projection_loader_node.cpp deleted file mode 100644 index 1d9336be0d9dd..0000000000000 --- a/map/map_projection_loader/src/map_projection_loader_node.cpp +++ /dev/null @@ -1,23 +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 "map_projection_loader/map_projection_loader.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} 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/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py index b8540550ce9da..0d0b5cb31afba 100644 --- a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py index c7697038cc253..6a17ff340b19f 100644 --- a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py index f75beddc6827c..37cfd9936bf20 100644 --- a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py index 765f3cde04679..7bccdc7875454 100644 --- a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_tf_generator/README.md b/map/map_tf_generator/README.md index 643f4233c06f0..eef36c34750ca 100644 --- a/map/map_tf_generator/README.md +++ b/map/map_tf_generator/README.md @@ -25,9 +25,9 @@ The following are the supported methods to calculate the position of the `viewer #### vector_map_tf_generator -| Name | Type | Description | -| ----------------- | -------------------------------------------- | ------------------------------------------------------------- | -| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Subscribe vector map to calculate position of `viewer` frames | +| Name | Type | Description | +| ----------------- | --------------------------------------- | ------------------------------------------------------------- | +| `/map/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | Subscribe vector map to calculate position of `viewer` frames | ### Output diff --git a/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp b/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp index 345703a19bc06..cda3bc5c49a0c 100644 --- a/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp +++ b/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp @@ -18,7 +18,7 @@ #include #include -std::vector UniformRandom(const size_t max_exclusive, const size_t n) +std::vector inline uniform_random(const size_t max_exclusive, const size_t n) { std::default_random_engine generator; std::uniform_int_distribution distribution(0, max_exclusive - 1); diff --git a/map/map_tf_generator/launch/map_tf_generator.launch.xml b/map/map_tf_generator/launch/map_tf_generator.launch.xml index 43d487c2bc982..197085f31d9c4 100644 --- a/map/map_tf_generator/launch/map_tf_generator.launch.xml +++ b/map/map_tf_generator/launch/map_tf_generator.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp b/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp index 69a66cf52c865..d4b8ccb797224 100644 --- a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp @@ -26,22 +26,22 @@ #include #include +#include -constexpr size_t N_SAMPLES = 20; +constexpr size_t n_samples = 20; class PcdMapTFGeneratorNode : public rclcpp::Node { public: using PointCloud = pcl::PointCloud; explicit PcdMapTFGeneratorNode(const rclcpp::NodeOptions & options) - : Node("pcd_map_tf_generator", options) + : Node("pcd_map_tf_generator", options), + map_frame_(declare_parameter("map_frame")), + viewer_frame_(declare_parameter("viewer_frame")) { - map_frame_ = declare_parameter("map_frame"); - viewer_frame_ = declare_parameter("viewer_frame"); - sub_ = create_subscription( "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&PcdMapTFGeneratorNode::onPointCloud, this, std::placeholders::_1)); + std::bind(&PcdMapTFGeneratorNode::on_point_cloud, this, std::placeholders::_1)); static_broadcaster_ = std::make_shared(this); } @@ -53,7 +53,7 @@ class PcdMapTFGeneratorNode : public rclcpp::Node std::shared_ptr static_broadcaster_; - void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr clouds_ros) + void on_point_cloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr clouds_ros) { // fix random seed to produce the same viewer position every time // 3939 is just the author's favorite number @@ -62,32 +62,32 @@ class PcdMapTFGeneratorNode : public rclcpp::Node PointCloud clouds; pcl::fromROSMsg(*clouds_ros, clouds); - const std::vector indices = UniformRandom(clouds.size(), N_SAMPLES); + const std::vector indices = uniform_random(clouds.size(), n_samples); double coordinate[3] = {0, 0, 0}; for (const auto i : indices) { coordinate[0] += clouds.points[i].x; coordinate[1] += clouds.points[i].y; coordinate[2] += clouds.points[i].z; } - coordinate[0] = coordinate[0] / indices.size(); - coordinate[1] = coordinate[1] / indices.size(); - coordinate[2] = coordinate[2] / indices.size(); - - geometry_msgs::msg::TransformStamped static_transformStamped; - static_transformStamped.header.stamp = this->now(); - static_transformStamped.header.frame_id = map_frame_; - static_transformStamped.child_frame_id = viewer_frame_; - static_transformStamped.transform.translation.x = coordinate[0]; - static_transformStamped.transform.translation.y = coordinate[1]; - static_transformStamped.transform.translation.z = coordinate[2]; + coordinate[0] = coordinate[0] / static_cast(indices.size()); + coordinate[1] = coordinate[1] / static_cast(indices.size()); + coordinate[2] = coordinate[2] / static_cast(indices.size()); + + geometry_msgs::msg::TransformStamped static_transform_stamped; + static_transform_stamped.header.stamp = this->now(); + static_transform_stamped.header.frame_id = map_frame_; + static_transform_stamped.child_frame_id = viewer_frame_; + static_transform_stamped.transform.translation.x = coordinate[0]; + static_transform_stamped.transform.translation.y = coordinate[1]; + static_transform_stamped.transform.translation.z = coordinate[2]; tf2::Quaternion quat; quat.setRPY(0, 0, 0); - static_transformStamped.transform.rotation.x = quat.x(); - static_transformStamped.transform.rotation.y = quat.y(); - static_transformStamped.transform.rotation.z = quat.z(); - static_transformStamped.transform.rotation.w = quat.w(); + static_transform_stamped.transform.rotation.x = quat.x(); + static_transform_stamped.transform.rotation.y = quat.y(); + static_transform_stamped.transform.rotation.z = quat.z(); + static_transform_stamped.transform.rotation.w = quat.w(); - static_broadcaster_->sendTransform(static_transformStamped); + static_broadcaster_->sendTransform(static_transform_stamped); RCLCPP_INFO_STREAM( get_logger(), "broadcast static tf. map_frame:" diff --git a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp index 20ca1c037a578..df3075bdce1fe 100644 --- a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #include #include @@ -29,14 +29,13 @@ class VectorMapTFGeneratorNode : public rclcpp::Node { public: explicit VectorMapTFGeneratorNode(const rclcpp::NodeOptions & options) - : Node("vector_map_tf_generator", options) + : Node("vector_map_tf_generator", options), + map_frame_(declare_parameter("map_frame")), + viewer_frame_(declare_parameter("viewer_frame")) { - map_frame_ = declare_parameter("map_frame"); - viewer_frame_ = declare_parameter("viewer_frame"); - - sub_ = create_subscription( + sub_ = create_subscription( "vector_map", rclcpp::QoS{1}.transient_local(), - std::bind(&VectorMapTFGeneratorNode::onVectorMap, this, std::placeholders::_1)); + std::bind(&VectorMapTFGeneratorNode::on_vector_map, this, std::placeholders::_1)); static_broadcaster_ = std::make_shared(this); } @@ -44,12 +43,12 @@ class VectorMapTFGeneratorNode : public rclcpp::Node private: std::string map_frame_; std::string viewer_frame_; - rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr sub_; std::shared_ptr static_broadcaster_; std::shared_ptr lanelet_map_ptr_; - void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + void on_vector_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); @@ -66,27 +65,27 @@ class VectorMapTFGeneratorNode : public rclcpp::Node points_z.push_back(point_z); } const double coordinate_x = - std::accumulate(points_x.begin(), points_x.end(), 0.0) / points_x.size(); + std::accumulate(points_x.begin(), points_x.end(), 0.0) / static_cast(points_x.size()); const double coordinate_y = - std::accumulate(points_y.begin(), points_y.end(), 0.0) / points_y.size(); + std::accumulate(points_y.begin(), points_y.end(), 0.0) / static_cast(points_y.size()); const double coordinate_z = - std::accumulate(points_z.begin(), points_z.end(), 0.0) / points_z.size(); + std::accumulate(points_z.begin(), points_z.end(), 0.0) / static_cast(points_z.size()); - geometry_msgs::msg::TransformStamped static_transformStamped; - static_transformStamped.header.stamp = this->now(); - static_transformStamped.header.frame_id = map_frame_; - static_transformStamped.child_frame_id = viewer_frame_; - static_transformStamped.transform.translation.x = coordinate_x; - static_transformStamped.transform.translation.y = coordinate_y; - static_transformStamped.transform.translation.z = coordinate_z; + geometry_msgs::msg::TransformStamped static_transform_stamped; + static_transform_stamped.header.stamp = this->now(); + static_transform_stamped.header.frame_id = map_frame_; + static_transform_stamped.child_frame_id = viewer_frame_; + static_transform_stamped.transform.translation.x = coordinate_x; + static_transform_stamped.transform.translation.y = coordinate_y; + static_transform_stamped.transform.translation.z = coordinate_z; tf2::Quaternion quat; quat.setRPY(0, 0, 0); - static_transformStamped.transform.rotation.x = quat.x(); - static_transformStamped.transform.rotation.y = quat.y(); - static_transformStamped.transform.rotation.z = quat.z(); - static_transformStamped.transform.rotation.w = quat.w(); + static_transform_stamped.transform.rotation.x = quat.x(); + static_transform_stamped.transform.rotation.y = quat.y(); + static_transform_stamped.transform.rotation.z = quat.z(); + static_transform_stamped.transform.rotation.w = quat.w(); - static_broadcaster_->sendTransform(static_transformStamped); + static_broadcaster_->sendTransform(static_transform_stamped); RCLCPP_INFO_STREAM( get_logger(), "broadcast static tf. map_frame:" diff --git a/map/map_tf_generator/test/test_uniform_random.cpp b/map/map_tf_generator/test/test_uniform_random.cpp index 455edc2d5dfd2..19e558ebb2add 100644 --- a/map/map_tf_generator/test/test_uniform_random.cpp +++ b/map/map_tf_generator/test/test_uniform_random.cpp @@ -21,10 +21,10 @@ using testing::Each; using testing::Ge; using testing::Lt; -TEST(UniformRandom, UniformRandom) +TEST(uniform_random, uniform_random) { { - const std::vector random = UniformRandom(4, 0); + const std::vector random = uniform_random(4, 0); ASSERT_EQ(random.size(), static_cast(0)); } @@ -35,7 +35,7 @@ TEST(UniformRandom, UniformRandom) const size_t max_exclusive = 4; for (int i = 0; i < 50; i++) { - const std::vector random = UniformRandom(4, 10); + const std::vector random = uniform_random(4, 10); ASSERT_EQ(random.size(), 10U); ASSERT_THAT(random, Each(AllOf(Ge(min_inclusive), Lt(max_exclusive)))); // in range [0, 4) } diff --git a/map/util/lanelet2_map_preprocessor/package.xml b/map/util/lanelet2_map_preprocessor/package.xml index 421f04257de67..e84bd8a80c202 100644 --- a/map/util/lanelet2_map_preprocessor/package.xml +++ b/map/util/lanelet2_map_preprocessor/package.xml @@ -4,7 +4,6 @@ lanelet2_map_preprocessor 0.1.0 The lanelet2_map_preprocessor package - Ryohsuke Mitsudome Yamato Ando Kento Yabuuchi Masahiro Sakamoto diff --git a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp index 8900381060472..a406e5381357d 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -48,11 +48,6 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return std::find(set.begin(), set.end(), element) != set.end(); -} - lanelet::Lanelets convertToVector(lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Lanelets lanelets; diff --git a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp index b0eca472ee1f3..758fee3addc06 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp @@ -91,11 +91,6 @@ double getMinHeightAroundPoint( return min_height; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return std::find(set.begin(), set.end(), element) != set.end(); -} - void adjustHeight( const pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr) { @@ -108,7 +103,7 @@ void adjustHeight( for (lanelet::Lanelet & llt : lanelet_map_ptr->laneletLayer) { for (lanelet::Point3d & pt : llt.leftBound()) { - if (exists(done, pt.id())) { + if (done.find(pt.id()) != done.end()) { continue; } pcl::PointXYZ pcl_pt; @@ -122,7 +117,7 @@ void adjustHeight( done.insert(pt.id()); } for (lanelet::Point3d & pt : llt.rightBound()) { - if (exists(done, pt.id())) { + if (done.find(pt.id()) != done.end()) { continue; } pcl::PointXYZ pcl_pt; diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp index a9e45b3b31785..d001bdc54a680 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp @@ -49,7 +49,7 @@ bool loadLaneletMap( bool exists(std::unordered_set & set, lanelet::Id element) { - return std::find(set.begin(), set.end(), element) != set.end(); + return set.find(element) != set.end(); } lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp index ab77b18493120..beb736e809275 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp @@ -45,7 +45,7 @@ bool loadLaneletMap( bool exists(std::unordered_set & set, lanelet::Id element) { - return std::find(set.begin(), set.end(), element) != set.end(); + return set.find(element) != set.end(); } lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) diff --git a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp index ca488b3acb790..e6c4feb4cee9a 100644 --- a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp +++ b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp @@ -43,11 +43,6 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return std::find(set.begin(), set.end(), element) != set.end(); -} - lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Points3d points; diff --git a/perception/autoware_crosswalk_traffic_light_estimator/CMakeLists.txt b/perception/autoware_crosswalk_traffic_light_estimator/CMakeLists.txt new file mode 100644 index 0000000000000..31a098a8686ec --- /dev/null +++ b/perception/autoware_crosswalk_traffic_light_estimator/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_crosswalk_traffic_light_estimator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +########### +## Build ## +########### + +include_directories() + +ament_auto_add_library(autoware_crosswalk_traffic_light_estimator SHARED + src/node.cpp +) + +rclcpp_components_register_node(autoware_crosswalk_traffic_light_estimator + PLUGIN "autoware::crosswalk_traffic_light_estimator::CrosswalkTrafficLightEstimatorNode" + EXECUTABLE crosswalk_traffic_light_estimator_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +############# +## Install ## +############# + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/perception/autoware_crosswalk_traffic_light_estimator/README.md b/perception/autoware_crosswalk_traffic_light_estimator/README.md new file mode 100644 index 0000000000000..b14fefbd43beb --- /dev/null +++ b/perception/autoware_crosswalk_traffic_light_estimator/README.md @@ -0,0 +1,84 @@ +# crosswalk_traffic_light_estimator + +## Purpose + +`crosswalk_traffic_light_estimator` is a module that estimates pedestrian traffic signals from HDMap and detected vehicle traffic signals. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------------ | ------------------------------------------------ | ------------------ | +| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | +| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | route | +| `~/input/classified/traffic_signals` | `tier4_perception_msgs::msg::TrafficSignalArray` | classified signals | + +### Output + +| Name | Type | Description | +| -------------------------- | ------------------------------------------------------- | --------------------------------------------------------- | +| `~/output/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | output that contains estimated pedestrian traffic signals | + +## Parameters + +| Name | Type | Description | Default value | +| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | `true` | +| `last_detect_color_hold_time` | `double` | The time threshold to hold for last detect color. | `2.0` | + +## Inner-workings / Algorithms + +```plantuml + +start +:subscribe detected traffic signals & HDMap; +:extract crosswalk lanelets from HDMap; +:extract road lanelets that conflicts crosswalk; +:initialize non_red_lanelets(lanelet::ConstLanelets); +if (Latest detection result is **GREEN** or **AMBER**?) then (yes) + :push back non_red_lanelets; +else (no) + if (use_last_detect_color is **true**?) then (yes) + if (Latest detection result is **UNKNOWN** and last detection result is **GREEN** or **AMBER**?) then (yes) + :push back non_red_lanelets; + endif + endif +endif +if (Is there **STRAIGHT-NON-RED** road lanelet in non_red_lanelets?) then (yes) + :estimate related pedestrian's traffic signal as **RED**; +else if (Is there both **LEFT-NON-RED** and **RIGHT-NON-RED** road lanelet in non_red_lanelets?) then (yes) + :estimate related pedestrian's traffic signal as **RED**; +else (no) + :estimate related pedestrian's traffic signal as **UNKNOWN**; +endif +end + +``` + +If traffic between pedestrians and vehicles is controlled by traffic signals, the crosswalk traffic signal maybe **RED** in order to prevent pedestrian from crossing when the following conditions are satisfied. + +### Situation1 + +- crosswalk conflicts **STRAIGHT** lanelet +- the lanelet refers **GREEN** or **AMBER** traffic signal (The following pictures show only **GREEN** case) + +
+ +
+
+ +
+ +### Situation2 + +- crosswalk conflicts different turn direction lanelets (STRAIGHT and LEFT, LEFT and RIGHT, RIGHT and STRAIGHT) +- the lanelets refer **GREEN** or **AMBER** traffic signal (The following pictures show only **GREEN** case) + +
+ +
+ +## Assumptions / Known limits + +## Future extensions / Unimplemented parts diff --git a/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml b/perception/autoware_crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml similarity index 100% rename from perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml rename to perception/autoware_crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml diff --git a/perception/crosswalk_traffic_light_estimator/images/intersection1.svg b/perception/autoware_crosswalk_traffic_light_estimator/images/intersection1.svg similarity index 100% rename from perception/crosswalk_traffic_light_estimator/images/intersection1.svg rename to perception/autoware_crosswalk_traffic_light_estimator/images/intersection1.svg diff --git a/perception/crosswalk_traffic_light_estimator/images/intersection2.svg b/perception/autoware_crosswalk_traffic_light_estimator/images/intersection2.svg similarity index 100% rename from perception/crosswalk_traffic_light_estimator/images/intersection2.svg rename to perception/autoware_crosswalk_traffic_light_estimator/images/intersection2.svg diff --git a/perception/crosswalk_traffic_light_estimator/images/straight.drawio.svg b/perception/autoware_crosswalk_traffic_light_estimator/images/straight.drawio.svg similarity index 100% rename from perception/crosswalk_traffic_light_estimator/images/straight.drawio.svg rename to perception/autoware_crosswalk_traffic_light_estimator/images/straight.drawio.svg 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 new file mode 100644 index 0000000000000..8efb90cc87c89 --- /dev/null +++ b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp @@ -0,0 +1,122 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ +#define AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +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_debug_msgs::msg::Float64Stamped; +using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; +using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; +using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; +using TrafficSignalAndTime = std::pair; +using TrafficLightIdMap = std::unordered_map; + +using TrafficLightIdArray = std::unordered_map>; + +class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node +{ +public: + explicit CrosswalkTrafficLightEstimatorNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Subscription::SharedPtr sub_traffic_light_array_; + rclcpp::Publisher::SharedPtr pub_traffic_light_array_; + + lanelet::LaneletMapPtr lanelet_map_ptr_; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; + lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + std::shared_ptr overall_graphs_ptr_; + + lanelet::ConstLanelets conflicting_crosswalks_; + + void onMap(const LaneletMapBin::ConstSharedPtr msg); + void onRoute(const LaneletRoute::ConstSharedPtr msg); + void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg); + + void updateLastDetectedSignal(const TrafficLightIdMap & traffic_signals); + void updateLastDetectedSignals(const TrafficLightIdMap & traffic_signals); + void updateFlashingState(const TrafficSignal & signal); + uint8_t updateAndGetColorState(const TrafficSignal & signal); + void setCrosswalkTrafficSignal( + const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, + TrafficSignalArray & output); + + lanelet::ConstLanelets getNonRedLanelets( + const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const; + + uint8_t estimateCrosswalkTrafficSignal( + const lanelet::ConstLanelet & crosswalk, const lanelet::ConstLanelets & non_red_lanelets) const; + + boost::optional getHighestConfidenceTrafficSignal( + const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, + const TrafficLightIdMap & traffic_light_id_map) const; + + boost::optional getHighestConfidenceTrafficSignal( + const lanelet::Id & id, const TrafficLightIdMap & traffic_light_id_map) const; + + void removeDuplicateIds(TrafficSignalArray & signal_array) const; + + // Node param + bool use_last_detect_color_; + double last_detect_color_hold_time_; + double last_colors_hold_time_; + + // Signal history + TrafficLightIdMap last_detect_color_; + TrafficLightIdArray last_colors_; + + // State + std::map is_flashing_; + std::map current_color_state_; + + // Stop watch + StopWatch stop_watch_; + + // Debug + std::shared_ptr pub_processing_time_; +}; + +} // namespace autoware::crosswalk_traffic_light_estimator + +#endif // AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ diff --git a/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml b/perception/autoware_crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml similarity index 78% rename from perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml rename to perception/autoware_crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml index 21d718c3439cd..2e1437ecd7d93 100644 --- a/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml +++ b/perception/autoware_crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml @@ -11,9 +11,9 @@ limitations under the License. --> - + - + diff --git a/perception/autoware_crosswalk_traffic_light_estimator/package.xml b/perception/autoware_crosswalk_traffic_light_estimator/package.xml new file mode 100644 index 0000000000000..18b0b8a41264f --- /dev/null +++ b/perception/autoware_crosswalk_traffic_light_estimator/package.xml @@ -0,0 +1,30 @@ + + + autoware_crosswalk_traffic_light_estimator + 0.1.0 + The autoware_crosswalk_traffic_light_estimator package + + Satoshi Ota + Shunsuke Miura + Tao Zhong + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_map_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_universe_utils + lanelet2_extension + rclcpp + rclcpp_components + tier4_perception_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json b/perception/autoware_crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json similarity index 100% rename from perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json rename to perception/autoware_crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json diff --git a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp new file mode 100644 index 0000000000000..2c4da0b95b161 --- /dev/null +++ b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp @@ -0,0 +1,546 @@ +// Copyright 2022-2023 UCI SORA Lab, 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_crosswalk_traffic_light_estimator/node.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace autoware::crosswalk_traffic_light_estimator +{ +namespace +{ + +bool hasMergeLane( + const lanelet::ConstLanelet & lanelet_1, const lanelet::ConstLanelet & lanelet_2, + const lanelet::routing::RoutingGraphPtr & routing_graph_ptr) +{ + const auto next_lanelets_1 = routing_graph_ptr->following(lanelet_1); + const auto next_lanelets_2 = routing_graph_ptr->following(lanelet_2); + + for (const auto & next_lanelet_1 : next_lanelets_1) { + for (const auto & next_lanelet_2 : next_lanelets_2) { + if (next_lanelet_1.id() == next_lanelet_2.id()) { + return true; + } + } + } + + return false; +} + +bool hasMergeLane( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr & routing_graph_ptr) +{ + for (size_t i = 0; i < lanelets.size(); ++i) { + for (size_t j = i + 1; j < lanelets.size(); ++j) { + const auto lanelet_1 = lanelets.at(i); + const auto lanelet_2 = lanelets.at(j); + + if (lanelet_1.id() == lanelet_2.id()) { + continue; + } + + const std::string turn_direction_1 = lanelet_1.attributeOr("turn_direction", "none"); + const std::string turn_direction_2 = lanelet_2.attributeOr("turn_direction", "none"); + if (turn_direction_1 == turn_direction_2) { + continue; + } + + if (!hasMergeLane(lanelet_1, lanelet_2, routing_graph_ptr)) { + continue; + } + + return true; + } + } + + return false; +} +} // namespace + +CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( + const rclcpp::NodeOptions & options) +: Node("crosswalk_traffic_light_estimator", options) +{ + using std::placeholders::_1; + + use_last_detect_color_ = declare_parameter("use_last_detect_color"); + last_detect_color_hold_time_ = declare_parameter("last_detect_color_hold_time"); + last_colors_hold_time_ = declare_parameter("last_colors_hold_time"); + + sub_map_ = create_subscription( + "~/input/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&CrosswalkTrafficLightEstimatorNode::onMap, this, _1)); + sub_route_ = create_subscription( + "~/input/route", rclcpp::QoS{1}.transient_local(), + std::bind(&CrosswalkTrafficLightEstimatorNode::onRoute, this, _1)); + sub_traffic_light_array_ = create_subscription( + "~/input/classified/traffic_signals", rclcpp::QoS{1}, + std::bind(&CrosswalkTrafficLightEstimatorNode::onTrafficLightArray, this, _1)); + + pub_traffic_light_array_ = + this->create_publisher("~/output/traffic_signals", rclcpp::QoS{1}); + pub_processing_time_ = std::make_shared(this, "~/debug"); +} + +void CrosswalkTrafficLightEstimatorNode::onMap(const LaneletMapBin::ConstSharedPtr msg) +{ + RCLCPP_DEBUG(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Start loading lanelet"); + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Pedestrian); + lanelet::routing::RoutingGraphConstPtr vehicle_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules); + lanelet::routing::RoutingGraphConstPtr pedestrian_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules); + lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); + overall_graphs_ptr_ = + std::make_shared(overall_graphs); + RCLCPP_DEBUG(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Map is loaded"); +} + +void CrosswalkTrafficLightEstimatorNode::onRoute(const LaneletRoute::ConstSharedPtr msg) +{ + if (lanelet_map_ptr_ == nullptr) { + RCLCPP_WARN(get_logger(), "cannot set traffic light in route because don't receive map"); + return; + } + + lanelet::ConstLanelets route_lanelets; + for (const auto & segment : msg->segments) { + for (const auto & primitive : segment.primitives) { + try { + route_lanelets.push_back(lanelet_map_ptr_->laneletLayer.get(primitive.id)); + } catch (const lanelet::NoSuchPrimitiveError & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + return; + } + } + } + + conflicting_crosswalks_.clear(); + + for (const auto & route_lanelet : route_lanelets) { + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflict_lls = + overall_graphs_ptr_->conflictingInGraph(route_lanelet, PEDESTRIAN_GRAPH_ID); + for (const auto & lanelet : conflict_lls) { + conflicting_crosswalks_.push_back(lanelet); + } + } +} + +void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray( + const TrafficSignalArray::ConstSharedPtr msg) +{ + StopWatch stop_watch; + stop_watch.tic("Total"); + + TrafficSignalArray output = *msg; + + TrafficLightIdMap traffic_light_id_map; + for (const auto & traffic_signal : msg->traffic_light_groups) { + traffic_light_id_map[traffic_signal.traffic_light_group_id] = + std::pair(traffic_signal, get_clock()->now()); + } + for (const auto & crosswalk : conflicting_crosswalks_) { + constexpr int VEHICLE_GRAPH_ID = 0; + const auto conflict_lls = overall_graphs_ptr_->conflictingInGraph(crosswalk, VEHICLE_GRAPH_ID); + const auto non_red_lanelets = getNonRedLanelets(conflict_lls, traffic_light_id_map); + + const auto crosswalk_tl_color = estimateCrosswalkTrafficSignal(crosswalk, non_red_lanelets); + setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, *msg, output); + } + + removeDuplicateIds(output); + + updateLastDetectedSignal(traffic_light_id_map); + updateLastDetectedSignals(traffic_light_id_map); + + pub_traffic_light_array_->publish(output); + pub_processing_time_->publish("processing_time_ms", stop_watch.toc("Total")); + + return; +} + +void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal( + const TrafficLightIdMap & traffic_light_id_map) +{ + for (const auto & input_traffic_signal : traffic_light_id_map) { + const auto & elements = input_traffic_signal.second.first.elements; + + if (elements.empty()) { + continue; + } + + if (elements.front().color == TrafficSignalElement::UNKNOWN) { + continue; + } + + const auto & id = input_traffic_signal.second.first.traffic_light_group_id; + + if (last_detect_color_.count(id) == 0) { + last_detect_color_.insert(std::make_pair(id, input_traffic_signal.second)); + continue; + } + + last_detect_color_.at(id) = input_traffic_signal.second; + } + + std::vector erase_id_list; + for (auto & last_traffic_signal : last_detect_color_) { + const auto & id = last_traffic_signal.second.first.traffic_light_group_id; + + if (traffic_light_id_map.count(id) == 0) { + // hold signal recognition results for [last_detect_color_hold_time_] seconds. + const auto time_from_last_detected = + (get_clock()->now() - last_traffic_signal.second.second).seconds(); + if (time_from_last_detected > last_detect_color_hold_time_) { + erase_id_list.emplace_back(id); + } + } + } + for (const auto id : erase_id_list) { + last_detect_color_.erase(id); + is_flashing_.erase(id); + current_color_state_.erase(id); + } +} + +void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignals( + const TrafficLightIdMap & traffic_light_id_map) +{ + for (const auto & input_traffic_signal : traffic_light_id_map) { + const auto & elements = input_traffic_signal.second.first.elements; + + if (elements.empty()) { + continue; + } + + if ( + elements.front().color == TrafficSignalElement::UNKNOWN && elements.front().confidence == 1) { + continue; + } + + const auto & id = input_traffic_signal.second.first.traffic_light_group_id; + + if (last_colors_.count(id) == 0) { + std::vector signal{input_traffic_signal.second}; + last_colors_.insert(std::make_pair(id, signal)); + continue; + } + + last_colors_.at(id).push_back(input_traffic_signal.second); + } + + std::vector erase_id_list; + for (auto & last_traffic_signal : last_colors_) { + const auto & id = last_traffic_signal.first; + for (auto it = last_traffic_signal.second.begin(); it != last_traffic_signal.second.end();) { + auto sig = (*it).first; + rclcpp::Time t = (*it).second; + + // hold signal recognition results for [last_colors_hold_time_] seconds. + const auto time_from_last_detected = (get_clock()->now() - t).seconds(); + if (time_from_last_detected > last_colors_hold_time_) { + it = last_traffic_signal.second.erase(it); + } else { + ++it; + } + } + if (last_traffic_signal.second.empty()) { + erase_id_list.emplace_back(id); + } + } + for (const auto id : erase_id_list) last_colors_.erase(id); +} + +void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( + const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, + TrafficSignalArray & output) +{ + const auto tl_reg_elems = crosswalk.regulatoryElementsAs(); + + std::unordered_map valid_id2idx_map; // detected traffic light + + for (size_t i = 0; i < msg.traffic_light_groups.size(); ++i) { + auto signal = msg.traffic_light_groups[i]; + valid_id2idx_map[signal.traffic_light_group_id] = i; + } + + for (const auto & tl_reg_elem : tl_reg_elems) { + auto id = tl_reg_elem->id(); + // if valid prediction exists, overwrite the estimation; else, use the estimation + if (valid_id2idx_map.count(id)) { + size_t idx = valid_id2idx_map[id]; + auto signal = msg.traffic_light_groups[idx]; + updateFlashingState(signal); // check if it is flashing + // update output msg according to flashing and current state + output.traffic_light_groups[idx].elements[0].color = updateAndGetColorState(signal); + } else { + TrafficSignal output_traffic_signal; + TrafficSignalElement output_traffic_signal_element; + output_traffic_signal_element.color = color; + output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE; + output_traffic_signal_element.confidence = 1.0; + output_traffic_signal.elements.push_back(output_traffic_signal_element); + output_traffic_signal.traffic_light_group_id = id; + output.traffic_light_groups.push_back(output_traffic_signal); + } + } +} + +void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal) +{ + const auto id = signal.traffic_light_group_id; + + // no record of detected color in last_detect_color_hold_time_ + if (is_flashing_.count(id) == 0) { + is_flashing_.insert(std::make_pair(id, false)); + return; + } + + // flashing green + if ( + signal.elements.front().color == TrafficSignalElement::UNKNOWN && + signal.elements.front().confidence != 0 && // not due to occlusion + current_color_state_.at(id) != TrafficSignalElement::UNKNOWN) { + is_flashing_.at(id) = true; + return; + } + + // history exists + if (last_colors_.count(id) > 0) { + std::vector history = last_colors_.at(id); + for (const auto & h : history) { + if (h.first.elements.front().color != signal.elements.front().color) { + // keep the current value if not same with input signal + return; + } + } + // all history is same with input signal + is_flashing_.at(id) = false; + } + + // no record of detected color in last_color_hold_time_ + // keep the current value + return; +} + +uint8_t CrosswalkTrafficLightEstimatorNode::updateAndGetColorState(const TrafficSignal & signal) +{ + const auto id = signal.traffic_light_group_id; + const auto color = signal.elements[0].color; + + if (current_color_state_.count(id) == 0) { + current_color_state_.insert(std::make_pair(id, color)); + } else if (is_flashing_.at(id) == false) { + current_color_state_.at(id) = color; + } else if (is_flashing_.at(id) == true) { + if ( + current_color_state_.at(id) == TrafficSignalElement::GREEN && + color == TrafficSignalElement::RED) { + current_color_state_.at(id) = TrafficSignalElement::RED; + } else if ( + current_color_state_.at(id) == TrafficSignalElement::RED && + color == TrafficSignalElement::GREEN) { + current_color_state_.at(id) = TrafficSignalElement::GREEN; + } else if (current_color_state_.at(id) == TrafficSignalElement::UNKNOWN) { + if (color == TrafficSignalElement::GREEN || color == TrafficSignalElement::UNKNOWN) + current_color_state_.at(id) = TrafficSignalElement::GREEN; + if (color == TrafficSignalElement::RED) + current_color_state_.at(id) = TrafficSignalElement::RED; + } + } + + return current_color_state_.at(id); +} + +lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets( + const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const +{ + lanelet::ConstLanelets non_red_lanelets{}; + + for (const auto & lanelet : lanelets) { + const auto tl_reg_elems = lanelet.regulatoryElementsAs(); + + if (tl_reg_elems.empty()) { + continue; + } + + const auto tl_reg_elem = tl_reg_elems.front(); + const auto current_detected_signal = + getHighestConfidenceTrafficSignal(tl_reg_elem->id(), traffic_light_id_map); + + if (!current_detected_signal && !use_last_detect_color_) { + continue; + } + + const auto current_is_not_red = + current_detected_signal ? current_detected_signal.get() == TrafficSignalElement::GREEN || + current_detected_signal.get() == TrafficSignalElement::AMBER + : true; + + const auto current_is_unknown_or_none = + current_detected_signal ? current_detected_signal.get() == TrafficSignalElement::UNKNOWN + : true; + + const auto last_detected_signal = + getHighestConfidenceTrafficSignal(tl_reg_elem->id(), last_detect_color_); + + if (!last_detected_signal) { + continue; + } + + const auto was_not_red = current_is_unknown_or_none && + (last_detected_signal.get() == TrafficSignalElement::GREEN || + last_detected_signal.get() == TrafficSignalElement::AMBER) && + use_last_detect_color_; + + if (!current_is_not_red && !was_not_red) { + continue; + } + + non_red_lanelets.push_back(lanelet); + } + + return non_red_lanelets; +} + +uint8_t CrosswalkTrafficLightEstimatorNode::estimateCrosswalkTrafficSignal( + const lanelet::ConstLanelet & crosswalk, const lanelet::ConstLanelets & non_red_lanelets) const +{ + bool has_left_non_red_lane = false; + bool has_right_non_red_lane = false; + bool has_straight_non_red_lane = false; + bool has_related_non_red_tl = false; + + const std::string related_tl_id = crosswalk.attributeOr("related_traffic_light", "none"); + + for (const auto & lanelet : non_red_lanelets) { + const std::string turn_direction = lanelet.attributeOr("turn_direction", "none"); + + if (turn_direction == "left") { + has_left_non_red_lane = true; + } else if (turn_direction == "right") { + has_right_non_red_lane = true; + } else { + has_straight_non_red_lane = true; + } + + const auto tl_reg_elems = lanelet.regulatoryElementsAs(); + if (tl_reg_elems.front()->id() == std::atoi(related_tl_id.c_str())) { + has_related_non_red_tl = true; + } + } + + if (has_straight_non_red_lane || has_related_non_red_tl) { + return TrafficSignalElement::RED; + } + + const auto has_merge_lane = hasMergeLane(non_red_lanelets, routing_graph_ptr_); + return !has_merge_lane && has_left_non_red_lane && has_right_non_red_lane + ? TrafficSignalElement::RED + : TrafficSignalElement::UNKNOWN; +} + +boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal( + const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, + const TrafficLightIdMap & traffic_light_id_map) const +{ + boost::optional ret{boost::none}; + + double highest_confidence = 0.0; + for (const auto & traffic_light : traffic_lights) { + if (!traffic_light.isLineString()) { + continue; + } + + const int id = static_cast(traffic_light).id(); + if (traffic_light_id_map.count(id) == 0) { + continue; + } + + const auto & elements = traffic_light_id_map.at(id).first.elements; + if (elements.empty()) { + continue; + } + + const auto & color = elements.front().color; + const auto & confidence = elements.front().confidence; + if (confidence < highest_confidence) { + continue; + } + + highest_confidence = confidence; + ret = color; + } + + return ret; +} + +boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal( + const lanelet::Id & id, const TrafficLightIdMap & traffic_light_id_map) const +{ + boost::optional ret{boost::none}; + + double highest_confidence = 0.0; + if (traffic_light_id_map.count(id) == 0) { + return ret; + } + + for (const auto & element : traffic_light_id_map.at(id).first.elements) { + if (element.confidence < highest_confidence) { + continue; + } + + highest_confidence = element.confidence; + ret = element.color; + } + + return ret; +} + +void CrosswalkTrafficLightEstimatorNode::removeDuplicateIds(TrafficSignalArray & signal_array) const +{ + auto & signals = signal_array.traffic_light_groups; + std::sort(signals.begin(), signals.end(), [](const auto & s1, const auto & s2) { + return s1.traffic_light_group_id < s2.traffic_light_group_id; + }); + + signals.erase( + std::unique( + signals.begin(), signals.end(), + [](const auto & s1, const auto s2) { + return s1.traffic_light_group_id == s2.traffic_light_group_id; + }), + signals.end()); +} + +} // namespace autoware::crosswalk_traffic_light_estimator + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::crosswalk_traffic_light_estimator::CrosswalkTrafficLightEstimatorNode) diff --git a/perception/autoware_map_based_prediction/CMakeLists.txt b/perception/autoware_map_based_prediction/CMakeLists.txt new file mode 100644 index 0000000000000..9a1d9a1947c7c --- /dev/null +++ b/perception/autoware_map_based_prediction/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_map_based_prediction) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) + +find_package(glog REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} +) + +ament_auto_add_library(map_based_prediction_node SHARED + src/map_based_prediction_node.cpp + src/path_generator.cpp + src/debug.cpp +) + +target_link_libraries(map_based_prediction_node glog::glog) + +rclcpp_components_register_node(map_based_prediction_node + PLUGIN "autoware::map_based_prediction::MapBasedPredictionNode" + EXECUTABLE map_based_prediction +) + +## Tests +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(test_map_based_prediction ${test_files}) + + target_link_libraries(test_map_based_prediction + map_based_prediction_node + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/perception/autoware_map_based_prediction/README.md b/perception/autoware_map_based_prediction/README.md new file mode 100644 index 0000000000000..bd063050b9241 --- /dev/null +++ b/perception/autoware_map_based_prediction/README.md @@ -0,0 +1,263 @@ +# map_based_prediction + +## Role + +`map_based_prediction` is a module to predict the future paths (and their probabilities) of other vehicles and pedestrians according to the shape of the map and the surrounding environment. + +## Assumptions + +- The following information about the target obstacle is needed + - Label (type of person, car, etc.) + - The object position in the current time and predicted position in the future time. +- The following information about the surrounding environment is needed + - Road network information with Lanelet2 format + +## Inner-workings / Algorithms + +### Flow chart + +
+ +
+ +### Path prediction for road users + +#### Remove old object history + +Store time-series data of objects to determine the vehicle's route and to detect lane change for several duration. Object Data contains the object's position, speed, and time information. + +#### Get current lanelet and update Object history + +Search one or more lanelets satisfying the following conditions for each target object and store them in the ObjectData. + +- The CoG of the object must be inside the lanelet. +- The centerline of the lanelet must have two or more points. +- The angle difference between the lanelet and the direction of the object must be within the threshold given by the parameters. + - The angle flip is allowed, the condition is `diff_yaw < threshold or diff_yaw > pi - threshold`. +- The lanelet must be reachable from the lanelet recorded in the past history. + +#### Get predicted reference path + +- Get reference path: + - Create a reference path for the object from the associated lanelet. +- Predict object maneuver: + - Generate predicted paths for the object. + - Assign probability to each maneuver of `Lane Follow`, `Left Lane Change`, and `Right Lane Change` based on the object history and the reference path obtained in the first step. + - Lane change decision is based on two domains: + - Geometric domain: the lateral distance between the center of gravity of the object and left/right boundaries of the lane. + - Time domain: estimated time margin for the object to reach the left/right bound. + +The conditions for left lane change detection are: + +- Check if the distance to the left lane boundary is less than the distance to the right lane boundary. +- Check if the distance to the left lane boundary is less than a `dist_threshold_to_bound_`. +- Check if the lateral velocity direction is towards the left lane boundary. +- Check if the time to reach the left lane boundary is less than `time_threshold_to_bound_`. + +Lane change logics is illustrated in the figure below.An example of how to tune the parameters is described later. + +![lane change detection](./media/lane_change_detection.drawio.svg) + +- Calculate object probability: + - The path probability obtained above is calculated based on the current position and angle of the object. +- Refine predicted paths for smooth movement: + - The generated predicted paths are recomputed to take the vehicle dynamics into account. + - The path is calculated with minimum jerk trajectory implemented by 4th/5th order spline for lateral/longitudinal motion. + +### Tuning lane change detection logic + +Currently we provide three parameters to tune lane change detection: + +- `dist_threshold_to_bound_`: maximum distance from lane boundary allowed for lane changing vehicle +- `time_threshold_to_bound_`: maximum time allowed for lane change vehicle to reach the boundary +- `cutoff_freq_of_velocity_lpf_`: cutoff frequency of low pass filter for lateral velocity + +You can change these parameters in rosparam in the table below. + +| param name | default value | +| --------------------------------------------------- | ------------- | +| `dist_threshold_for_lane_change_detection` | `1.0` [m] | +| `time_threshold_for_lane_change_detection` | `5.0` [s] | +| `cutoff_freq_of_velocity_for_lane_change_detection` | `0.1` [Hz] | + +#### Tuning threshold parameters + +Increasing these two parameters will slow down and stabilize the lane change estimation. + +Normally, we recommend tuning only `time_threshold_for_lane_change_detection` because it is the more important factor for lane change decision. + +#### Tuning lateral velocity calculation + +Lateral velocity calculation is also a very important factor for lane change decision because it is used in the time domain decision. + +The predicted time to reach the lane boundary is calculated by + +$$ +t_{predicted} = \dfrac{d_{lat}}{v_{lat}} +$$ + +where $d_{lat}$ and $v_{lat}$ represent the lateral distance to the lane boundary and the lateral velocity, respectively. + +Lowering the cutoff frequency of the low-pass filter for lateral velocity will make the lane change decision more stable but slower. Our setting is very conservative, so you may increase this parameter if you want to make the lane change decision faster. + +For the additional information, here we show how we calculate lateral velocity. + +| lateral velocity calculation method | equation | description | +| ------------------------------------------------------------- | ---------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| [**applied**] time derivative of lateral distance | $\dfrac{\Delta d_{lat}}{\Delta t}$ | Currently, we use this method to deal with winding roads. Since this time differentiation easily becomes noisy, we also use a low-pass filter to get smoothed velocity. | +| [not applied] Object Velocity Projection to Lateral Direction | $v_{obj} \sin(\theta)$ | Normally, object velocities are less noisy than the time derivative of lateral distance. But the yaw difference $\theta$ between the lane and object directions sometimes becomes discontinuous, so we did not adopt this method. | + +Currently, we use the upper method with a low-pass filter to calculate lateral velocity. + +### Path generation + +Path generation is generated on the frenet frame. The path is generated by the following steps: + +1. Get the frenet frame of the reference path. +2. Generate the frenet frame of the current position of the object and the finite position of the object. +3. Optimize the path in each longitudinal and lateral coordinate of the frenet frame. (Use the quintic polynomial to fit start and end conditions.) +4. Convert the path to the global coordinate. + +See paper [2] for more details. + +#### Tuning lateral path shape + +`lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.) + +#### Pruning predicted paths with lateral acceleration constraint (for vehicle obstacles) + +It is possible to apply a maximum lateral acceleration constraint to generated vehicle paths. This check verifies if it is possible for the vehicle to perform the predicted path without surpassing a lateral acceleration threshold `max_lateral_accel` when taking a curve. If it is not possible, it checks if the vehicle can slow down on time to take the curve with a deceleration of `min_acceleration_before_curve` and comply with the constraint. If that is also not possible, the path is eliminated. + +Currently we provide three parameters to tune the lateral acceleration constraint: + +- `check_lateral_acceleration_constraints_`: to enable the constraint check. +- `max_lateral_accel_`: max acceptable lateral acceleration for predicted paths (absolute value). +- `min_acceleration_before_curve_`: the minimum acceleration the vehicle would theoretically use to slow down before a curve is taken (must be negative). + +You can change these parameters in rosparam in the table below. + +| param name | default value | +| ---------------------------------------- | -------------- | +| `check_lateral_acceleration_constraints` | `false` [bool] | +| `max_lateral_accel` | `2.0` [m/s^2] | +| `min_acceleration_before_curve` | `-2.0` [m/s^2] | + +## Using Vehicle Acceleration for Path Prediction (for Vehicle Obstacles) + +By default, the `map_based_prediction` module uses the current obstacle's velocity to compute its predicted path length. However, it is possible to use the obstacle's current acceleration to calculate its predicted path's length. + +### Decaying Acceleration Model + +Since this module tries to predict the vehicle's path several seconds into the future, it is not practical to consider the current vehicle's acceleration as constant (it is not assumed the vehicle will be accelerating for `prediction_time_horizon` seconds after detection). Instead, a decaying acceleration model is used. With the decaying acceleration model, a vehicle's acceleration is modeled as: + +$\ a(t) = a\_{t0} \cdot e^{-\lambda \cdot t} $ + +where $\ a\_{t0} $ is the vehicle acceleration at the time of detection $\ t0 $, and $\ \lambda $ is the decay constant $\ \lambda = \ln(2) / hl $ and $\ hl $ is the exponential's half life. + +Furthermore, the integration of $\ a(t) $ over time gives us equations for velocity, $\ v(t) $ and distance $\ x(t) $ as: + +$\ v(t) = v*{t0} + a*{t0} \* (1/\lambda) \cdot (1 - e^{-\lambda \cdot t}) $ + +and + +$\ x(t) = x*{t0} + (v*{t0} + a*{t0} \* (1/\lambda)) \cdot t + a*{t0}(1/λ^2)(e^{-\lambda \cdot t} - 1) $ + +With this model, the influence of the vehicle's detected instantaneous acceleration on the predicted path's length is diminished but still considered. This feature also considers that the obstacle might not accelerate past its road's speed limit (multiplied by a tunable factor). + +Currently, we provide three parameters to tune the use of obstacle acceleration for path prediction: + +- `use_vehicle_acceleration`: to enable the feature. +- `acceleration_exponential_half_life`: The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds. +- `speed_limit_multiplier`: Set the vehicle type obstacle's maximum predicted speed as the legal speed limit in that lanelet times this value. This value should be at least equal or greater than 1.0. + +You can change these parameters in `rosparam` in the table below. + +| Param Name | Default Value | +| ------------------------------------ | -------------- | +| `use_vehicle_acceleration` | `false` [bool] | +| `acceleration_exponential_half_life` | `2.5` [s] | +| `speed_limit_multiplier` | `1.5` [] | + +### Path prediction for crosswalk users + +This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions: + +- move toward the crosswalk +- stop near the crosswalk + +
+ +
+ +If there are a reachable crosswalk entry points within the `prediction_time_horizon` and the objects satisfies above condition, this module outputs additional predicted path to cross the opposite side via the crosswalk entry point. + +This module takes into account the corresponding traffic light information. +When RED signal is indicated, we assume the target object will not walk across. +In addition, if the target object is stopping (not moving) against GREEN signal, we assume the target object will not walk across either. +This prediction comes from the assumption that the object should move if the traffic light is green and the object is intended to cross. + +
+ +
+ +If the target object is inside the road or crosswalk, this module outputs one or two additional prediction path(s) to reach exit point of the crosswalk. The number of prediction paths are depend on whether object is moving or not. If the object is moving, this module outputs one prediction path toward an exit point that existed in the direction of object's movement. One the other hand, if the object has stopped, it is impossible to infer which exit points the object want to go, so this module outputs two prediction paths toward both side exit point. + +
+ +
+ +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------------------------------------------- | ------------------------------------------------------- | ---------------------------------------------------------- | +| `~/perception/object_recognition/tracking/objects` | `autoware_perception_msgs::msg::TrackedObjects` | tracking objects without predicted path. | +| `~/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | binary data of Lanelet2 Map. | +| `~/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | rearranged information on the corresponding traffic lights | + +### Output + +| Name | Type | Description | +| ---------------------------- | ------------------------------------------------- | ------------------------------------------------------------------------------------- | +| `~/input/objects` | `autoware_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | +| `~/output/objects` | `autoware_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | +| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | +| `~/debug/processing_time_ms` | `std_msgs::msg::Float64` | processing time of this module. | +| `~/debug/cyclic_time_ms` | `std_msgs::msg::Float64` | cyclic time of this module. | + +## Parameters + +| Parameter | Unit | Type | Description | +| ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- | +| `enable_delay_compensation` | [-] | bool | flag to enable the time delay compensation for the position of the object | +| `prediction_time_horizon` | [s] | double | predict time duration for predicted path | +| `lateral_control_time_horizon` | [s] | double | time duration for predicted path will reach the reference path (mostly center of the lane) | +| `prediction_sampling_delta_time` | [s] | double | sampling time for points in predicted path | +| `min_velocity_for_map_based_prediction` | [m/s] | double | apply map-based prediction to the objects with higher velocity than this value | +| `min_crosswalk_user_velocity` | [m/s] | double | minimum velocity used when crosswalk user's velocity is calculated | +| `max_crosswalk_user_delta_yaw_threshold_for_lanelet` | [rad] | double | maximum yaw difference between crosswalk user and lanelet to use in path prediction for crosswalk users | +| `dist_threshold_for_searching_lanelet` | [m] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `delta_yaw_threshold_for_searching_lanelet` | [rad] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `sigma_lateral_offset` | [m] | double | Standard deviation for lateral position of objects | +| `sigma_yaw_angle_deg` | [deg] | double | Standard deviation yaw angle of objects | +| `object_buffer_time_length` | [s] | double | Time span of object history to store the information | +| `history_time_length` | [s] | double | Time span of object information used for prediction | +| `prediction_time_horizon_rate_for_validate_shoulder_lane_length` | [-] | double | prediction path will disabled when the estimated path length exceeds lanelet length. This parameter control the estimated path length | + +## Assumptions / Known limits + +- For object types of passenger car, bus, and truck + - The predicted path of the object follows the road structure. + - For the object not being on any roads, the predicted path is generated by just a straight line prediction. + - For the object on a lanelet but moving in a different direction of the road, the predicted path is just straight. + - Vehicle dynamics may not be properly considered in the predicted path. +- For object types of person and motorcycle + - The predicted path is generated by just a straight line in all situations except for "around crosswalk". +- For all obstacles + - In the prediction, the vehicle motion is assumed to be a constant velocity due to a lack of acceleration information. + +## Reference + +1. M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectory generation for dynamic street scenario in a frenet frame,” IEEE International Conference on Robotics and Automation, Anchorage, Alaska, USA, May 2010. +2. A. Houenou, P. Bonnifait, V. Cherfaoui, and Wen Yao, “Vehicle trajectory prediction based on motion model and maneuver recognition,” in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, nov 2013, pp. 4363-4369. diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml similarity index 96% rename from perception/map_based_prediction/config/map_based_prediction.param.yaml rename to perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml index a1b776bdb6393..a5b7b0ad6be01 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml @@ -1,7 +1,10 @@ /**: ros__parameters: enable_delay_compensation: true - prediction_time_horizon: 10.0 #[s] + prediction_time_horizon: + vehicle: 15.0 #[s] + pedestrian: 10.0 #[s] + unknown: 10.0 #[s] lateral_control_time_horizon: 5.0 #[s] prediction_sampling_delta_time: 0.5 #[s] min_velocity_for_map_based_prediction: 1.39 #[m/s] diff --git a/perception/map_based_prediction/images/exception.svg b/perception/autoware_map_based_prediction/images/exception.svg similarity index 100% rename from perception/map_based_prediction/images/exception.svg rename to perception/autoware_map_based_prediction/images/exception.svg diff --git a/perception/map_based_prediction/images/inside_road.svg b/perception/autoware_map_based_prediction/images/inside_road.svg similarity index 100% rename from perception/map_based_prediction/images/inside_road.svg rename to perception/autoware_map_based_prediction/images/inside_road.svg diff --git a/perception/map_based_prediction/images/outside_road.svg b/perception/autoware_map_based_prediction/images/outside_road.svg similarity index 100% rename from perception/map_based_prediction/images/outside_road.svg rename to perception/autoware_map_based_prediction/images/outside_road.svg diff --git a/perception/map_based_prediction/images/target_objects.svg b/perception/autoware_map_based_prediction/images/target_objects.svg similarity index 100% rename from perception/map_based_prediction/images/target_objects.svg rename to perception/autoware_map_based_prediction/images/target_objects.svg diff --git a/perception/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 similarity index 82% rename from perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp rename to perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 213f18d63ef3a..ce1c587cc3b01 100644 --- a/perception/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,23 +15,23 @@ #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 "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include -#include -#include -#include + +#include +#include +#include +#include #include #include #include @@ -52,7 +52,7 @@ #include #include -namespace map_based_prediction +namespace autoware::map_based_prediction { struct LateralKinematicsToLanelet { @@ -89,7 +89,7 @@ struct ObjectData struct CrosswalkUserData { std_msgs::msg::Header header; - autoware_auto_perception_msgs::msg::TrackedObject tracked_object; + autoware_perception_msgs::msg::TrackedObject tracked_object; }; struct LaneletData @@ -106,22 +106,30 @@ struct PredictedRefPath Maneuver maneuver; }; +struct PredictionTimeHorizon +{ + // NOTE(Mamoru Sobue): motorcycle belongs to "vehicle" and bicycle to "pedestrian" + double vehicle; + double pedestrian; + double unknown; +}; + using LaneletsData = std::vector; using ManeuverProbability = std::unordered_map; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjectKinematics; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; -using autoware_auto_perception_msgs::msg::TrackedObjects; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using autoware_perception_msgs::msg::TrafficSignal; -using autoware_perception_msgs::msg::TrafficSignalArray; -using autoware_perception_msgs::msg::TrafficSignalElement; -using tier4_autoware_utils::StopWatch; +using autoware::universe_utils::StopWatch; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjectKinematics; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjectKinematics; +using autoware_perception_msgs::msg::TrackedObjects; +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_debug_msgs::msg::StringStamped; using TrajectoryPoints = std::vector; class MapBasedPredictionNode : public rclcpp::Node @@ -134,12 +142,13 @@ class MapBasedPredictionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_objects_; rclcpp::Publisher::SharedPtr pub_debug_markers_; rclcpp::Subscription::SharedPtr sub_objects_; - rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Subscription::SharedPtr sub_traffic_signals_; + rclcpp::Subscription::SharedPtr sub_map_; + 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; @@ -152,7 +161,7 @@ class MapBasedPredictionNode : public rclcpp::Node std::shared_ptr routing_graph_ptr_; std::shared_ptr traffic_rules_ptr_; - std::unordered_map traffic_signal_id_map_; + std::unordered_map traffic_signal_id_map_; // parameter update OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -160,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_; @@ -170,7 +179,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Parameters bool enable_delay_compensation_; - double prediction_time_horizon_; + PredictionTimeHorizon prediction_time_horizon_; double lateral_control_time_horizon_; double prediction_time_horizon_rate_for_validate_lane_length_; double prediction_sampling_time_interval_; @@ -212,11 +221,11 @@ 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 HADMapBin::ConstSharedPtr msg); - void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg); + void mapCallback(const LaneletMapBin::ConstSharedPtr msg); + void trafficSignalsCallback(const TrafficLightGroupArray::ConstSharedPtr msg); void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); bool doesPathCrossAnyFence(const PredictedPath & predicted_path); @@ -253,7 +262,7 @@ class MapBasedPredictionNode : public rclcpp::Node const std::string & object_id, std::unordered_map & current_users); std::vector getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, - const double object_detected_time); + const double object_detected_time, const double time_horizon); Maneuver predictObjectManeuver( const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time); @@ -285,7 +294,7 @@ class MapBasedPredictionNode : public rclcpp::Node bool isDuplicated( const PredictedPath & predicted_path, const std::vector & predicted_paths); std::optional getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet); - std::optional getTrafficSignalElement(const lanelet::Id & id); + std::optional getTrafficSignalElement(const lanelet::Id & id); bool calcIntentionToCrossWithTrafficSignal( const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, const lanelet::Id & signal_id); @@ -305,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); @@ -409,6 +418,6 @@ class MapBasedPredictionNode : public rclcpp::Node return true; }; }; -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction #endif // MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp new file mode 100644 index 0000000000000..bd098894f8a88 --- /dev/null +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -0,0 +1,150 @@ +// Copyright 2021 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 MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ +#define MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::map_based_prediction +{ +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjectKinematics; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjectKinematics; +using autoware_perception_msgs::msg::TrackedObjects; + +struct FrenetPoint +{ + double s; + double d; + float s_vel; + float d_vel; + float s_acc; + float d_acc; +}; + +struct CrosswalkEdgePoints +{ + Eigen::Vector2d front_center_point; + Eigen::Vector2d front_right_point; + Eigen::Vector2d front_left_point; + Eigen::Vector2d back_center_point; + Eigen::Vector2d back_right_point; + Eigen::Vector2d back_left_point; + + void swap() + { + const Eigen::Vector2d tmp_center_point = front_center_point; + const Eigen::Vector2d tmp_right_point = front_right_point; + const Eigen::Vector2d tmp_left_point = front_left_point; + + front_center_point = back_center_point; + front_right_point = back_right_point; + front_left_point = back_left_point; + + back_center_point = tmp_center_point; + back_right_point = tmp_right_point; + back_left_point = tmp_left_point; + } +}; + +using FrenetPath = std::vector; +using PosePath = std::vector; + +class PathGenerator +{ +public: + PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity); + + PredictedPath generatePathForNonVehicleObject( + const TrackedObject & object, const double duration) const; + + PredictedPath generatePathForLowSpeedVehicle( + const TrackedObject & object, const double duration) const; + + PredictedPath generatePathForOffLaneVehicle( + const TrackedObject & object, const double duration) const; + + PredictedPath generatePathForOnLaneVehicle( + const TrackedObject & object, const PosePath & ref_paths, const double duration, + const double lateral_duration, const double speed_limit = 0.0) const; + + PredictedPath generatePathForCrosswalkUser( + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, + const double duration) const; + + PredictedPath generatePathToTargetPoint( + const TrackedObject & object, const Eigen::Vector2d & point) const; + + void setUseVehicleAcceleration(const bool use_vehicle_acceleration) + { + use_vehicle_acceleration_ = use_vehicle_acceleration; + } + + void setAccelerationHalfLife(const double acceleration_exponential_half_life) + { + acceleration_exponential_half_life_ = acceleration_exponential_half_life; + } + +private: + // Parameters + double sampling_time_interval_; + double min_crosswalk_user_velocity_; + bool use_vehicle_acceleration_; + double acceleration_exponential_half_life_; + + // Member functions + PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const; + + PredictedPath generatePolynomialPath( + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double lateral_duration, const double speed_limit = 0.0) const; + + FrenetPath generateFrenetPath( + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, + const double duration, const double lateral_duration) const; + Eigen::Vector3d calcLatCoefficients( + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const; + Eigen::Vector2d calcLonCoefficients( + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const; + + PosePath interpolateReferencePath( + const PosePath & base_path, const FrenetPath & frenet_predicted_path) const; + + PredictedPath convertToPredictedPath( + const TrackedObject & object, const FrenetPath & frenet_predicted_path, + const PosePath & ref_path) const; + + FrenetPoint getFrenetPoint( + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double speed_limit = 0.0) const; +}; +} // namespace autoware::map_based_prediction + +#endif // MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ diff --git a/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml b/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml new file mode 100644 index 0000000000000..b65f0de892dcc --- /dev/null +++ b/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/perception/map_based_prediction/media/lane_change_detection.drawio.svg b/perception/autoware_map_based_prediction/media/lane_change_detection.drawio.svg similarity index 100% rename from perception/map_based_prediction/media/lane_change_detection.drawio.svg rename to perception/autoware_map_based_prediction/media/lane_change_detection.drawio.svg diff --git a/perception/map_based_prediction/media/lanechange_detection_right_to_left.png b/perception/autoware_map_based_prediction/media/lanechange_detection_right_to_left.png similarity index 100% rename from perception/map_based_prediction/media/lanechange_detection_right_to_left.png rename to perception/autoware_map_based_prediction/media/lanechange_detection_right_to_left.png diff --git a/perception/map_based_prediction/media/map_based_prediction_flow.drawio.svg b/perception/autoware_map_based_prediction/media/map_based_prediction_flow.drawio.svg similarity index 100% rename from perception/map_based_prediction/media/map_based_prediction_flow.drawio.svg rename to perception/autoware_map_based_prediction/media/map_based_prediction_flow.drawio.svg diff --git a/perception/map_based_prediction/media/object_history.drawio.svg b/perception/autoware_map_based_prediction/media/object_history.drawio.svg similarity index 100% rename from perception/map_based_prediction/media/object_history.drawio.svg rename to perception/autoware_map_based_prediction/media/object_history.drawio.svg diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml new file mode 100644 index 0000000000000..37bb7aafb01bf --- /dev/null +++ b/perception/autoware_map_based_prediction/package.xml @@ -0,0 +1,38 @@ + + + + autoware_map_based_prediction + 0.1.0 + This package implements a map_based_prediction + Tomoya Kimura + Yoshi Ri + Takayuki Murooka + Kyoichi Sugahara + Kotaro Uetake + Apache License 2.0 + + ament_cmake + autoware_cmake + + autoware_motion_utils + autoware_perception_msgs + autoware_universe_utils + glog + interpolation + lanelet2_extension + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tf2_ros + tier4_debug_msgs + unique_identifier_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/map_based_prediction/schema/map_based_prediction.schema.json b/perception/autoware_map_based_prediction/schema/map_based_prediction.schema.json similarity index 90% rename from perception/map_based_prediction/schema/map_based_prediction.schema.json rename to perception/autoware_map_based_prediction/schema/map_based_prediction.schema.json index 8ddb122ebb56e..8e5ef9e542f54 100644 --- a/perception/map_based_prediction/schema/map_based_prediction.schema.json +++ b/perception/autoware_map_based_prediction/schema/map_based_prediction.schema.json @@ -12,9 +12,23 @@ "description": "flag to enable the time delay compensation for the position of the object" }, "prediction_time_horizon": { - "type": "number", - "default": "10.0", - "description": "predict time duration for predicted path" + "properties": { + "vehicle": { + "type": "number", + "default": "15.0", + "description": "predict time duration for predicted path of vehicle" + }, + "pedestrian": { + "type": "number", + "default": "10.0", + "description": "predict time duration for predicted path of pedestrian" + }, + "unknown": { + "type": "number", + "default": "10.0", + "description": "predict time duration for predicted path of unknown" + } + } }, "lateral_control_time_horizon": { "type": "number", diff --git a/perception/autoware_map_based_prediction/src/debug.cpp b/perception/autoware_map_based_prediction/src/debug.cpp new file mode 100644 index 0000000000000..bd40ee0981c0e --- /dev/null +++ b/perception/autoware_map_based_prediction/src/debug.cpp @@ -0,0 +1,49 @@ +// Copyright 2021 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/universe_utils/ros/marker_helper.hpp" +#include "map_based_prediction/map_based_prediction_node.hpp" + +namespace autoware::map_based_prediction +{ +visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( + const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num) +{ + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + marker.ns = "maneuver"; + + marker.id = static_cast(obj_num); + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = object.kinematics.pose_with_covariance.pose; + marker.scale = autoware::universe_utils::createMarkerScale(3.0, 1.0, 1.0); + + // Color by maneuver + double r = 0.0; + double g = 0.0; + double b = 0.0; + if (maneuver == Maneuver::LEFT_LANE_CHANGE) { + g = 1.0; + } else if (maneuver == Maneuver::RIGHT_LANE_CHANGE) { + r = 1.0; + } else { + b = 1.0; + } + marker.color = autoware::universe_utils::createMarkerColor(r, g, b, 0.8); + + return marker; +} +} // namespace autoware::map_based_prediction diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp similarity index 92% rename from perception/map_based_prediction/src/map_based_prediction_node.cpp rename to perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index b55ea72402d74..f505cb7cc5592 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -14,18 +14,18 @@ #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 +#include #include #include @@ -50,7 +50,7 @@ #include #include -namespace map_based_prediction +namespace autoware::map_based_prediction { namespace @@ -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); @@ -766,7 +766,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ google::InstallFailureSignalHandler(); } enable_delay_compensation_ = declare_parameter("enable_delay_compensation"); - prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); + prediction_time_horizon_.vehicle = declare_parameter("prediction_time_horizon.vehicle"); + prediction_time_horizon_.pedestrian = + declare_parameter("prediction_time_horizon.pedestrian"); + prediction_time_horizon_.unknown = declare_parameter("prediction_time_horizon.unknown"); lateral_control_time_horizon_ = declare_parameter("lateral_control_time_horizon"); // [s] for lateral control point prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time"); @@ -840,8 +843,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ "crosswalk_with_signal.timeout_set_for_no_intention_to_walk"); path_generator_ = std::make_shared( - prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, - min_crosswalk_user_velocity_); + prediction_sampling_time_interval_, min_crosswalk_user_velocity_); path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); @@ -849,24 +851,23 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ sub_objects_ = this->create_subscription( "~/input/objects", 1, std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1)); - sub_map_ = this->create_subscription( + sub_map_ = this->create_subscription( "/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1)); - sub_traffic_signals_ = this->create_subscription( - "/traffic_signals", 1, - std::bind(&MapBasedPredictionNode::trafficSignalsCallback, this, std::placeholders::_1)); pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); 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"); } @@ -874,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_); @@ -911,13 +912,15 @@ PredictedObject MapBasedPredictionNode::convertToPredictedObject( predicted_object.kinematics = convertToPredictedKinematics(tracked_object.kinematics); predicted_object.classification = tracked_object.classification; predicted_object.object_id = tracked_object.object_id; - predicted_object.shape = tracked_object.shape; + predicted_object.shape.type = tracked_object.shape.type; + predicted_object.shape.footprint = tracked_object.shape.footprint; + predicted_object.shape.dimensions = tracked_object.shape.dimensions; predicted_object.existence_probability = tracked_object.existence_probability; return predicted_object; } -void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg) +void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg) { RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Start loading lanelet"); lanelet_map_ptr_ = std::make_shared(); @@ -932,17 +935,27 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg) crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end()); } -void MapBasedPredictionNode::trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg) +void MapBasedPredictionNode::trafficSignalsCallback( + const TrafficLightGroupArray::ConstSharedPtr msg) { traffic_signal_id_map_.clear(); - for (const auto & signal : msg->signals) { - traffic_signal_id_map_[signal.traffic_signal_id] = signal; + for (const auto & signal : msg->traffic_light_groups) { + traffic_signal_id_map_[signal.traffic_light_group_id] = signal; } } void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { stop_watch_ptr_->toc("processing_time", true); + + // take traffic_signal + { + const auto msg = sub_traffic_signals_.takeData(); + if (msg) { + trafficSignalsCallback(msg); + } + } + // Guard for map pointer and frame transformation if (!lanelet_map_ptr_) { return; @@ -997,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 @@ -1023,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); } @@ -1050,8 +1063,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // For off lane obstacles if (current_lanelets.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForOffLaneVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForOffLaneVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1066,8 +1079,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt transformed_object.kinematics.twist_with_covariance.twist.linear.x, transformed_object.kinematics.twist_with_covariance.twist.linear.y); if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1079,13 +1092,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Get Predicted Reference Path for Each Maneuver and current lanelets // return: - const auto ref_paths = - getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); + const auto ref_paths = getPredictedReferencePath( + transformed_object, current_lanelets, objects_detected_time, + prediction_time_horizon_.vehicle); // If predicted reference path is empty, assume this object is out of the lane if (ref_paths.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1110,7 +1124,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt TrackedObject yaw_fixed_transformed_object = transformed_object; if ( transformed_object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE) { + autoware_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE) { replaceObjectYawWithLaneletsYaw(current_lanelets, yaw_fixed_transformed_object); } // Generate Predicted Path @@ -1120,7 +1134,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( - yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit); + yaw_fixed_transformed_object, ref_path.path, prediction_time_horizon_.vehicle, + lateral_control_time_horizon_, ref_path.speed_limit); if (predicted_path.path.empty()) continue; if (!check_lateral_acceleration_constraints_) { @@ -1183,8 +1198,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } default: { auto predicted_unknown_object = convertToPredictedObject(transformed_object); - PredictedPath predicted_path = - path_generator_->generatePathForNonVehicleObject(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject( + transformed_object, prediction_time_horizon_.unknown); predicted_path.confidence = 1.0; predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path); @@ -1324,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(); } @@ -1337,7 +1352,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( { auto predicted_object = convertToPredictedObject(object); { - PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(object); + PredictedPath predicted_path = + path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_.pedestrian); predicted_path.confidence = 1.0; predicted_object.kinematics.predicted_paths.push_back(predicted_path); @@ -1388,7 +1404,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( if (hasPotentialToReach( object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_ * 2.0, + edge_points.front_left_point, prediction_time_horizon_.pedestrian * 2.0, min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point); @@ -1398,7 +1414,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( if (hasPotentialToReach( object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_ * 2.0, + edge_points.back_left_point, prediction_time_horizon_.pedestrian * 2.0, min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point); @@ -1422,27 +1438,27 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const auto reachable_first = hasPotentialToReach( object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + edge_points.front_left_point, prediction_time_horizon_.pedestrian, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); const auto reachable_second = hasPotentialToReach( object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + edge_points.back_left_point, prediction_time_horizon_.pedestrian, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); if (!reachable_first && !reachable_second) { continue; } const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_, + object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_.pedestrian, min_crosswalk_user_velocity_); if (!reachable_crosswalk) { continue; } - PredictedPath predicted_path = - path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); + PredictedPath predicted_path = path_generator_->generatePathForCrosswalkUser( + object, reachable_crosswalk.get(), prediction_time_horizon_.pedestrian); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) { @@ -1468,14 +1484,14 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) { if ( object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { return; } // 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 @@ -1489,20 +1505,21 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) if (abs_object_speed < min_abs_speed) return; switch (object.kinematics.orientation_availability) { - case autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN: { + case autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN: { const auto original_yaw = 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; } } @@ -1518,8 +1535,8 @@ void MapBasedPredictionNode::removeStaleTrafficLightInfo( for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) { const bool isDisappeared = std::none_of( in_objects->objects.begin(), in_objects->objects.end(), - [&it](autoware_auto_perception_msgs::msg::TrackedObject obj) { - return tier4_autoware_utils::toHexString(obj.object_id) == it->first.first; + [&it](autoware_perception_msgs::msg::TrackedObject obj) { + return autoware::universe_utils::toHexString(obj.object_id) == it->first.first; }); if (isDisappeared) { it = stopped_times_against_green_.erase(it); @@ -1628,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; @@ -1646,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 @@ -1673,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(); @@ -1683,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 @@ -1702,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; @@ -1711,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; @@ -1740,7 +1758,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory( std::vector MapBasedPredictionNode::getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, - const double object_detected_time) + const double object_detected_time, const double time_horizon) { const double obj_vel = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -1752,7 +1770,7 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( object.kinematics.acceleration_with_covariance.accel.linear.x, object.kinematics.acceleration_with_covariance.accel.linear.y) : 0.0; - const double t_h = prediction_time_horizon_; + const double t_h = time_horizon; const double λ = std::log(2) / acceleration_exponential_half_life_; auto get_search_distance_with_decaying_acc = [&]() -> double { @@ -1915,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; } @@ -1954,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; } @@ -2025,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; } @@ -2063,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; @@ -2073,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; @@ -2158,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; } @@ -2171,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( @@ -2186,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; } @@ -2313,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; } @@ -2335,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; @@ -2344,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; } @@ -2352,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); } @@ -2384,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; } @@ -2409,7 +2427,7 @@ std::optional MapBasedPredictionNode::getTrafficSignalId( return traffic_light_reg_elems.front()->id(); } -std::optional MapBasedPredictionNode::getTrafficSignalElement( +std::optional MapBasedPredictionNode::getTrafficSignalElement( const lanelet::Id & id) { if (traffic_signal_id_map_.count(id) != 0) { @@ -2430,13 +2448,14 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( { const auto signal_color = [&] { const auto elem_opt = getTrafficSignalElement(signal_id); - return elem_opt ? elem_opt.value().color : TrafficSignalElement::UNKNOWN; + 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 == TrafficSignalElement::GREEN && - tier4_autoware_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < + signal_color == TrafficLightElement::GREEN && + 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()); @@ -2480,14 +2499,14 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( // If the pedestrian disappears, another function erases the old data. } - if (signal_color == TrafficSignalElement::RED) { + if (signal_color == TrafficLightElement::RED) { return false; } return true; } -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction #include -RCLCPP_COMPONENTS_REGISTER_NODE(map_based_prediction::MapBasedPredictionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::map_based_prediction::MapBasedPredictionNode) diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp similarity index 82% rename from perception/map_based_prediction/src/path_generator.cpp rename to perception/autoware_map_based_prediction/src/path_generator.cpp index 83fbc16feb7fa..aeb1b0fedd33f 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -14,27 +14,25 @@ #include "map_based_prediction/path_generator.hpp" +#include +#include #include -#include -#include #include -namespace map_based_prediction +namespace autoware::map_based_prediction { PathGenerator::PathGenerator( - const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval, - const double min_crosswalk_user_velocity) -: time_horizon_(time_horizon), - lateral_time_horizon_(lateral_time_horizon), - sampling_time_interval_(sampling_time_interval), + const double sampling_time_interval, const double min_crosswalk_user_velocity) +: sampling_time_interval_(sampling_time_interval), min_crosswalk_user_velocity_(min_crosswalk_user_velocity) { } -PredictedPath PathGenerator::generatePathForNonVehicleObject(const TrackedObject & object) +PredictedPath PathGenerator::generatePathForNonVehicleObject( + const TrackedObject & object, const double duration) const { - return generateStraightPath(object); + return generateStraightPath(object, duration); } PredictedPath PathGenerator::generatePathToTargetPoint( @@ -51,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; @@ -75,7 +74,8 @@ PredictedPath PathGenerator::generatePathToTargetPoint( } PredictedPath PathGenerator::generatePathForCrosswalkUser( - const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, + const double duration) const { PredictedPath predicted_path{}; const double ep = 0.001; @@ -93,13 +93,14 @@ 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 < time_horizon_ + ep; dt += sampling_time_interval_) { + for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { geometry_msgs::msg::Pose world_frame_pose; if (dt < arrival_time) { world_frame_pose.position.x = @@ -131,45 +132,49 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( return predicted_path; } -PredictedPath PathGenerator::generatePathForLowSpeedVehicle(const TrackedObject & object) const +PredictedPath PathGenerator::generatePathForLowSpeedVehicle( + const TrackedObject & object, const double duration) const { PredictedPath path; path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); const double ep = 0.001; - for (double dt = 0.0; dt < time_horizon_ + ep; dt += sampling_time_interval_) { + for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { path.path.push_back(object.kinematics.pose_with_covariance.pose); } return path; } -PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & object) +PredictedPath PathGenerator::generatePathForOffLaneVehicle( + const TrackedObject & object, const double duration) const { - return generateStraightPath(object); + return generateStraightPath(object, duration); } PredictedPath PathGenerator::generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) + const TrackedObject & object, const PosePath & ref_paths, const double duration, + const double lateral_duration, const double speed_limit) const { if (ref_paths.size() < 2) { - return generateStraightPath(object); + return generateStraightPath(object, duration); } - return generatePolynomialPath(object, ref_paths, speed_limit); + return generatePolynomialPath(object, ref_paths, duration, lateral_duration, speed_limit); } -PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) const +PredictedPath PathGenerator::generateStraightPath( + const TrackedObject & object, const double longitudinal_duration) const { const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; constexpr double ep = 0.001; - const double duration = time_horizon_ + ep; + const double duration = longitudinal_duration + ep; PredictedPath path; 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); } @@ -178,11 +183,12 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) } PredictedPath PathGenerator::generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double lateral_duration, const double speed_limit) const { // Get current Frenet Point - const double ref_path_len = motion_utils::calcArcLength(ref_path); - const auto current_point = getFrenetPoint(object, ref_path, speed_limit); + 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 // Note that we do not set position s, @@ -196,13 +202,13 @@ PredictedPath PathGenerator::generatePolynomialPath( // Step2. Generate Predicted Path on a Frenet coordinate const auto frenet_predicted_path = - generateFrenetPath(current_point, terminal_point, ref_path_len); + generateFrenetPath(current_point, terminal_point, ref_path_len, duration, lateral_duration); // Step3. Interpolate Reference Path for converting predicted path coordinate const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path); if (frenet_predicted_path.size() < 2 || interpolated_ref_path.size() < 2) { - return generateStraightPath(object); + return generateStraightPath(object, duration); } // Step4. Convert predicted trajectory from Frenet to Cartesian coordinate @@ -210,11 +216,10 @@ PredictedPath PathGenerator::generatePolynomialPath( } FrenetPath PathGenerator::generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length) + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, + const double duration, const double lateral_duration) const { FrenetPath path; - const double duration = time_horizon_; - const double lateral_duration = lateral_time_horizon_; // Compute Lateral and Longitudinal Coefficients to generate the trajectory const Eigen::Vector3d lat_coeff = @@ -252,7 +257,7 @@ FrenetPath PathGenerator::generateFrenetPath( } Eigen::Vector3d PathGenerator::calcLatCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const { // Lateral Path Calculation // Quintic polynomial for d @@ -278,7 +283,7 @@ Eigen::Vector3d PathGenerator::calcLatCoefficients( } Eigen::Vector2d PathGenerator::calcLonCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const { // Longitudinal Path Calculation // Quadric polynomial @@ -296,7 +301,7 @@ Eigen::Vector2d PathGenerator::calcLonCoefficients( } PosePath PathGenerator::interpolateReferencePath( - const PosePath & base_path, const FrenetPath & frenet_predicted_path) + const PosePath & base_path, const FrenetPath & frenet_predicted_path) const { PosePath interpolated_path; const size_t interpolate_num = frenet_predicted_path.size(); @@ -314,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)); } } @@ -339,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; @@ -356,7 +361,8 @@ PosePath PathGenerator::interpolateReferencePath( } PredictedPath PathGenerator::convertToPredictedPath( - const TrackedObject & object, const FrenetPath & frenet_predicted_path, const PosePath & ref_path) + const TrackedObject & object, const FrenetPath & frenet_predicted_path, + const PosePath & ref_path) const { PredictedPath predicted_path; predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); @@ -369,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; } @@ -385,14 +392,16 @@ PredictedPath PathGenerator::convertToPredictedPath( } FrenetPoint PathGenerator::getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double speed_limit) const { 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 = @@ -411,7 +420,7 @@ FrenetPoint PathGenerator::getFrenetPoint( : 0.0; // Using a decaying acceleration model. Consult the README for more information about the model. - const double t_h = time_horizon_; + const double t_h = duration; const float λ = std::log(2) / acceleration_exponential_half_life_; auto have_same_sign = [](double a, double b) -> bool { @@ -471,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) + @@ -482,4 +492,4 @@ FrenetPoint PathGenerator::getFrenetPoint( return frenet_point; } -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction diff --git a/perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp b/perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp new file mode 100644 index 0000000000000..08e5b4977e583 --- /dev/null +++ b/perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp @@ -0,0 +1,213 @@ +// 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 "map_based_prediction/path_generator.hpp" + +#include +#include + +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjectKinematics; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjectKinematics; +using autoware_perception_msgs::msg::TrackedObjects; + +TrackedObject generate_static_object(const int label) +{ + ObjectClassification classification; + classification.probability = 1.0; + classification.label = label; + + TrackedObjectKinematics kinematics; + kinematics.pose_with_covariance = geometry_msgs::msg::PoseWithCovariance(); // At origin + kinematics.twist_with_covariance = geometry_msgs::msg::TwistWithCovariance(); // Not moving + kinematics.acceleration_with_covariance = + geometry_msgs::msg::AccelWithCovariance(); // Not accelerating + kinematics.orientation_availability = TrackedObjectKinematics::UNAVAILABLE; + + TrackedObject tracked_object; + tracked_object.object_id = unique_identifier_msgs::msg::UUID(); + tracked_object.existence_probability = 1.0; + tracked_object.classification.push_back(classification); + tracked_object.kinematics = kinematics; + + return tracked_object; +} + +TEST(PathGenerator, test_generatePathForNonVehicleObject) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); + + // Generate pedestrian object + TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForNonVehicleObject(tracked_object, prediction_time_horizon); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForLowSpeedVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForLowSpeedVehicle(tracked_object, prediction_time_horizon); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForOffLaneVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + const PredictedPath predicted_path = + path_generator.generatePathForOffLaneVehicle(tracked_object, prediction_time_horizon); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForOnLaneVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate reference path + autoware::map_based_prediction::PosePath ref_paths; + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + ref_paths.push_back(pose); + + // Generate predicted path + const PredictedPath predicted_path = path_generator.generatePathForOnLaneVehicle( + tracked_object, ref_paths, prediction_time_horizon, lateral_control_time_horizon); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForCrosswalkUser) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); + + // Generate dummy crosswalk + autoware::map_based_prediction::CrosswalkEdgePoints reachable_crosswalk; + reachable_crosswalk.front_center_point << 0.0, 0.0; + reachable_crosswalk.front_right_point << 1.0, 0.0; + reachable_crosswalk.front_left_point << -1.0, 0.0; + reachable_crosswalk.back_center_point << 0.0, 1.0; + reachable_crosswalk.back_right_point << 1.0, 1.0; + reachable_crosswalk.back_left_point << -1.0, 1.0; + + // Generate predicted path + const PredictedPath predicted_path = path_generator.generatePathForCrosswalkUser( + tracked_object, reachable_crosswalk, prediction_time_horizon); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathToTargetPoint) +{ + // Generate Path generator + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate target point + Eigen::Vector2d target_point; + target_point << 0.0, 0.0; + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathToTargetPoint(tracked_object, target_point); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} diff --git a/perception/autoware_map_based_prediction/test/test_map_based_prediction.cpp b/perception/autoware_map_based_prediction/test/test_map_based_prediction.cpp new file mode 100644 index 0000000000000..4c8ad7dd25916 --- /dev/null +++ b/perception/autoware_map_based_prediction/test/test_map_based_prediction.cpp @@ -0,0 +1,26 @@ +// 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 + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/perception/bytetrack/package.xml b/perception/bytetrack/package.xml index 0d1ccc284f888..231a1845bb0ea 100644 --- a/perception/bytetrack/package.xml +++ b/perception/bytetrack/package.xml @@ -15,7 +15,7 @@ cudnn_cmake_module tensorrt_cmake_module - autoware_auto_perception_msgs + autoware_perception_msgs cuda_utils cv_bridge eigen diff --git a/perception/bytetrack/src/bytetrack_node.cpp b/perception/bytetrack/src/bytetrack_node.cpp index eb5e73312a890..64adc83a51abd 100644 --- a/perception/bytetrack/src/bytetrack_node.cpp +++ b/perception/bytetrack/src/bytetrack_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" #include @@ -62,7 +62,7 @@ void ByteTrackNode::on_connect() void ByteTrackNode::on_rect( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr msg) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + using Label = autoware_perception_msgs::msg::ObjectClassification; tier4_perception_msgs::msg::DetectedObjectsWithFeature out_objects; tier4_perception_msgs::msg::DynamicObjectArray out_objects_uuid; @@ -97,7 +97,7 @@ void ByteTrackNode::on_rect( object.feature.roi.height = static_cast(output_height); object.object.existence_probability = tracked_object.score; object.object.classification.emplace_back( - autoware_auto_perception_msgs::build