diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index e298dccefd827..ea86022e7134d 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -19,9 +19,9 @@ common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp -common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp -common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @@ -34,8 +34,9 @@ common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato. 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 kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@tier4.jp -common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@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/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 kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp @@ -43,7 +44,7 @@ common/tier4_debug_tools/** ryohsuke.mitsudome@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@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 yutaka.shimizu@tier4.jp +common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp common/tier4_screen_capture_rviz_plugin/** 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 @@ -54,9 +55,8 @@ 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/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp -common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +control/autonomous_emergency_braking/** 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 takamasa.horibe@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 @@ -81,20 +81,22 @@ launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.j launch/tier4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp launch/tier4_map_launch/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@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 yutaka.shimizu@tier4.jp zulfaqar.azmi@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 launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -launch/tier4_system_launch/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp +launch/tier4_system_launch/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp +localization/geo_pose_projector/** koji.minoda@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/landmark_based_localizer/ar_tag_based_localizer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp -localization/landmark_based_localizer/landmark_parser/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/landmark_manager/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/localization_error_monitor/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/localization_util/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose_initializer/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/pose_instability_detector/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/stop_filter/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/tree_structured_parzen_estimator/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/twist2accel/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp @@ -113,20 +115,20 @@ perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihir 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/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +perception/detected_object_validation/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/detection_by_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp perception/euclidean_cluster/** yukihiro.saito@tier4.jp perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/heatmap_visualizer/** kotaro.uetake@tier4.jp -perception/image_projection_based_fusion/** yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp +perception/image_projection_based_fusion/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp perception/lidar_centerpoint/** kenzo.lobos@tier4.jp yusuke.muramatsu@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 shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp +perception/map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yukihiro.saito@tier4.jp @@ -152,7 +154,10 @@ perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhon perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp -planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp +planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@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 tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner_common/** fumiya.watanabe@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_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 satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -161,31 +166,33 @@ planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai 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 yutaka.shimizu@tier4.jp -planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +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/** 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/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@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 yutaka.shimizu@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 yutaka.shimizu@tier4.jp -planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp -planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp -planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@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 ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@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 yutaka.shimizu@tier4.jp +planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp 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_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp -planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp -planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp +planning/route_handler/** fumiya.watanabe@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 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 @@ -216,15 +223,15 @@ system/default_ad_api/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabut system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp -system/dummy_diag_publisher/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp +system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp system/duplicated_node_checker/** shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp system/emergency_handler/** makoto.kurihara@tier4.jp -system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp -system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp +system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp +system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp system/system_diagnostic_graph/** isamu.takagi@tier4.jp system/system_error_monitor/** fumihito.ito@tier4.jp -system/system_monitor/** akihiro.sakurai@tier4.jp 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 tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp diff --git a/.github/workflows/spell-check-all.yaml b/.github/workflows/spell-check-all.yaml index 4feea0dbadcf7..057bd78da2a82 100644 --- a/.github/workflows/spell-check-all.yaml +++ b/.github/workflows/spell-check-all.yaml @@ -2,6 +2,8 @@ name: spell-check-all on: workflow_dispatch: + schedule: + - cron: 0 0 * * * jobs: spell-check-all: diff --git a/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml b/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml index 5b8c019de5a20..46738a33ae0bd 100644 --- a/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml +++ b/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml @@ -1,3 +1,4 @@ /**: ros__parameters: update_rate: 10.0 + oneshot: false diff --git a/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml b/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml index 7a796049384bc..7ba2eb45c9233 100644 --- a/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml +++ b/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml @@ -1,8 +1,6 @@ - - diff --git a/common/goal_distance_calculator/schema/goal_distance_calculator.schema.json b/common/goal_distance_calculator/schema/goal_distance_calculator.schema.json new file mode 100644 index 0000000000000..bc8d7d808619f --- /dev/null +++ b/common/goal_distance_calculator/schema/goal_distance_calculator.schema.json @@ -0,0 +1,36 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Goal Distance Calculator Node", + "type": "object", + "definitions": { + "goal_distance_calculator": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": "10.0", + "exclusiveMinimum": 0, + "description": "Timer callback period. [Hz]" + }, + "oneshot": { + "type": "boolean", + "default": "false", + "description": "Publish deviations just once or repeatedly." + } + }, + "required": ["update_rate", "oneshot"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/goal_distance_calculator" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} 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 6b147ed66d690..c6062d2a156ee 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -40,8 +40,8 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions durable_qos.transient_local(); // Node Parameter - node_param_.update_rate = declare_parameter("update_rate", 10.0); - node_param_.oneshot = declare_parameter("oneshot", true); + node_param_.update_rate = declare_parameter("update_rate"); + node_param_.oneshot = declare_parameter("oneshot"); // Core goal_distance_calculator_ = std::make_unique(); diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index c95fc902ade44..94baf50992cf5 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -222,16 +222,48 @@ TEST(spline_interpolation, splineByAkima) TEST(spline_interpolation, SplineInterpolation) { - // curve: query_keys is random - const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; - const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; - const std::vector query_keys{0.0, 8.0, 18.0}; - const std::vector ans{-0.075611, 0.997242, 1.573258}; + { + // curve: query_keys is random + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 18.0}; + const std::vector ans{-0.075611, 0.997242, 1.573258}; + + SplineInterpolation s(base_keys, base_values); + const std::vector query_values = s.getSplineInterpolatedValues(query_keys); + + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { + // getSplineInterpolatedDiffValues + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 12.0, 18.0}; + const std::vector ans{0.671301, 0.0509853, 0.209426, -0.253628}; - SplineInterpolation s(base_keys, base_values); - const std::vector query_values = s.getSplineInterpolatedValues(query_keys); + SplineInterpolation s(base_keys, base_values); + const std::vector query_values = s.getSplineInterpolatedDiffValues(query_keys); - for (size_t i = 0; i < query_values.size(); ++i) { - EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { + // getSplineInterpolatedQuadDiffValues + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 12.0, 18.0}; + const std::vector ans{-0.156582, 0.0440771, -0.0116873, -0.0495025}; + + SplineInterpolation s(base_keys, base_values); + const std::vector query_values = s.getSplineInterpolatedQuadDiffValues(query_keys); + + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } } } diff --git a/common/motion_utils/include/motion_utils/distance/distance.hpp b/common/motion_utils/include/motion_utils/distance/distance.hpp index 2e25f5f0e53d7..0a0d00fc9f260 100644 --- a/common/motion_utils/include/motion_utils/distance/distance.hpp +++ b/common/motion_utils/include/motion_utils/distance/distance.hpp @@ -15,17 +15,16 @@ #ifndef MOTION_UTILS__DISTANCE__DISTANCE_HPP_ #define MOTION_UTILS__DISTANCE__DISTANCE_HPP_ -#include - #include #include #include +#include #include #include namespace motion_utils { -boost::optional calcDecelDistWithJerkAndAccConstraints( +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); diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index 41a9e9dd41a40..c23478605c41a 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -21,10 +21,9 @@ #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include - #include #include +#include #include #include 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 index 74862a6229a46..e088285147735 100644 --- 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 @@ -18,12 +18,11 @@ #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include -#include - +#include #include namespace motion_utils { -boost::optional> getPathIndexRangeWithLaneId( +std::optional> getPathIndexRangeWithLaneId( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); size_t findNearestIndexFromLaneId( diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index f4f9f45935347..cb815f7dd0a0c 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -26,10 +26,9 @@ #include #include -#include - #include #include +#include #include #include #include @@ -93,10 +92,10 @@ void validateNonSharpAngle( * @return (forward / backward) driving (true / false) */ template -boost::optional isDrivingForward(const T & points) +std::optional isDrivingForward(const T & points) { if (points.size() < 2) { - return boost::none; + return std::nullopt; } // check the first point direction @@ -106,13 +105,13 @@ boost::optional isDrivingForward(const T & points) return tier4_autoware_utils::isDrivingForward(first_pose, second_pose); } -extern template boost::optional +extern template std::optional isDrivingForward>( const std::vector &); -extern template boost::optional +extern template std::optional isDrivingForward>( const std::vector &); -extern template boost::optional +extern template std::optional isDrivingForward>( const std::vector &); @@ -123,10 +122,10 @@ isDrivingForward> * @return (forward / backward) driving (true, false, none "if velocity is zero") */ template -boost::optional isDrivingForwardWithTwist(const T & points_with_twist) +std::optional isDrivingForwardWithTwist(const T & points_with_twist) { if (points_with_twist.empty()) { - return boost::none; + return std::nullopt; } if (points_with_twist.size() == 1) { if (0.0 < tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { @@ -134,20 +133,20 @@ boost::optional isDrivingForwardWithTwist(const T & points_with_twist) } else if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { return false; } else { - return boost::none; + return std::nullopt; } } return isDrivingForward(points_with_twist); } -extern template boost::optional +extern template std::optional isDrivingForwardWithTwist>( const std::vector &); -extern template boost::optional +extern template std::optional isDrivingForwardWithTwist>( const std::vector &); -extern template boost::optional +extern template std::optional isDrivingForwardWithTwist>( const std::vector &); @@ -209,7 +208,7 @@ removeOverlapPoints -boost::optional searchZeroVelocityIndex( +std::optional searchZeroVelocityIndex( const T & points_with_twist, const size_t src_idx, const size_t dst_idx) { try { @@ -229,7 +228,7 @@ boost::optional searchZeroVelocityIndex( return {}; } -extern template boost::optional +extern template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist, const size_t src_idx, const size_t dst_idx); @@ -242,7 +241,7 @@ searchZeroVelocityIndex -boost::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t src_idx) +std::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t src_idx) { try { validateNonEmpty(points_with_twist); @@ -254,7 +253,7 @@ boost::optional searchZeroVelocityIndex(const T & points_with_twist, con return searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); } -extern template boost::optional +extern template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist, const size_t src_idx); @@ -266,12 +265,12 @@ searchZeroVelocityIndex -boost::optional searchZeroVelocityIndex(const T & points_with_twist) +std::optional searchZeroVelocityIndex(const T & points_with_twist) { return searchZeroVelocityIndex(points_with_twist, 0, points_with_twist.size()); } -extern template boost::optional +extern template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist); @@ -329,7 +328,7 @@ findNearestIndex> * @return index of nearest point (index or none if not found) */ template -boost::optional findNearestIndex( +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()) @@ -363,20 +362,24 @@ boost::optional findNearestIndex( min_idx = i; is_nearest_found = true; } - return is_nearest_found ? boost::optional(min_idx) : boost::none; + + if (is_nearest_found) { + return min_idx; + } + return std::nullopt; } -extern template boost::optional +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 boost::optional +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 boost::optional +extern template std::optional findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), @@ -507,7 +510,7 @@ findNearestSegmentIndex -boost::optional findNearestSegmentIndex( +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()) @@ -515,7 +518,7 @@ boost::optional findNearestSegmentIndex( const auto nearest_idx = findNearestIndex(points, pose, max_dist, max_yaw); if (!nearest_idx) { - return boost::none; + return std::nullopt; } if (*nearest_idx == 0) { @@ -534,17 +537,17 @@ boost::optional findNearestSegmentIndex( return *nearest_idx; } -extern template boost::optional +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 boost::optional +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 boost::optional +extern template std::optional findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), @@ -998,7 +1001,7 @@ calcCurvatureAndArcLength -boost::optional calcDistanceToForwardStopPoint( +std::optional calcDistanceToForwardStopPoint( const T & points_with_twist, const size_t src_idx = 0) { try { @@ -1011,13 +1014,13 @@ boost::optional calcDistanceToForwardStopPoint( const auto closest_stop_idx = searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); if (!closest_stop_idx) { - return boost::none; + return std::nullopt; } return std::max(0.0, calcSignedArcLength(points_with_twist, src_idx, *closest_stop_idx)); } -extern template boost::optional +extern template std::optional calcDistanceToForwardStopPoint>( const std::vector & points_with_twist, const size_t src_idx = 0); @@ -1032,7 +1035,7 @@ calcDistanceToForwardStopPoint -boost::optional calcLongitudinalOffsetPoint( +std::optional calcLongitudinalOffsetPoint( const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) { try { @@ -1087,15 +1090,15 @@ boost::optional calcLongitudinalOffsetPoint( return {}; } -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); @@ -1109,7 +1112,7 @@ calcLongitudinalOffsetPoint -boost::optional calcLongitudinalOffsetPoint( +std::optional calcLongitudinalOffsetPoint( const T & points, const geometry_msgs::msg::Point & src_point, const double offset) { try { @@ -1132,15 +1135,15 @@ boost::optional calcLongitudinalOffsetPoint( return calcLongitudinalOffsetPoint(points, src_seg_idx, offset + signed_length_src_offset); } -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); @@ -1156,7 +1159,7 @@ calcLongitudinalOffsetPoint -boost::optional calcLongitudinalOffsetPose( +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) { @@ -1228,17 +1231,17 @@ boost::optional calcLongitudinalOffsetPose( return {}; } -extern template boost::optional +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 boost::optional +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 boost::optional +extern template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const size_t src_idx, const double offset, @@ -1255,7 +1258,7 @@ calcLongitudinalOffsetPose -boost::optional calcLongitudinalOffsetPose( +std::optional calcLongitudinalOffsetPose( const T & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true) { @@ -1275,17 +1278,17 @@ boost::optional calcLongitudinalOffsetPose( set_orientation_from_position_direction); } -extern template boost::optional +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 boost::optional +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 boost::optional +extern template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, @@ -1301,7 +1304,7 @@ calcLongitudinalOffsetPose -boost::optional insertTargetPoint( +std::optional insertTargetPoint( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points, const double overlap_threshold = 1e-3) { @@ -1339,7 +1342,7 @@ boost::optional insertTargetPoint( geometry_msgs::msg::Pose target_pose; { - const auto p_base = is_driving_forward.get() ? p_back : p_front; + 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); @@ -1352,7 +1355,7 @@ boost::optional insertTargetPoint( geometry_msgs::msg::Pose base_pose; { - const auto p_base = is_driving_forward.get() ? p_front : p_back; + 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); @@ -1361,7 +1364,7 @@ boost::optional insertTargetPoint( } if (!overlap_with_front && !overlap_with_back) { - if (is_driving_forward.get()) { + 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)); @@ -1377,17 +1380,17 @@ boost::optional insertTargetPoint( return seg_idx; } -extern template boost::optional +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 boost::optional +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 boost::optional +extern template std::optional insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, @@ -1404,18 +1407,18 @@ insertTargetPoint * @return index of segment id, where point is inserted */ template -boost::optional insertTargetPoint( +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 boost::none; + return std::nullopt; } // Get Nearest segment index - boost::optional segment_idx = boost::none; + 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); @@ -1426,23 +1429,23 @@ boost::optional insertTargetPoint( } if (!segment_idx) { - return boost::none; + return std::nullopt; } return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } -extern template boost::optional +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 boost::optional +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 boost::optional +extern template std::optional insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, @@ -1459,18 +1462,18 @@ insertTargetPoint * @return index of insert point */ template -boost::optional insertTargetPoint( +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 boost::none; + return std::nullopt; } // Get Nearest segment index - boost::optional segment_idx = boost::none; + 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); @@ -1490,7 +1493,7 @@ boost::optional insertTargetPoint( } if (!segment_idx) { - return boost::none; + return std::nullopt; } // Get Target Point @@ -1505,17 +1508,17 @@ boost::optional insertTargetPoint( return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } -extern template boost::optional +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 boost::optional +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 boost::optional +extern template std::optional insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, @@ -1535,7 +1538,7 @@ insertTargetPoint * @return index of insert point */ template -boost::optional insertTargetPoint( +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) @@ -1543,12 +1546,12 @@ boost::optional insertTargetPoint( validateNonEmpty(points); if (insert_point_length < 0.0) { - return boost::none; + return std::nullopt; } const auto nearest_segment_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); if (!nearest_segment_idx) { - return boost::none; + return std::nullopt; } const double offset_length = @@ -1558,19 +1561,19 @@ boost::optional insertTargetPoint( *nearest_segment_idx, insert_point_length + offset_length, points, overlap_threshold); } -extern template boost::optional +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 boost::optional +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 boost::optional +extern template std::optional insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, std::vector & points, @@ -1587,20 +1590,20 @@ insertTargetPoint * @return index of stop point */ template -boost::optional insertStopPoint( +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 boost::none; + return std::nullopt; } const auto stop_idx = insertTargetPoint( src_segment_idx, distance_to_stop_point, points_with_twist, overlap_threshold); if (!stop_idx) { - return boost::none; + return std::nullopt; } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { @@ -1610,17 +1613,17 @@ boost::optional insertStopPoint( return stop_idx; } -extern template boost::optional +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 boost::optional +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 boost::optional +extern template std::optional insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, std::vector & points_with_twist, @@ -1635,13 +1638,13 @@ insertStopPoint>( * @return index of stop point */ template -boost::optional insertStopPoint( +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 boost::none; + return std::nullopt; } double accumulated_length = 0; @@ -1656,20 +1659,20 @@ boost::optional insertStopPoint( accumulated_length += length; } - return boost::none; + return std::nullopt; } -extern template boost::optional +extern template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, @@ -1689,7 +1692,7 @@ insertStopPoint>( * @return index of stop point */ template -boost::optional insertStopPoint( +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) @@ -1697,14 +1700,14 @@ boost::optional insertStopPoint( validateNonEmpty(points_with_twist); if (distance_to_stop_point < 0.0) { - return boost::none; + 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 boost::none; + return std::nullopt; } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { @@ -1714,19 +1717,19 @@ boost::optional insertStopPoint( return stop_idx; } -extern template boost::optional +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 boost::optional +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 boost::optional +extern template std::optional insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, std::vector & points_with_twist, @@ -1743,7 +1746,7 @@ insertStopPoint>( * @return index of stop point */ template -boost::optional insertStopPoint( +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) { @@ -1751,17 +1754,17 @@ boost::optional insertStopPoint( motion_utils::insertTargetPoint(stop_seg_idx, stop_point, points_with_twist, overlap_threshold); if (!insert_idx) { - return boost::none; + return std::nullopt; } - for (size_t i = insert_idx.get(); i < points_with_twist.size(); ++i) { + 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 boost::optional +extern template std::optional insertStopPoint>( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, std::vector & points_with_twist, @@ -1775,7 +1778,7 @@ insertStopPoint>( * @param points_with_twist output points of trajectory, path, ... (with velocity) */ template -boost::optional insertDecelPoint( +std::optional insertDecelPoint( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, T & points_with_twist) { @@ -1786,14 +1789,14 @@ boost::optional insertDecelPoint( return {}; } - const auto seg_idx = findNearestSegmentIndex(points_with_twist, decel_point.get()); - const auto insert_idx = insertTargetPoint(seg_idx, decel_point.get(), points_with_twist); + 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.get(); i < points_with_twist.size(); ++i) { + 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( @@ -1803,7 +1806,7 @@ boost::optional insertDecelPoint( return insert_idx; } -extern template boost::optional +extern template std::optional insertDecelPoint>( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, @@ -2183,7 +2186,7 @@ extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< * longitudinal velocity */ template -boost::optional calcDistanceToForwardStopPoint( +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()) @@ -2199,14 +2202,14 @@ boost::optional calcDistanceToForwardStopPoint( motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); if (!nearest_segment_idx) { - return boost::none; + 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 boost::none; + return std::nullopt; } const auto closest_stop_dist = @@ -2215,17 +2218,17 @@ boost::optional calcDistanceToForwardStopPoint( return std::max(0.0, closest_stop_dist); } -extern template boost::optional +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 boost::optional +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 boost::optional +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(), diff --git a/common/motion_utils/src/distance/distance.cpp b/common/motion_utils/src/distance/distance.cpp index 14ba02ab38703..c6de95dd568cf 100644 --- a/common/motion_utils/src/distance/distance.cpp +++ b/common/motion_utils/src/distance/distance.cpp @@ -91,7 +91,7 @@ std::tuple update( * @param (t_during_min_acc) duration of constant deceleration [s] * @return moving distance until velocity is reached vt [m] */ -boost::optional calcDecelDistPlanType1( +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) { @@ -155,7 +155,7 @@ boost::optional calcDecelDistPlanType1( * @param (jd) minimum jerk [m/sss] * @return moving distance until velocity is reached vt [m] */ -boost::optional calcDecelDistPlanType2( +std::optional calcDecelDistPlanType2( const double v0, const double vt, const double a0, const double ja, const double jd) { constexpr double epsilon = 1e-3; @@ -212,7 +212,7 @@ boost::optional calcDecelDistPlanType2( * @param (ja) maximum jerk [m/sss] * @return moving distance until velocity is reached vt [m] */ -boost::optional calcDecelDistPlanType3( +std::optional calcDecelDistPlanType3( const double v0, const double vt, const double a0, const double ja) { constexpr double epsilon = 1e-3; @@ -234,7 +234,7 @@ boost::optional calcDecelDistPlanType3( } } // namespace -boost::optional calcDecelDistWithJerkAndAccConstraints( +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) { diff --git a/common/motion_utils/src/trajectory/path_with_lane_id.cpp b/common/motion_utils/src/trajectory/path_with_lane_id.cpp index c74db0f61d6fb..c7e85554dbddd 100644 --- a/common/motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/motion_utils/src/trajectory/path_with_lane_id.cpp @@ -24,7 +24,7 @@ namespace motion_utils { -boost::optional> getPathIndexRangeWithLaneId( +std::optional> getPathIndexRangeWithLaneId( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) { size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error diff --git a/common/motion_utils/src/trajectory/tmp_conversion.cpp b/common/motion_utils/src/trajectory/tmp_conversion.cpp index 5c907a5926046..18705637a8387 100644 --- a/common/motion_utils/src/trajectory/tmp_conversion.cpp +++ b/common/motion_utils/src/trajectory/tmp_conversion.cpp @@ -36,12 +36,7 @@ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( const std::vector & trajectory) { autoware_auto_planning_msgs::msg::Trajectory output{}; - for (const auto & pt : trajectory) { - output.points.push_back(pt); - if (output.points.size() >= output.CAPACITY) { - break; - } - } + for (const auto & pt : trajectory) output.points.push_back(pt); return output; } diff --git a/common/motion_utils/src/trajectory/trajectory.cpp b/common/motion_utils/src/trajectory/trajectory.cpp index 12074e537c106..51c07a75076c8 100644 --- a/common/motion_utils/src/trajectory/trajectory.cpp +++ b/common/motion_utils/src/trajectory/trajectory.cpp @@ -26,24 +26,24 @@ template void validateNonEmpty &); // -template boost::optional +template std::optional isDrivingForward>( const std::vector &); -template boost::optional +template std::optional isDrivingForward>( const std::vector &); -template boost::optional +template std::optional isDrivingForward>( const std::vector &); // -template boost::optional +template std::optional isDrivingForwardWithTwist>( const std::vector &); -template boost::optional +template std::optional isDrivingForwardWithTwist>( const std::vector &); -template boost::optional +template std::optional isDrivingForwardWithTwist>( const std::vector &); @@ -61,19 +61,19 @@ removeOverlapPoints +template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist, const size_t src_idx, const size_t dst_idx); // -template boost::optional +template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist, const size_t src_idx); // -template boost::optional +template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist); @@ -90,15 +90,15 @@ template size_t findNearestIndex +template std::optional findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template boost::optional +template std::optional findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template boost::optional +template std::optional findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); @@ -131,15 +131,15 @@ findNearestSegmentIndex +template std::optional findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template boost::optional +template std::optional findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template boost::optional +template std::optional findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); @@ -261,201 +261,201 @@ calcCurvatureAndArcLength & points); // -template boost::optional +template std::optional calcDistanceToForwardStopPoint>( const std::vector & points_with_twist, const size_t src_idx); // -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); // -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); // -template boost::optional +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 boost::optional +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 boost::optional +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 boost::optional +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 boost::optional +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 boost::optional +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 boost::optional +template std::optional insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); // -template boost::optional +template std::optional insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); // -template boost::optional +template std::optional insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, const double overlap_threshold); // -template boost::optional +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 boost::optional +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 boost::optional +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 boost::optional +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 boost::optional +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 boost::optional +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 boost::optional +template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold); -template boost::optional +template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold); -template boost::optional +template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold); // -template boost::optional +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 boost::optional +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 boost::optional +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 boost::optional +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 boost::optional +template std::optional insertDecelPoint>( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, @@ -539,7 +539,7 @@ template size_t findFirstNearestSegmentIndexWithSoftConstraints< const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); // -template boost::optional +template std::optional calcDistanceToForwardStopPoint>( const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); diff --git a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp index 194c49ae3bff8..900a652033f28 100644 --- a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp +++ b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp @@ -124,7 +124,7 @@ bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_durati return false; } - return std::abs(motion_utils::calcSignedArcLength(trajectory_ptr_->points, p, idx.get())) < + return std::abs(motion_utils::calcSignedArcLength(trajectory_ptr_->points, p, idx.value())) < th_arrived_distance_m; } diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 4df1ef0a094f7..a65147a65b12d 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -85,7 +85,7 @@ TEST(path_with_lane_id, getPathIndexRangeWithLaneId) } { const auto res = getPathIndexRangeWithLaneId(points, 4); - EXPECT_EQ(res, boost::none); + EXPECT_EQ(res, std::nullopt); } } @@ -93,7 +93,7 @@ TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { PathWithLaneId points; const auto res = getPathIndexRangeWithLaneId(points, 4); - EXPECT_EQ(res, boost::none); + EXPECT_EQ(res, std::nullopt); } } diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 6237813a8084c..22567b569d0ad 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -803,17 +803,6 @@ TEST(trajectory, convertToTrajectory) const auto traj = convertToTrajectory(traj_input); EXPECT_EQ(traj.points.size(), traj_input.size()); } - - // Clipping check - { - const auto traj_input = generateTestTrajectoryPointArray(10000, 1.0); - const auto traj = convertToTrajectory(traj_input); - EXPECT_EQ(traj.points.size(), traj.CAPACITY); - // Value check - for (size_t i = 0; i < traj.points.size(); ++i) { - EXPECT_EQ(traj.points.at(i), traj_input.at(i)); - } - } } TEST(trajectory, convertToTrajectoryPointArray) @@ -856,7 +845,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromIndex) // Boundary1 (Edge of the input trajectory) { const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 0); - EXPECT_NEAR(dist.get(), 50.0, epsilon); + EXPECT_NEAR(dist.value(), 50.0, epsilon); } // Boundary2 (Edge of the input trajectory) @@ -868,13 +857,13 @@ TEST(trajectory, calcDistanceToForwardStopPointFromIndex) // Boundary3 (On the Stop Point) { const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 50); - EXPECT_NEAR(dist.get(), 0.0, epsilon); + EXPECT_NEAR(dist.value(), 0.0, epsilon); } // Boundary4 (Right before the stop point) { const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 49); - EXPECT_NEAR(dist.get(), 1.0, epsilon); + EXPECT_NEAR(dist.value(), 1.0, epsilon); } // Boundary5 (Right behind the stop point) @@ -886,7 +875,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromIndex) // Random { const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 20); - EXPECT_NEAR(dist.get(), 30.0, epsilon); + EXPECT_NEAR(dist.value(), 30.0, epsilon); } } @@ -914,7 +903,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) { const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 50.0, epsilon); + EXPECT_NEAR(dist.value(), 50.0, epsilon); } // Trajectory Edge2 @@ -928,7 +917,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) { const auto pose = createPose(-10.0, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 60.0, epsilon); + EXPECT_NEAR(dist.value(), 60.0, epsilon); } // Out of Trajectory2 @@ -942,14 +931,14 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) { const auto pose = createPose(-30.0, 50.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 80.0, epsilon); + EXPECT_NEAR(dist.value(), 80.0, epsilon); } // Boundary Condition1 { const auto pose = createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 0.0, epsilon); + EXPECT_NEAR(dist.value(), 0.0, epsilon); } // Boundary Condition2 @@ -963,14 +952,14 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) { const auto pose = createPose(49.9, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 0.1, epsilon); + EXPECT_NEAR(dist.value(), 0.1, epsilon); } // Random { const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 47.0, epsilon); + EXPECT_NEAR(dist.value(), 47.0, epsilon); } } @@ -985,14 +974,14 @@ TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) { const auto pose = createPose(-4.9, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); - EXPECT_NEAR(dist.get(), 54.9, epsilon); + EXPECT_NEAR(dist.value(), 54.9, epsilon); } // Boundary Condition2 { const auto pose = createPose(-5.0, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); - EXPECT_NEAR(dist.get(), 55.0, epsilon); + EXPECT_NEAR(dist.value(), 55.0, epsilon); } // Boundary Condition3 @@ -1007,7 +996,7 @@ TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) { const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); - EXPECT_NEAR(dist.get(), 47.0, epsilon); + EXPECT_NEAR(dist.value(), 47.0, epsilon); } { @@ -1034,14 +1023,14 @@ TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) const auto pose = createPose(x, 0.0, 0.0, 0.0, 0.0, deg2rad(29.9)); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(30.0)); - EXPECT_NEAR(dist.get(), 48.0, epsilon); + EXPECT_NEAR(dist.value(), 48.0, epsilon); } { const auto pose = createPose(x, 0.0, 0.0, 0.0, 0.0, deg2rad(30.0)); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(30.0)); - EXPECT_NEAR(dist.get(), 48.0, epsilon); + EXPECT_NEAR(dist.value(), 48.0, epsilon); } { @@ -1058,7 +1047,7 @@ TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, deg2rad(15.0)); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(20.0)); - EXPECT_NEAR(dist.get(), 47.0, epsilon); + EXPECT_NEAR(dist.value(), 47.0, epsilon); } { @@ -1096,10 +1085,10 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) for (double len = 0.0; len < d_back + epsilon; len += 0.1) { const auto p_out = calcLongitudinalOffsetPoint(traj.points, i, std::min(len, d_back)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); x_ans += 0.1; } @@ -1114,10 +1103,10 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { const auto p_out = calcLongitudinalOffsetPoint(traj.points, i, std::max(len, d_front)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); x_ans -= 0.1; } @@ -1127,14 +1116,14 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) { const auto p_out = calcLongitudinalOffsetPoint(traj.points, 0, total_length + epsilon); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found { const auto p_out = calcLongitudinalOffsetPoint(traj.points, 9, -total_length - epsilon); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found(Trajectory size is 1) @@ -1142,7 +1131,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) const auto one_point_traj = generateTestTrajectory(1, 1.0); const auto p_out = calcLongitudinalOffsetPoint(one_point_traj.points, 0.0, 0.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } } @@ -1171,10 +1160,10 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) for (double len = 0.0; len < d_back + epsilon; len += 0.1) { const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, std::min(len, d_back)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); x_ans += 0.1; } @@ -1191,10 +1180,10 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, std::max(len, d_front)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); x_ans -= 0.1; } @@ -1205,7 +1194,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) const auto p_src = createPoint(0.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, total_length + 1.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found @@ -1213,7 +1202,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) const auto p_src = createPoint(9.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, -total_length - 1.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // Out of range(Trajectory size is 1) @@ -1250,14 +1239,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) for (double len = 0.0; len < d_back + epsilon; len += 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, i, std::min(len, d_back)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); x_ans += 0.1; } @@ -1272,14 +1261,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, i, std::max(len, d_front)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); x_ans -= 0.1; } @@ -1289,14 +1278,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) { const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length + epsilon); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found { const auto p_out = calcLongitudinalOffsetPose(traj.points, 9, -total_length - epsilon); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found(Trajectory size is 1) @@ -1304,7 +1293,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) const auto one_point_traj = generateTestTrajectory(1, 1.0); const auto p_out = calcLongitudinalOffsetPose(one_point_traj.points, 0.0, 0.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } } @@ -1338,14 +1327,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, len * std::cos(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.y, len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Found pose(backward) @@ -1353,14 +1342,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Boundary condition @@ -1368,14 +1357,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Boundary condition @@ -1383,14 +1372,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1427,14 +1416,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, len * std::cos(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.y, len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Found pose(backward) @@ -1445,14 +1434,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Boundary condition @@ -1460,14 +1449,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length, false); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Boundary condition @@ -1475,14 +1464,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0, false); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1511,14 +1500,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) for (double len = 0.0; len < d_back + epsilon; len += 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, std::min(len, d_back)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); x_ans += 0.1; } @@ -1535,14 +1524,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, std::max(len, d_front)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); x_ans -= 0.1; } @@ -1553,7 +1542,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) const auto p_src = createPoint(0.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, total_length + 1.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found @@ -1561,7 +1550,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) const auto p_src = createPoint(9.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, -total_length - 1.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // Out of range(Trajectory size is 1) @@ -1612,16 +1601,16 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, len); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); - EXPECT_NE(p_out, boost::none); + EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR( - p_out.get().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); + p_out.value().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); EXPECT_NEAR( - p_out.get().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + p_out.value().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1635,14 +1624,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1689,16 +1678,16 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); - EXPECT_NE(p_out, boost::none); + EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR( - p_out.get().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); + p_out.value().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); EXPECT_NEAR( - p_out.get().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + p_out.value().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1713,14 +1702,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset, false); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1745,8 +1734,8 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1754,7 +1743,7 @@ TEST(trajectory, insertTargetPoint) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -1783,8 +1772,8 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1792,7 +1781,7 @@ TEST(trajectory, insertTargetPoint) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -1821,8 +1810,8 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1830,7 +1819,7 @@ TEST(trajectory, insertTargetPoint) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -1859,8 +1848,8 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1876,8 +1865,8 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1895,7 +1884,7 @@ TEST(trajectory, insertTargetPoint) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) @@ -1908,7 +1897,7 @@ TEST(trajectory, insertTargetPoint) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) @@ -1921,7 +1910,7 @@ TEST(trajectory, insertTargetPoint) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid base index @@ -1932,7 +1921,7 @@ TEST(trajectory, insertTargetPoint) const auto p_target = createPoint(10.0, 0.0, 0.0); const auto insert_idx = insertTargetPoint(segment_idx, p_target, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Empty @@ -1969,8 +1958,8 @@ TEST(trajectory, insertTargetPoint_Reverse) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1979,7 +1968,7 @@ TEST(trajectory, insertTargetPoint_Reverse) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2024,8 +2013,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2034,7 +2023,7 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2064,8 +2053,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2083,8 +2072,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2115,8 +2104,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2124,7 +2113,7 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2153,8 +2142,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start + 1.1e-3, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2162,7 +2151,7 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2191,8 +2180,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(9.0, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2200,7 +2189,7 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2229,8 +2218,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2238,7 +2227,7 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2267,8 +2256,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start + 1e-4, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2284,8 +2273,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start - 1e-4, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2300,7 +2289,7 @@ TEST(trajectory, insertTargetPoint_Length) const auto p_target = createPoint(-1.0, 0.0, 0.0); const auto insert_idx = insertTargetPoint(-1.0, p_target, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind the end point) @@ -2310,7 +2299,7 @@ TEST(trajectory, insertTargetPoint_Length) const auto p_target = createPoint(10.0, 0.0, 0.0); const auto insert_idx = insertTargetPoint(10.0, p_target, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) @@ -2322,7 +2311,7 @@ TEST(trajectory, insertTargetPoint_Length) const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Empty @@ -2355,8 +2344,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2364,7 +2353,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2393,8 +2382,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start + 1.1e-3, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2402,7 +2391,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2431,8 +2420,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, 9.0, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2440,7 +2429,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2469,8 +2458,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start + 1e-4, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2486,8 +2475,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start - 1e-4, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2501,7 +2490,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const auto insert_idx = insertTargetPoint(0, -1.0, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind the end point) @@ -2510,7 +2499,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const auto insert_idx = insertTargetPoint(0, 10.0, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Empty @@ -2541,8 +2530,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2550,7 +2539,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2581,8 +2570,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2590,7 +2579,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2621,8 +2610,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2630,7 +2619,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2661,8 +2650,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2670,7 +2659,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2701,8 +2690,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2710,7 +2699,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2734,19 +2723,19 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id // No Insert (Index Out of range) { auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(9, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(9, 1.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(10, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(10, 1.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(9, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(9, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(10, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(10, 1.0, traj_out.points), std::nullopt); } // No Insert (Length out of range) { auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(0, 10.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(0, 9.0001, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(5, 5.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(8, 1.0001, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(0, 10.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(0, 9.0001, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(5, 5.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(8, 1.0001, traj_out.points), std::nullopt); } } @@ -2771,8 +2760,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2780,7 +2769,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2811,8 +2800,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2820,7 +2809,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2851,8 +2840,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2860,7 +2849,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2891,8 +2880,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2900,7 +2889,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2924,16 +2913,16 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero // No Insert (Index Out of range) { auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), std::nullopt); } // No Insert (Length out of range) { auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(9, -10.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(9, -9.0001, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(1, -1.0001, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(9, -10.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(9, -9.0001, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(1, -1.0001, traj_out.points), std::nullopt); } } @@ -2959,8 +2948,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2968,7 +2957,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3002,8 +2991,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3011,7 +3000,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3046,8 +3035,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3069,8 +3058,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3078,7 +3067,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3113,8 +3102,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3122,7 +3111,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3151,8 +3140,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const double src_pose_y = 0.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(src_pose, 0.5, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 0.5, traj_out.points), std::nullopt); } // Insert from the point behind the trajectory (Invalid) @@ -3162,9 +3151,9 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points), std::nullopt); } // Insert from the point in front of the trajectory (Boundary Condition) @@ -3179,8 +3168,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3188,7 +3177,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3221,8 +3210,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3230,7 +3219,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3258,8 +3247,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, -1.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(src_pose, -10.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(src_pose, -1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, -10.0, traj_out.points), std::nullopt); } // No Insert (Too Far from the source point) @@ -3269,8 +3258,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points, 1.0), boost::none); - EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points, 1.0), boost::none); + EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points, 1.0), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points, 1.0), std::nullopt); } // No Insert (Too large yaw deviation) @@ -3283,9 +3272,9 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, src_yaw); const double max_dist = std::numeric_limits::max(); EXPECT_EQ( - insertTargetPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), boost::none); + insertTargetPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); EXPECT_EQ( - insertTargetPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), boost::none); + insertTargetPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); } } @@ -3310,13 +3299,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3324,7 +3313,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3355,13 +3344,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3369,7 +3358,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3400,13 +3389,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3414,7 +3403,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3445,13 +3434,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3459,7 +3448,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3490,13 +3479,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3504,7 +3493,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3528,19 +3517,19 @@ TEST(trajectory, insertStopPoint_from_a_source_index) // No Insert (Index Out of range) { auto traj_out = traj; - EXPECT_EQ(insertStopPoint(9, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(9, 1.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(10, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(10, 1.0, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(9, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(9, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(10, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(10, 1.0, traj_out.points), std::nullopt); } // No Insert (Length out of range) { auto traj_out = traj; - EXPECT_EQ(insertStopPoint(0, 10.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(0, 9.0001, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(5, 5.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(8, 1.0001, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(0, 10.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(0, 9.0001, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(5, 5.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(8, 1.0001, traj_out.points), std::nullopt); } } @@ -3564,13 +3553,13 @@ TEST(trajectory, insertStopPoint_from_front_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(x_start + 2.0, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3578,7 +3567,7 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3608,13 +3597,13 @@ TEST(trajectory, insertStopPoint_from_front_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(8.0 + x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3622,7 +3611,7 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3651,13 +3640,13 @@ TEST(trajectory, insertStopPoint_from_front_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(9.0, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3665,7 +3654,7 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3695,13 +3684,13 @@ TEST(trajectory, insertStopPoint_from_front_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3709,7 +3698,7 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3733,11 +3722,11 @@ TEST(trajectory, insertStopPoint_from_front_point) // No Insert (Length out of range) { auto traj_out = traj; - EXPECT_EQ(insertStopPoint(9.0 + 1e-2, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(10.0, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(9.0 + 1e-2, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(10.0, traj_out.points), std::nullopt); EXPECT_EQ( - insertStopPoint(-std::numeric_limits::epsilon(), traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(-3.0, traj_out.points), boost::none); + insertStopPoint(-std::numeric_limits::epsilon(), traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(-3.0, traj_out.points), std::nullopt); } } @@ -3763,13 +3752,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3777,7 +3766,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3811,13 +3800,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3825,7 +3814,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3860,13 +3849,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3888,13 +3877,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3902,7 +3891,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3937,13 +3926,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3951,7 +3940,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3980,8 +3969,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) const double src_pose_y = 0.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, 0.5, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 0.5, traj_out.points), std::nullopt); } // Insert from the point behind the trajectory (Invalid) @@ -3991,9 +3980,9 @@ TEST(trajectory, insertStopPoint_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points), std::nullopt); } // Insert from the point in front of the trajectory (Boundary Condition) @@ -4008,13 +3997,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -4022,7 +4011,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -4055,13 +4044,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -4069,7 +4058,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -4097,8 +4086,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, -1.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, -10.0, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(src_pose, -1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, -10.0, traj_out.points), std::nullopt); } // No Insert (Too Far from the source point) @@ -4108,8 +4097,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, 1.0), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points, 1.0), boost::none); + EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, 1.0), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points, 1.0), std::nullopt); } // No Insert (Too large yaw deviation) @@ -4121,8 +4110,9 @@ TEST(trajectory, insertStopPoint_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, src_yaw); const double max_dist = std::numeric_limits::max(); - EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), boost::none); + EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); + EXPECT_EQ( + insertStopPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); } } @@ -4147,23 +4137,23 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } - for (size_t i = 0; i < insert_idx.get(); ++i) { + for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); } - for (size_t i = insert_idx.get(); i < traj_out.points.size(); ++i) { + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -4192,22 +4182,22 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } - for (size_t i = 0; i < insert_idx.get(); ++i) { + for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); } - for (size_t i = insert_idx.get(); i < traj_out.points.size(); ++i) { + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -4236,22 +4226,22 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } - for (size_t i = 0; i < insert_idx.get(); ++i) { + for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); } - for (size_t i = insert_idx.get(); i < traj_out.points.size(); ++i) { + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -4280,17 +4270,17 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } - for (size_t i = 0; i < insert_idx.get(); ++i) { + for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); } - for (size_t i = insert_idx.get(); i < traj_out.points.size(); ++i) { + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); } } @@ -4303,17 +4293,17 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } - for (size_t i = 0; i < insert_idx.get(); ++i) { + for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); } - for (size_t i = insert_idx.get(); i < traj_out.points.size(); ++i) { + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); } } @@ -4328,7 +4318,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) @@ -4341,7 +4331,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) @@ -4354,7 +4344,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid base index @@ -4365,7 +4355,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const auto p_target = createPoint(10.0, 0.0, 0.0); const auto insert_idx = insertStopPoint(segment_idx, p_target, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Empty @@ -4398,13 +4388,13 @@ TEST(trajectory, insertDecelPoint_from_a_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); } else { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); @@ -4424,13 +4414,13 @@ TEST(trajectory, insertDecelPoint_from_a_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); } else { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); @@ -4450,13 +4440,13 @@ TEST(trajectory, insertDecelPoint_from_a_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); } else { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 6106e3a15f4c7..63cf6305e8158 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -462,7 +462,8 @@ bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose) * pose. */ geometry_msgs::msg::Pose calcOffsetPose( - const geometry_msgs::msg::Pose & p, const double x, const double y, const double z); + const geometry_msgs::msg::Pose & p, const double x, const double y, const double z, + const double yaw = 0.0); /** * @brief Calculate a point by linear interpolation. 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 index 1d72b49a553d1..c00c3c1c364df 100644 --- 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 @@ -19,8 +19,7 @@ #include -#include - +#include #include namespace tier4_autoware_utils @@ -75,7 +74,7 @@ visualization_msgs::msg::Marker createDeletedDefaultMarker( void appendMarkerArray( const visualization_msgs::msg::MarkerArray & additional_marker_array, visualization_msgs::msg::MarkerArray * marker_array, - const boost::optional & current_time = {}); + const std::optional & current_time = {}); } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp index ab3ca18097e54..efb5564bdaa71 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp @@ -17,6 +17,9 @@ #include +#include + +#include #include #include @@ -40,6 +43,21 @@ inline std::string toHexString(const unique_identifier_msgs::msg::UUID & id) } return ss.str(); } + +inline boost::uuids::uuid toBoostUUID(const unique_identifier_msgs::msg::UUID & id) +{ + boost::uuids::uuid boost_uuid{}; + std::copy(id.uuid.begin(), id.uuid.end(), boost_uuid.begin()); + return boost_uuid; +} + +inline unique_identifier_msgs::msg::UUID toUUIDMsg(const boost::uuids::uuid & id) +{ + unique_identifier_msgs::msg::UUID ros_uuid{}; + std::copy(id.begin(), id.end(), ros_uuid.uuid.begin()); + return ros_uuid; +} + } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index e37cd0cc33975..04a07b88edf4e 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -18,7 +18,6 @@ builtin_interfaces diagnostic_msgs geometry_msgs - libboost-dev logging_demo pcl_conversions pcl_ros diff --git a/common/tier4_autoware_utils/src/geometry/geometry.cpp b/common/tier4_autoware_utils/src/geometry/geometry.cpp index 2c2de2d07a3dc..b88646b73dd7c 100644 --- a/common/tier4_autoware_utils/src/geometry/geometry.cpp +++ b/common/tier4_autoware_utils/src/geometry/geometry.cpp @@ -327,12 +327,13 @@ double calcCurvature( * pose. */ geometry_msgs::msg::Pose calcOffsetPose( - const geometry_msgs::msg::Pose & p, const double x, const double y, const double z) + const geometry_msgs::msg::Pose & p, const double x, const double y, const double z, + const double yaw) { geometry_msgs::msg::Pose pose; geometry_msgs::msg::Transform transform; transform.translation = createTranslation(x, y, z); - transform.rotation = createQuaternion(0.0, 0.0, 0.0, 1.0); + transform.rotation = createQuaternionFromYaw(yaw); tf2::Transform tf_pose; tf2::Transform tf_offset; tf2::fromMsg(transform, tf_offset); @@ -378,6 +379,7 @@ std::optional intersect( geometry_msgs::msg::Point intersect_point; intersect_point.x = t * p1.x + (1.0 - t) * p2.x; intersect_point.y = t * p1.y + (1.0 - t) * p2.y; + intersect_point.z = t * p1.z + (1.0 - t) * p2.z; return intersect_point; } diff --git a/common/tier4_autoware_utils/src/ros/marker_helper.cpp b/common/tier4_autoware_utils/src/ros/marker_helper.cpp index fe3a579e41d19..fda997edc83db 100644 --- a/common/tier4_autoware_utils/src/ros/marker_helper.cpp +++ b/common/tier4_autoware_utils/src/ros/marker_helper.cpp @@ -57,13 +57,13 @@ visualization_msgs::msg::Marker createDeletedDefaultMarker( void appendMarkerArray( const visualization_msgs::msg::MarkerArray & additional_marker_array, visualization_msgs::msg::MarkerArray * marker_array, - const boost::optional & current_time) + 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.get(); + marker_array->markers.back().header.stamp = current_time.value(); } } } diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp index ce91614155491..c10e04736b5bb 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp @@ -1138,6 +1138,7 @@ TEST(geometry, calcOffsetPose) 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); @@ -1185,6 +1186,40 @@ TEST(geometry, calcOffsetPose) 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) 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 index 23a5123d8d8e2..11dd050658aeb 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -85,3 +85,24 @@ Control: 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_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp index 3e95afe247c86..e27fc9b6cfa81 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 @@ -107,62 +107,9 @@ void VelocitySteeringFactorsPanel::onVelocityFactors(const VelocityFactorArray:: for (std::size_t i = 0; i < msg->factors.size(); i++) { const auto & e = msg->factors.at(i); - // type + // behavior { - auto label = new QLabel(); - switch (e.type) { - case VelocityFactor::SURROUNDING_OBSTACLE: - label->setText("SURROUNDING_OBSTACLE"); - break; - case VelocityFactor::ROUTE_OBSTACLE: - label->setText("ROUTE_OBSTACLE"); - break; - case VelocityFactor::INTERSECTION: - label->setText("INTERSECTION"); - break; - case VelocityFactor::CROSSWALK: - label->setText("CROSSWALK"); - break; - case VelocityFactor::REAR_CHECK: - label->setText("REAR_CHECK"); - break; - case VelocityFactor::USER_DEFINED_DETECTION_AREA: - label->setText("USER_DEFINED_DETECTION_AREA"); - break; - case VelocityFactor::NO_STOPPING_AREA: - label->setText("NO_STOPPING_AREA"); - break; - case VelocityFactor::STOP_SIGN: - label->setText("STOP_SIGN"); - break; - case VelocityFactor::TRAFFIC_SIGNAL: - label->setText("TRAFFIC_SIGNAL"); - break; - case VelocityFactor::V2I_GATE_CONTROL_ENTER: - label->setText("V2I_GATE_CONTROL_ENTER"); - break; - case VelocityFactor::V2I_GATE_CONTROL_LEAVE: - label->setText("V2I_GATE_CONTROL_LEAVE"); - break; - case VelocityFactor::MERGE: - label->setText("MERGE"); - break; - case VelocityFactor::SIDEWALK: - label->setText("SIDEWALK"); - break; - case VelocityFactor::LANE_CHANGE: - label->setText("LANE_CHANGE"); - break; - case VelocityFactor::AVOIDANCE: - label->setText("AVOIDANCE"); - break; - case VelocityFactor::EMERGENCY_STOP_OPERATION: - label->setText("EMERGENCY_STOP_OPERATION"); - break; - default: - label->setText("UNKNOWN"); - break; - } + auto label = new QLabel(e.behavior.empty() ? "UNKNOWN" : e.behavior.c_str()); label->setAlignment(Qt::AlignCenter); velocity_factors_table_->setCellWidget(i, 0, label); } @@ -213,38 +160,9 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: for (std::size_t i = 0; i < msg->factors.size(); i++) { const auto & e = msg->factors.at(i); - // type + // behavior { - auto label = new QLabel(); - switch (e.type) { - case SteeringFactor::INTERSECTION: - label->setText("INTERSECTION"); - break; - case SteeringFactor::LANE_CHANGE: - label->setText("LANE_CHANGE"); - break; - case SteeringFactor::AVOIDANCE_PATH_CHANGE: - label->setText("AVOIDANCE_PATH_CHANGE"); - break; - case SteeringFactor::AVOIDANCE_PATH_RETURN: - label->setText("AVOIDANCE_PATH_RETURN"); - break; - case SteeringFactor::STATION: - label->setText("STATION"); - break; - case SteeringFactor::START_PLANNER: - label->setText("START_PLANNER"); - break; - case SteeringFactor::GOAL_PLANNER: - label->setText("GOAL_PLANNER"); - break; - case SteeringFactor::EMERGENCY_OPERATION: - label->setText("EMERGENCY_OPERATION"); - break; - default: - label->setText("UNKNOWN"); - break; - } + auto label = new QLabel(e.behavior.empty() ? "UNKNOWN" : e.behavior.c_str()); label->setAlignment(Qt::AlignCenter); steering_factors_table_->setCellWidget(i, 0, label); } diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp index fe867964239af..70f35ed3793ab 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -35,6 +36,7 @@ namespace rviz_plugins { class VelocitySteeringFactorsPanel : public rviz_common::Panel { + using PlanningBehavior = autoware_adapi_v1_msgs::msg::PlanningBehavior; using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray; using VelocityFactor = autoware_adapi_v1_msgs::msg::VelocityFactor; using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray; diff --git a/common/trtexec_vendor/CMakeLists.txt b/common/trtexec_vendor/CMakeLists.txt deleted file mode 100644 index 0e5eebc621e39..0000000000000 --- a/common/trtexec_vendor/CMakeLists.txt +++ /dev/null @@ -1,83 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(trtexec_vendor) - -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(ament_cmake REQUIRED) -find_package(CUDA) -find_package(cudnn_cmake_module REQUIRED) -find_package(CUDNN) -find_package(tensorrt_cmake_module REQUIRED) -find_package(TENSORRT) - -if(NOT (CUDA_FOUND AND CUDNN_FOUND AND TENSORRT_FOUND)) - message(WARNING "cuda, cudnn, tensorrt libraries are not found") - return() -endif() - -if(TENSORRT_VERSION VERSION_LESS 8.2.1) - message(WARNING "The tensorrt version less than 8.2.1 isn't supported.") - return() -endif() - -set(TRTEXEC_DEFAULT_BIN /usr/src/tensorrt/bin/trtexec) -if(NOT EXISTS TRTEXEC_DEFAULT_BIN) - include(FetchContent) - fetchcontent_declare(tensorrt - GIT_REPOSITORY https://github.com/NVIDIA/TensorRT - GIT_TAG release/${TENSORRT_VERSION_MAJOR}.${TENSORRT_VERSION_MINOR} - GIT_SUBMODULES "" - ) - fetchcontent_getproperties(tensorrt) - if(NOT tensorrt_POPULATED) - fetchcontent_populate(tensorrt) - endif() - set(TRTEXEC_SOURCES - ${tensorrt_SOURCE_DIR}/samples/trtexec/trtexec.cpp - ${tensorrt_SOURCE_DIR}/samples/common/sampleEngines.cpp - ${tensorrt_SOURCE_DIR}/samples/common/sampleInference.cpp - ${tensorrt_SOURCE_DIR}/samples/common/sampleOptions.cpp - ${tensorrt_SOURCE_DIR}/samples/common/sampleReporting.cpp - ${tensorrt_SOURCE_DIR}/samples/common/logger.cpp - ) - if(TENSORRT_VERSION VERSION_GREATER_EQUAL 8.4) - list(APPEND TRTEXEC_SOURCES - ${tensorrt_SOURCE_DIR}/samples/common/sampleUtils.cpp - ) - endif() - cuda_add_executable(${PROJECT_NAME} - ${TRTEXEC_SOURCES} - ) - target_link_libraries(${PROJECT_NAME} - ${TENSORRT_LIBRARIES} - ) - target_include_directories(${PROJECT_NAME} - PRIVATE ${tensorrt_SOURCE_DIR}/samples/common - ) - - set_target_properties(${PROJECT_NAME} - PROPERTIES OUTPUT_NAME trtexec - ) - - install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - ) -endif() - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in") - -ament_package() diff --git a/common/trtexec_vendor/README.md b/common/trtexec_vendor/README.md deleted file mode 100644 index aec49936fd99e..0000000000000 --- a/common/trtexec_vendor/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# trtexec_vendor - -## Purpose - -This is a vendor package for trtexec. -It can build tensorrt engine from onnx file with trtexec. - -e.g. - -```bash -trtexec --onnx=(ros2 pkg prefix tensorrt_yolox --share)/data/yolox-tiny.onnx -``` diff --git a/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in b/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in deleted file mode 100644 index 7c2ef456ecfec..0000000000000 --- a/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in +++ /dev/null @@ -1,5 +0,0 @@ -TRTEXEC_DEFAULT_BIN_DIR=/usr/src/tensorrt/bin -TRTEXEC_DEFAULT_BIN=$TRTEXEC_DEFAULT_BIN_DIR/trtexec -if [ -f $TRTEXEC_DEFAULT_BIN ]; then - ament_prepend_unique_value PATH $TRTEXEC_DEFAULT_BIN_DIR -fi diff --git a/common/trtexec_vendor/package.xml b/common/trtexec_vendor/package.xml deleted file mode 100644 index 1fc6c928e3ec5..0000000000000 --- a/common/trtexec_vendor/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - trtexec_vendor - 0.1.0 - The vendor package of trtexec - Daisuke Nishimatsu - Yusuke Muramatsu - Apache 2.0 - - ament_cmake - cudnn_cmake_module - tensorrt_cmake_module - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - 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 275be99f418f1..6bf8fb1c34f5e 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 @@ -33,6 +33,7 @@ #include #include +#include #include #include @@ -79,7 +80,7 @@ class ControlPerformanceAnalysisCore void setOdomHistory(const Odometry & odom); void setSteeringStatus(const SteeringReport & steering); - boost::optional findCurveRefIdx(); + std::optional findCurveRefIdx(); std::pair findClosestPrevWayPointIdx_path_direction(); double estimateCurvature(); double estimatePurePursuitCurvature(); 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 777f5dee3e470..1ceef81e10c56 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -93,8 +94,8 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - idx_prev_wp_ = std::make_unique(closest_segment.get()); - idx_next_wp_ = std::make_unique(closest_segment.get() + 1); + idx_prev_wp_ = std::make_unique(closest_segment.value()); + idx_next_wp_ = std::make_unique(closest_segment.value() + 1); return std::make_pair(true, *idx_prev_wp_); } @@ -360,13 +361,13 @@ void ControlPerformanceAnalysisCore::setSteeringStatus(const SteeringReport & st current_vec_steering_msg_ptr_ = std::make_shared(steering); } -boost::optional ControlPerformanceAnalysisCore::findCurveRefIdx() +std::optional ControlPerformanceAnalysisCore::findCurveRefIdx() { // Get the previous waypoint as the reference if (!interpolated_pose_ptr_) { RCLCPP_WARN_THROTTLE( logger_, clock_, 1000, "Cannot set the curvature_idx, no valid interpolated pose ..."); - return boost::none; + return std::nullopt; } auto fun_distance_cond = [this](auto point_t) { @@ -428,7 +429,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() RCLCPP_WARN(logger_, "Cannot find index of curvature reference waypoint "); return 0; } - const auto idx_curve_ref_wp = idx_curve_ref_wp_optional.get(); + const auto idx_curve_ref_wp = idx_curve_ref_wp_optional.value(); const auto & points = current_trajectory_ptr_->points; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index b40a0d0473135..06d11133920f4 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -53,13 +53,13 @@ std::array triangle2points( return points; } -std::map getRouteLanelets( +std::map getRouteLanelets( const lanelet::LaneletMapPtr & lanelet_map, const lanelet::routing::RoutingGraphPtr & routing_graph, const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr, const double vehicle_length) { - std::map route_lanelets; + std::map route_lanelets; bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr, lanelet_map); if (!is_route_valid) { @@ -315,7 +315,7 @@ void LaneDepartureCheckerNode::onTimer() // In order to wait for both of map and route will be ready, write this not in callback but here if (last_route_ != route_ && !route_->segments.empty()) { - std::map::iterator itr; + std::map::iterator itr; auto map_route_lanelets_ = getRouteLanelets(lanelet_map_, routing_graph_, route_, vehicle_length_m_); diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 50e9d44a0a7a4..6bfba5818bd06 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -41,9 +41,7 @@ For the optimization, a Quadratic Programming (QP) solver is used and two option ### Filtering Filtering is required for good noise reduction. -A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as -input of the MPC as well as for -the output steering angle. +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 diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 6bd885b78beb7..153c2ae61d076 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -204,7 +204,7 @@ void MPC::setReferenceTrajectory( 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.get() : m_is_forward_shift; + m_is_forward_shift = is_forward_shift ? is_forward_shift.value() : m_is_forward_shift; // path smoothing MPCTrajectory mpc_traj_smoothed = mpc_traj_resampled; // smooth filtered trajectory diff --git a/control/obstacle_collision_checker/schema/obstacle_collision_checker.json b/control/obstacle_collision_checker/schema/obstacle_collision_checker.json new file mode 100644 index 0000000000000..d1ba379a732df --- /dev/null +++ b/control/obstacle_collision_checker/schema/obstacle_collision_checker.json @@ -0,0 +1,60 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for obstacle collision checker", + "type": "object", + "definitions": { + "vehicle_cmd_gate": { + "type": "object", + "properties": { + "delay_time": { + "type": "number", + "default": 0.3, + "description": "Delay time of vehicles." + }, + "update_rate": { + "type": "number" + }, + "footprint_margin": { + "type": "number", + "default": 0.0, + "description": "Foot print margin." + }, + "max_deceleration": { + "type": "number", + "default": 2.0, + "description": "Max deceleration for ego vehicle to stop." + }, + "resample_interval": { + "type": "number", + "default": 0.3, + "description": "Interval for resampling trajectory." + }, + "search_radius": { + "type": "number", + "default": 5.0, + "description": "Search distance from trajectory to point cloud" + } + }, + "required": [ + "delay_time", + "footprint_margin", + "max_deceleration", + "resample_interval", + "search_radius", + "update_rate" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/obstacle_collision_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index 3cb07a9bd7821..ea0688e88d9f1 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -83,6 +83,8 @@ For the backward compatibility (to be removed): ## 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 | diff --git a/control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json b/control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json new file mode 100644 index 0000000000000..8ce8eda4c6c1c --- /dev/null +++ b/control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json @@ -0,0 +1,168 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Operation Mode Transition Manager Node", + "type": "object", + "definitions": { + "operation_mode_transition_manager": { + "type": "object", + "properties": { + "transition_timeout": { + "type": "number", + "description": "If the state transition is not completed within this time, it is considered a transition failure.", + "default": "10.0", + "minimum": 0.0 + }, + "frequency_hz": { + "type": "number", + "description": "running hz", + "default": "10.0", + "minimum": 0.0 + }, + "enable_engage_on_driving": { + "type": "boolean", + "description": "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.", + "default": "false" + }, + "check_engage_condition": { + "type": "boolean", + "description": "If false, autonomous transition is always available.", + "default": "false" + }, + "nearest_dist_deviation_threshold": { + "type": "number", + "description": "distance threshold used to find nearest trajectory point [m]", + "default": "3.0", + "minimum": 0.0 + }, + "nearest_yaw_deviation_threshold": { + "type": "number", + "description": "angle threshold used to find nearest trajectory point [rad]", + "default": "1.57", + "minimum": -3.142 + }, + "engage_acceptable_limits": { + "type": "object", + "properties": { + "allow_autonomous_in_stopped": { + "type": "boolean", + "description": "If true, autonomous transition is available when the vehicle is stopped even if other checks fail.", + "default": "true" + }, + "dist_threshold": { + "type": "number", + "description": "The distance between the trajectory and ego vehicle must be within this distance for Autonomous transition.", + "default": "1.5", + "minimum": 0.0 + }, + "yaw_threshold": { + "type": "number", + "description": "The yaw angle between trajectory and ego vehicle must be within this threshold for Autonomous transition.", + "default": "0.524", + "minimum": -3.142 + }, + "speed_upper_threshold": { + "type": "number", + "description": "The velocity deviation between control command and ego vehicle must be within this threshold for Autonomous transition.", + "default": "10.0" + }, + "speed_lower_threshold": { + "type": "number", + "description": "The velocity deviation between control command and ego vehicle must be within this threshold for Autonomous transition.", + "default": "-10.0" + }, + "acc_threshold": { + "type": "number", + "description": "The control command acceleration must be less than this threshold for Autonomous transition.", + "default": "1.5", + "minimum": 0.0 + }, + "lateral_acc_threshold": { + "type": "number", + "description": "The control command lateral acceleration must be less than this threshold for Autonomous transition.", + "default": "1.0", + "minimum": 0.0 + }, + "lateral_acc_diff_threshold": { + "type": "number", + "description": "The lateral acceleration deviation between the control command must be less than this threshold for Autonomous transition.", + "default": "0.5", + "minimum": 0.0 + } + }, + "required": [ + "allow_autonomous_in_stopped", + "dist_threshold", + "yaw_threshold", + "speed_upper_threshold", + "speed_lower_threshold", + "acc_threshold", + "lateral_acc_threshold", + "lateral_acc_diff_threshold" + ] + }, + "stable_check": { + "type": "object", + "properties": { + "duration": { + "type": "number", + "description": "The stable condition must be satisfied for this duration to complete the transition.", + "default": "0.1", + "minimum": 0.0 + }, + "dist_threshold": { + "type": "number", + "description": "The distance between the trajectory and ego vehicle must be within this distance to complete Autonomous transition.", + "default": "1.5", + "minimum": 0.0 + }, + "speed_upper_threshold": { + "type": "number", + "description": "The velocity deviation between control command and ego vehicle must be within this threshold to complete Autonomous transition.", + "default": "2.0" + }, + "speed_lower_threshold": { + "type": "number", + "description": "The velocity deviation between control command and ego vehicle must be within this threshold to complete Autonomous transition.", + "default": "-2.0" + }, + "yaw_threshold": { + "type": "number", + "description": "The yaw angle between trajectory and ego vehicle must be within this threshold to complete Autonomous transition.", + "default": "0,262", + "minimum": -3.142 + } + }, + "required": [ + "duration", + "dist_threshold", + "speed_upper_threshold", + "speed_lower_threshold", + "yaw_threshold" + ] + } + }, + "required": [ + "transition_timeout", + "frequency_hz", + "enable_engage_on_driving", + "check_engage_condition", + "nearest_dist_deviation_threshold", + "nearest_yaw_deviation_threshold", + "engage_acceptable_limits", + "stable_check" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/operation_mode_transition_manager" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 9d25322c793e0..a110cb94d1d91 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -90,8 +90,13 @@ This PID logic has a maximum value for the output of each term. This is to preve - Large integral terms may cause unintended behavior by users. - Unintended noise may cause the output of the derivative term to be very large. -Also, the integral term is not accumulated when the vehicle is stopped. This is to prevent unintended accumulation of the integral term in cases such as Autoware assumes that the vehicle is engaged, but an external system has locked the vehicle to start. -On the other hand, if the vehicle gets stuck in a depression in the road surface when starting, the vehicle will not start forever, which is currently being addressed by developers. +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. @@ -207,7 +212,9 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | max_d_effort | double | max value of acceleration with d gain | 0.0 | | min_d_effort | double | min value of acceleration with d gain | 0.0 | | lpf_vel_error_gain | double | gain of low-pass filter for velocity error | 0.9 | -| 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 | +| 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) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 921cd3995f6a9..b4ac9e1506eb7 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -97,6 +98,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // vehicle info double m_wheel_base; + bool m_prev_vehicle_is_under_control{false}; + std::shared_ptr m_under_control_starting_time{nullptr}; // control state enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY }; @@ -145,7 +148,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // drive PIDController m_pid_vel; std::shared_ptr m_lpf_vel_error{nullptr}; + bool m_enable_integration_at_low_speed; double m_current_vel_threshold_pid_integrate; + double m_time_threshold_before_pid_integrate; bool m_enable_brake_keeping_before_stop; double m_brake_keeping_acc; @@ -384,6 +389,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro void updateDebugVelAcc( const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data); + + double getTimeUnderControl(); }; } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml index eb2ef443c4576..f1ddea7ebad88 100644 --- a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml +++ b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml @@ -32,7 +32,9 @@ max_d_effort: 0.0 min_d_effort: 0.0 lpf_vel_error_gain: 0.9 + enable_integration_at_low_speed: false current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 enable_brake_keeping_before_stop: false brake_keeping_acc: -0.2 diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index d13e628f2e1d4..12f0eece1e477 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -100,9 +100,14 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) const double lpf_vel_error_gain{node.declare_parameter("lpf_vel_error_gain")}; m_lpf_vel_error = std::make_shared(0.0, lpf_vel_error_gain); + m_enable_integration_at_low_speed = + node.declare_parameter("enable_integration_at_low_speed"); m_current_vel_threshold_pid_integrate = node.declare_parameter("current_vel_threshold_pid_integration"); // [m/s] + m_time_threshold_before_pid_integrate = + node.declare_parameter("time_threshold_before_pid_integration"); // [s] + m_enable_brake_keeping_before_stop = node.declare_parameter("enable_brake_keeping_before_stop"); // [-] m_brake_keeping_acc = node.declare_parameter("brake_keeping_acc"); // [m/s^2] @@ -284,6 +289,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); update_param("current_vel_threshold_pid_integration", m_current_vel_threshold_pid_integrate); + update_param("time_threshold_before_pid_integration", m_time_threshold_before_pid_integrate); } // stopping state @@ -556,6 +562,11 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; + if (is_under_control != m_prev_vehicle_is_under_control) { + m_prev_vehicle_is_under_control = is_under_control; + m_under_control_starting_time = + is_under_control ? std::make_shared(clock_->now()) : nullptr; + } // transit state // in DRIVE state if (m_control_state == ControlState::DRIVE) { @@ -959,8 +970,15 @@ double PidLongitudinalController::applyVelocityFeedback( const double diff_vel = (target_motion.vel - current_vel) * vel_sign; const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; + + const bool vehicle_is_moving = std::abs(current_vel) > m_current_vel_threshold_pid_integrate; + const double time_under_control = getTimeUnderControl(); + const bool vehicle_is_stuck = + !vehicle_is_moving && time_under_control > m_time_threshold_before_pid_integrate; + const bool enable_integration = - (std::abs(current_vel) > m_current_vel_threshold_pid_integrate) && is_under_control; + (vehicle_is_moving || (m_enable_integration_at_low_speed && vehicle_is_stuck)) && + is_under_control; const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel); std::vector pid_contributions(3); @@ -1059,4 +1077,10 @@ void PidLongitudinalController::checkControlState( stat.summary(level, msg); } +double PidLongitudinalController::getTimeUnderControl() +{ + if (!m_under_control_starting_time) return 0.0; + return (clock_->now() - *m_under_control_starting_time).seconds(); +} + } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/shift_decider/schema/shift_decider.schema.json b/control/shift_decider/schema/shift_decider.schema.json new file mode 100644 index 0000000000000..faba3c4e12064 --- /dev/null +++ b/control/shift_decider/schema/shift_decider.schema.json @@ -0,0 +1,30 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Shift Decider Node", + "type": "object", + "definitions": { + "shift_decider": { + "type": "object", + "properties": { + "park_on_goal": { + "type": "boolean", + "description": "Setting true to part on goal.", + "default": "true" + } + }, + "required": ["park_on_goal"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/shift_decider" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/trajectory_follower_node/param/longitudinal/pid.param.yaml index bc3213081d86e..c39088753fe64 100644 --- a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -32,7 +32,9 @@ max_d_effort: 0.0 min_d_effort: 0.0 lpf_vel_error_gain: 0.9 + enable_integration_at_low_speed: false current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 enable_brake_keeping_before_stop: false brake_keeping_acc: -0.2 diff --git a/control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json b/control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json new file mode 100644 index 0000000000000..983a001922053 --- /dev/null +++ b/control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json @@ -0,0 +1,104 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for vehicle cmd gate", + "type": "object", + "definitions": { + "vehicle_cmd_gate": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10.0, + "description": "To update period" + }, + "use_emergency_handling": { + "type": "boolean", + "default": "false", + "description": "true when emergency handler is used." + }, + "use_external_emergency_stop": { + "type": "boolean", + "default": "false", + "description": "true when external emergency stop information is used." + }, + "system_emergency_heartbeat_timeout": { + "type": "number", + "default": 0.5, + "description": "timeout for system emergency." + }, + "external_emergency_stop_heartbeat_timeout": { + "type": "number", + "default": 0.5, + "description": "timeout for external emergency." + }, + "stop_hold_acceleration": { + "type": "number", + "default": -1.5, + "description": "longitudinal acceleration cmd when vehicle should stop" + }, + "emergency_acceleration": { + "type": "number", + "default": -2.4, + "description": "longitudinal acceleration cmd when vehicle stop with emergency." + }, + "nominal.vel_lim": { + "type": "number", + "default": 25.0, + "description": "limit of longitudinal velocity (activated in AUTONOMOUS operation mode)." + }, + "nominal.lon_acc_lim": { + "type": "number", + "default": 5.0, + "description": "limit of longitudinal acceleration (activated in AUTONOMOUS operation mode)." + }, + "nominal.lon_jerk_lim": { + "type": "number", + "default": 5.0, + "description": "limit of longitudinal jerk (activated in AUTONOMOUS operation mode)." + }, + "nominal.lat_acc_lim": { + "type": "number", + "default": 5.0, + "description": "limit of lateral acceleration (activated in AUTONOMOUS operation mode)." + }, + "nominal.lat_jerk_lim": { + "type": "number", + "default": 5.0, + "description": "limit of lateral jerk (activated in AUTONOMOUS operation mode)." + }, + "nominal.actual_steer_diff_lim": { + "type": "number", + "default": 1.0, + "description": "limit of actual steer diff (activated in AUTONOMOUS operation mode)." + } + }, + "required": [ + "update_rate", + "use_emergency_handling", + "use_external_emergency_stop", + "system_emergency_heartbeat_timeout", + "external_emergency_stop_heartbeat_timeout", + "stop_hold_acceleration", + "emergency_acceleration", + "nominal.vel_lim", + "nominal.lon_acc_lim", + "nominal.lon_jerk_lim", + "nominal.lat_acc_lim", + "nominal.lat_jerk_lim", + "nominal.actual_steer_diff_lim" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/vehicle_cmd_gate" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index b3e9e9148ee60..227aac50d6d90 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -7,10 +7,8 @@ - - @@ -194,8 +192,9 @@ - - + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 1b4c3f38e038d..65caad54876ff 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -218,8 +218,9 @@ - - + + + 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 b69f1c0ee8b14..0649d8c5d5116 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 @@ -70,11 +70,9 @@ - - + - @@ -120,7 +118,6 @@ - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index ac521934265d7..2c8b7d6581c53 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -60,8 +60,9 @@ - - + + + 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 07b53fb671732..85c6e87c0fd19 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,8 +1,6 @@ - - @@ -19,8 +17,7 @@ - - + @@ -29,15 +26,12 @@ - - + - - diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 6582dfd32d91d..7f5f9d61c4bf0 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -14,6 +14,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 ba2318c438674..3a1f417106f54 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 @@ -16,6 +16,17 @@ + + + + + + + + + + + @@ -32,6 +43,60 @@ + + + + + + + + + + + + + + - - - - - - - - - - + diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 0870a384ea7d3..789cf136f1152 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -5,7 +5,7 @@ 0.1.0 The tier4_system_launch package Fumihito Ito - asana17 + TetsuKawa Apache License 2.0 ament_cmake_auto diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index e2ffff2195645..ba18b7dadd599 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -155,7 +155,6 @@ class EKFLocalizer : public rclcpp::Node const HyperParameters params_; - double ekf_rate_; double ekf_dt_; /* process noise variance for discrete model */ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index 873060c75fcfc..e88a59ffdfab9 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -30,6 +30,7 @@ #include #include +#include struct EKFDiagnosticInfo { @@ -76,12 +77,15 @@ class EKFModule std::array getCurrentPoseCovariance() const; std::array getCurrentTwistCovariance() const; + 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( - const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr, + const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info); bool measurementUpdateTwist( - const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr, + const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info); geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay( const PoseWithCovariance & pose, const double delay_time); @@ -91,6 +95,7 @@ class EKFModule std::shared_ptr warning_; const int dim_x_; + std::vector accumulated_delay_times_; const HyperParameters params_; }; diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index f6c664eb26451..e1eafdc6f7948 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -17,10 +17,9 @@ #include -std::string poseDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt); +std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold); std::string twistDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt); + 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); diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 5dd78a00f2fbd..a3d2f52a4058c 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -44,7 +44,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti : rclcpp::Node(node_name, node_options), warning_(std::make_shared(this)), params_(this), - ekf_rate_(params_.ekf_rate), ekf_dt_(params_.ekf_dt), pose_queue_(params_.pose_smoothing_steps), twist_queue_(params_.twist_smoothing_steps) @@ -109,9 +108,22 @@ void EKFLocalizer::updatePredictFrequency() if (get_clock()->now() < *last_predict_time_) { warning_->warn("Detected jump back in time"); } else { - ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds(); - DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_); - ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); + /* Measure dt */ + ekf_dt_ = (get_clock()->now() - *last_predict_time_).seconds(); + DEBUG_INFO( + get_logger(), "[EKF] update ekf_dt_ to %f seconds (= %f hz)", ekf_dt_, 1 / ekf_dt_); + + if (ekf_dt_ > 10.0) { + 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) { + RCLCPP_WARN( + get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_); + } + + /* Register dt and accumulate time delay */ + ekf_module_->accumulate_delay_time(ekf_dt_); /* Update discrete proc_cov*/ proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0); @@ -165,7 +177,7 @@ void EKFLocalizer::timerCallback() 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, ekf_dt_, t_curr, pose_diag_info_); + bool is_updated = ekf_module_->measurementUpdatePose(*pose, t_curr, pose_diag_info_); if (is_updated) { pose_is_updated = true; @@ -200,8 +212,7 @@ void EKFLocalizer::timerCallback() 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, ekf_dt_, t_curr, twist_diag_info_); + bool is_updated = ekf_module_->measurementUpdateTwist(*twist, t_curr, twist_diag_info_); if (is_updated) { twist_is_updated = true; } diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 0a5e3c98c96c8..573472fe2dabf 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -38,6 +38,7 @@ 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); @@ -131,6 +132,48 @@ double EKFModule::getYawBias() const return kalman_filter_.getLatestX()(IDX::YAWB); } +size_t EKFModule::find_closest_delay_time_index(double target_value) const +{ + // If target_value is too large, return last index + 1 + if (target_value > accumulated_delay_times_.back()) { + return accumulated_delay_times_.size(); + } + + auto lower = std::lower_bound( + accumulated_delay_times_.begin(), accumulated_delay_times_.end(), target_value); + + // If the lower bound is the first element, return its index. + // If the lower bound is beyond the last element, return the last index. + // If else, take the closest element. + if (lower == accumulated_delay_times_.begin()) { + return 0; + } else 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); + } +} + +void EKFModule::accumulate_delay_time(const double dt) +{ + // Shift the delay times to the right. + std::copy_backward( + accumulated_delay_times_.begin(), accumulated_delay_times_.end() - 1, + accumulated_delay_times_.end()); + + // Add a new element (=0) and, and add delay time to the previous elements. + accumulated_delay_times_.front() = 0.0; + for (size_t i = 1; i < accumulated_delay_times_.size(); ++i) { + accumulated_delay_times_[i] += dt; + } +} + void EKFModule::predictWithDelay(const double dt) { const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); @@ -147,8 +190,7 @@ void EKFModule::predictWithDelay(const double dt) } bool EKFModule::measurementUpdatePose( - const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr, - EKFDiagnosticInfo & pose_diag_info) + const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info) { if (pose.header.frame_id != params_.pose_frame_id) { warning_->warnThrottle( @@ -170,14 +212,15 @@ bool EKFModule::measurementUpdatePose( delay_time = std::max(delay_time, 0.0); - const int delay_step = std::roundf(delay_time / dt); + const int delay_step = static_cast(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 = params_.extend_state_step * dt; + 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(delay_time, params_.extend_state_step, dt), 2000); + poseDelayStepWarningMessage(pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), + 2000); return false; } @@ -243,7 +286,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDela } bool EKFModule::measurementUpdateTwist( - const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr, + const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info) { if (twist.header.frame_id != "base_link") { @@ -262,14 +305,16 @@ bool EKFModule::measurementUpdateTwist( } delay_time = std::max(delay_time, 0.0); - const int delay_step = std::roundf(delay_time / dt); + const int delay_step = static_cast(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 = params_.extend_state_step * dt; + 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(delay_time, params_.extend_state_step, dt), 2000); + twistDelayStepWarningMessage( + twist_diag_info.delay_time, twist_diag_info.delay_time_threshold), + 2000); return false; } diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index 9a7bbf47a94eb..c69f30b2d6601 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -18,22 +18,20 @@ #include -std::string poseDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt) +std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) { const std::string s = "Pose delay exceeds the compensation limit, ignored. " - "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; - return fmt::format(s, delay_time, extend_state_step * ekf_dt); + "delay: {:.3f}[s], limit: {:.3f}[s]"; + return fmt::format(s, delay_time, delay_time_threshold); } -std::string twistDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt) +std::string twistDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) { const std::string s = "Twist delay exceeds the compensation limit, ignored. " - "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; - return fmt::format(s, delay_time, extend_state_step * ekf_dt); + "delay: {:.3f}[s], limit: {:.3f}[s]"; + return fmt::format(s, delay_time, delay_time_threshold); } std::string poseDelayTimeWarningMessage(const double delay_time) diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index b4a05afa844dc..2069969e0ae5e 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -21,17 +21,17 @@ TEST(PoseDelayStepWarningMessage, SmokeTest) { EXPECT_STREQ( - poseDelayStepWarningMessage(6.0, 2, 2.0).c_str(), + poseDelayStepWarningMessage(6.0, 4.0).c_str(), "Pose delay exceeds the compensation limit, ignored. " - "delay: 6.000[s], limit = extend_state_step * ekf_dt : 4.000[s]"); + "delay: 6.000[s], limit: 4.000[s]"); } TEST(TwistDelayStepWarningMessage, SmokeTest) { EXPECT_STREQ( - twistDelayStepWarningMessage(10.0, 3, 2.0).c_str(), + twistDelayStepWarningMessage(10.0, 6.0).c_str(), "Twist delay exceeds the compensation limit, ignored. " - "delay: 10.000[s], limit = extend_state_step * ekf_dt : 6.000[s]"); + "delay: 10.000[s], limit: 6.000[s]"); } TEST(PoseDelayTimeWarningMessage, SmokeTest) diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md index e4c79b7941b12..86f3750ad11d9 100644 --- a/localization/landmark_based_localizer/README.md +++ b/localization/landmark_based_localizer/README.md @@ -105,3 +105,20 @@ For example, when using the AR tag, it would look like this. ... ``` + +## About `consider_orientation` + +The `calculate_new_self_pose` function in the `LandmarkManager` class includes a boolean argument named `consider_orientation`. This argument determines the method used to calculate the new self pose based on detected and mapped landmarks. The following image illustrates the difference between the two methods. + +![consider_orientation_figure](./doc_image/consider_orientation.drawio.svg) + +### `consider_orientation = true` + +In this mode, the new self pose is calculated so that the relative Pose of the "landmark detected from the current self pose" is equal to the relative Pose of the "landmark mapped from the new self pose". +This method can correct for orientation, but is strongly affected by the orientation error of the landmark detection. + +### `consider_orientation = false` + +In this mode, the new self pose is calculated so that only the relative position is correct for x, y, and z. + +This method can not correct for orientation, but it is not affected by the orientation error of the landmark detection. diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml index 29260e27cd43c..e8f90165376ce 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml @@ -18,6 +18,9 @@ # Detect AR-Tags within this range and publish the pose of ego vehicle distance_threshold: 13.0 # [m] + # consider_orientation + consider_orientation: false + # Detector parameters # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126 detection_mode: "DM_NORMAL" # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST] diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index a66277c699a00..e569a71bbb5b0 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -49,17 +49,21 @@ #include #include #include +#include #include #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif +#include + ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) : Node("ar_tag_based_localizer", options), cam_info_received_(false) { @@ -73,7 +77,8 @@ bool ArTagBasedLocalizer::setup() marker_size_ = static_cast(this->declare_parameter("marker_size")); target_tag_ids_ = this->declare_parameter>("target_tag_ids"); base_covariance_ = this->declare_parameter>("base_covariance"); - distance_threshold_squared_ = std::pow(this->declare_parameter("distance_threshold"), 2); + distance_threshold_ = this->declare_parameter("distance_threshold"); + consider_orientation_ = this->declare_parameter("consider_orientation"); ekf_time_tolerance_ = this->declare_parameter("ekf_time_tolerance"); ekf_position_tolerance_ = this->declare_parameter("ekf_position_tolerance"); std::string detection_mode = this->declare_parameter("detection_mode"); @@ -89,6 +94,8 @@ bool ArTagBasedLocalizer::setup() RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode); return false; } + ekf_pose_buffer_ = std::make_unique( + this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_); /* Log parameter info @@ -103,7 +110,6 @@ bool ArTagBasedLocalizer::setup() */ tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_unique(*tf_buffer_); - tf_broadcaster_ = std::make_unique(this); /* Initialize image transport @@ -119,6 +125,7 @@ bool ArTagBasedLocalizer::setup() rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_sub.best_effort(); + pose_array_pub_ = this->create_publisher("~/debug/detected_landmarks", qos_sub); image_sub_ = this->create_subscription( "~/input/image", qos_sub, std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1)); @@ -148,83 +155,85 @@ bool ArTagBasedLocalizer::setup() void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) { - const std::vector landmarks = - landmark_manager::parse_landmarks(msg, "apriltag_16h5", this->get_logger()); - for (const landmark_manager::Landmark & landmark : landmarks) { - landmark_map_[landmark.id] = landmark.pose; - } - - const MarkerArray marker_msg = landmark_manager::convert_landmarks_to_marker_array_msg(landmarks); + landmark_manager_.parse_landmarks(msg, "apriltag_16h5", this->get_logger()); + const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); marker_pub_->publish(marker_msg); } void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) { + // check subscribers if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers"); return; } + // check cam_info if (!cam_info_received_) { RCLCPP_DEBUG(this->get_logger(), "No cam_info has been received."); return; } - const builtin_interfaces::msg::Time curr_stamp = msg->header.stamp; - cv_bridge::CvImagePtr cv_ptr; - try { - cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8); - } catch (cv_bridge::Exception & e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + const builtin_interfaces::msg::Time sensor_stamp = msg->header.stamp; + + // get self pose + const std::optional interpolate_result = + ekf_pose_buffer_->interpolate(sensor_stamp); + if (!interpolate_result) { return; } + ekf_pose_buffer_->pop_old(sensor_stamp); + const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose; - cv::Mat in_image = cv_ptr->image; - - // detection results will go into "markers" - std::vector markers; - - // ok, let's detect - detector_.detect(in_image, markers, cam_param_, marker_size_, false); - - // for each marker, draw info and its boundaries in the image - for (const aruco::Marker & marker : markers) { - const tf2::Transform tf_cam_to_marker = aruco_marker_to_tf2(marker); + // detect + const std::vector landmarks = detect_landmarks(msg); + if (landmarks.empty()) { + return; + } - TransformStamped tf_cam_to_marker_stamped; - tf2::toMsg(tf_cam_to_marker, tf_cam_to_marker_stamped.transform); - tf_cam_to_marker_stamped.header.stamp = curr_stamp; - tf_cam_to_marker_stamped.header.frame_id = msg->header.frame_id; - tf_cam_to_marker_stamped.child_frame_id = "detected_marker_" + std::to_string(marker.id); - tf_broadcaster_->sendTransform(tf_cam_to_marker_stamped); + // for debug + if (pose_array_pub_->get_subscription_count() > 0) { + PoseArray pose_array_msg; + pose_array_msg.header.stamp = sensor_stamp; + pose_array_msg.header.frame_id = "map"; + for (const landmark_manager::Landmark & landmark : landmarks) { + const Pose detected_marker_on_map = + tier4_autoware_utils::transformPose(landmark.pose, self_pose); + pose_array_msg.poses.push_back(detected_marker_on_map); + } + pose_array_pub_->publish(pose_array_msg); + } - PoseStamped pose_cam_to_marker; - tf2::toMsg(tf_cam_to_marker, pose_cam_to_marker.pose); - pose_cam_to_marker.header.stamp = curr_stamp; - pose_cam_to_marker.header.frame_id = msg->header.frame_id; - publish_pose_as_base_link(pose_cam_to_marker, std::to_string(marker.id)); + // 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 double distance = + std::hypot(diff_pose.position.x, diff_pose.position.y, diff_pose.position.z); - // drawing the detected markers - marker.draw(in_image, cv::Scalar(0, 0, 255), 2); + // check distance + if (distance > ekf_position_tolerance_) { + RCLCPP_INFO_STREAM(this->get_logger(), "distance: " << distance); + return; } - // draw a 3d cube in each marker if there is 3d info - if (cam_param_.isValid()) { - for (aruco::Marker & marker : markers) { - aruco::CvDrawingUtils::draw3dAxis(in_image, marker, cam_param_); - } - } + // publish + PoseWithCovarianceStamped pose_with_covariance_stamped; + pose_with_covariance_stamped.header.stamp = sensor_stamp; + pose_with_covariance_stamped.header.frame_id = "map"; + pose_with_covariance_stamped.pose.pose = new_self_pose; - if (image_pub_.getNumSubscribers() > 0) { - // show input with augmented information - cv_bridge::CvImage out_msg; - out_msg.header.stamp = curr_stamp; - out_msg.encoding = sensor_msgs::image_encodings::RGB8; - out_msg.image = in_image; - image_pub_.publish(out_msg.toImageMsg()); + // ~5[m]: base_covariance + // 5~[m]: scaling base_covariance by std::pow(distance / 5, 3) + const double coeff = std::max(1.0, std::pow(distance / 5, 3)); + for (int i = 0; i < 36; i++) { + pose_with_covariance_stamped.pose.covariance[i] = coeff * base_covariance_[i]; } - const int detected_tags = static_cast(markers.size()); + pose_pub_->publish(pose_with_covariance_stamped); + + // publish diagnostics + const int detected_tags = static_cast(landmarks.size()); diagnostic_msgs::msg::DiagnosticStatus diag_status; @@ -252,150 +261,113 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) } // wait for one camera info, then shut down that subscriber -void ArTagBasedLocalizer::cam_info_callback(const CameraInfo & msg) +void ArTagBasedLocalizer::cam_info_callback(const CameraInfo::ConstSharedPtr & msg) { if (cam_info_received_) { return; } cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); - camera_matrix.at(0, 0) = msg.p[0]; - camera_matrix.at(0, 1) = msg.p[1]; - camera_matrix.at(0, 2) = msg.p[2]; - camera_matrix.at(0, 3) = msg.p[3]; - camera_matrix.at(1, 0) = msg.p[4]; - camera_matrix.at(1, 1) = msg.p[5]; - camera_matrix.at(1, 2) = msg.p[6]; - camera_matrix.at(1, 3) = msg.p[7]; - camera_matrix.at(2, 0) = msg.p[8]; - camera_matrix.at(2, 1) = msg.p[9]; - camera_matrix.at(2, 2) = msg.p[10]; - camera_matrix.at(2, 3) = msg.p[11]; + camera_matrix.at(0, 0) = msg->p[0]; + camera_matrix.at(0, 1) = msg->p[1]; + camera_matrix.at(0, 2) = msg->p[2]; + camera_matrix.at(0, 3) = msg->p[3]; + camera_matrix.at(1, 0) = msg->p[4]; + camera_matrix.at(1, 1) = msg->p[5]; + camera_matrix.at(1, 2) = msg->p[6]; + camera_matrix.at(1, 3) = msg->p[7]; + camera_matrix.at(2, 0) = msg->p[8]; + camera_matrix.at(2, 1) = msg->p[9]; + camera_matrix.at(2, 2) = msg->p[10]; + camera_matrix.at(2, 3) = msg->p[11]; cv::Mat distortion_coeff(4, 1, CV_64FC1); for (int i = 0; i < 4; ++i) { distortion_coeff.at(i, 0) = 0; } - const cv::Size size(static_cast(msg.width), static_cast(msg.height)); + const cv::Size size(static_cast(msg->width), static_cast(msg->height)); cam_param_ = aruco::CameraParameters(camera_matrix, distortion_coeff, size); cam_info_received_ = true; } -void ArTagBasedLocalizer::ekf_pose_callback(const PoseWithCovarianceStamped & msg) +void ArTagBasedLocalizer::ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg) { - latest_ekf_pose_ = msg; + if (msg->header.frame_id == "map") { + ekf_pose_buffer_->push_back(msg); + } else { + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), *this->get_clock(), 1000, + "Received initial pose message with frame_id " + << msg->header.frame_id << ", but expected map. " + << "Please check the frame_id in the input topic and ensure it is correct."); + } } -void ArTagBasedLocalizer::publish_pose_as_base_link( - const PoseStamped & sensor_to_tag, const std::string & tag_id) +std::vector ArTagBasedLocalizer::detect_landmarks( + const Image::ConstSharedPtr & msg) { - // Check tag_id - if (std::find(target_tag_ids_.begin(), target_tag_ids_.end(), tag_id) == target_tag_ids_.end()) { - RCLCPP_INFO_STREAM(this->get_logger(), "tag_id(" << tag_id << ") is not in target_tag_ids"); - return; - } - if (landmark_map_.count(tag_id) == 0) { - RCLCPP_INFO_STREAM(this->get_logger(), "tag_id(" << tag_id << ") is not in landmark_map_"); - return; - } + const builtin_interfaces::msg::Time sensor_stamp = msg->header.stamp; - // Range filter - const double distance_squared = sensor_to_tag.pose.position.x * sensor_to_tag.pose.position.x + - sensor_to_tag.pose.position.y * sensor_to_tag.pose.position.y + - sensor_to_tag.pose.position.z * sensor_to_tag.pose.position.z; - if (distance_threshold_squared_ < distance_squared) { - return; + // get image + cv::Mat in_image; + try { + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8); + cv_ptr->image.copyTo(in_image); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return std::vector{}; } - // Transform to base_link - PoseStamped base_link_to_tag; + // get transform from base_link to camera + TransformStamped transform_sensor_to_base_link; try { - const TransformStamped transform = - tf_buffer_->lookupTransform("base_link", sensor_to_tag.header.frame_id, tf2::TimePointZero); - tf2::doTransform(sensor_to_tag, base_link_to_tag, transform); - base_link_to_tag.header.frame_id = "base_link"; + transform_sensor_to_base_link = + tf_buffer_->lookupTransform("base_link", msg->header.frame_id, sensor_stamp); } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); - return; + return std::vector{}; } - // (1) map_to_tag - const Pose & map_to_tag = landmark_map_.at(tag_id); - const Eigen::Affine3d map_to_tag_affine = pose_to_affine3d(map_to_tag); - - // (2) tag_to_base_link - const Eigen::Affine3d base_link_to_tag_affine = pose_to_affine3d(base_link_to_tag.pose); - const Eigen::Affine3d tag_to_base_link_affine = base_link_to_tag_affine.inverse(); - - // calculate map_to_base_link - const Eigen::Affine3d map_to_base_link_affine = map_to_tag_affine * tag_to_base_link_affine; - const Pose map_to_base_link = tf2::toMsg(map_to_base_link_affine); - - // If latest_ekf_pose_ is older than seconds compared to current frame, it - // will not be published. - const rclcpp::Duration diff_time = - rclcpp::Time(sensor_to_tag.header.stamp) - rclcpp::Time(latest_ekf_pose_.header.stamp); - if (diff_time.seconds() > ekf_time_tolerance_) { - RCLCPP_INFO( - this->get_logger(), - "latest_ekf_pose_ is older than %f seconds compared to current frame. " - "latest_ekf_pose_.header.stamp: %d.%d, sensor_to_tag.header.stamp: %d.%d", - ekf_time_tolerance_, latest_ekf_pose_.header.stamp.sec, latest_ekf_pose_.header.stamp.nanosec, - sensor_to_tag.header.stamp.sec, sensor_to_tag.header.stamp.nanosec); - return; - } - - // If curr_pose differs from latest_ekf_pose_ by more than , it will not - // be published. - const Pose curr_pose = map_to_base_link; - const Pose latest_ekf_pose = latest_ekf_pose_.pose.pose; - const double diff_position = norm(curr_pose.position, latest_ekf_pose.position); - if (diff_position > ekf_position_tolerance_) { - RCLCPP_INFO( - this->get_logger(), - "curr_pose differs from latest_ekf_pose_ by more than %f m. " - "curr_pose: (%f, %f, %f), latest_ekf_pose: (%f, %f, %f)", - ekf_position_tolerance_, curr_pose.position.x, curr_pose.position.y, curr_pose.position.z, - latest_ekf_pose.position.x, latest_ekf_pose.position.y, latest_ekf_pose.position.z); - return; - } + // detect + std::vector markers; + detector_.detect(in_image, markers, cam_param_, marker_size_, false); - // Construct output message - PoseWithCovarianceStamped pose_with_covariance_stamped; - pose_with_covariance_stamped.header.stamp = sensor_to_tag.header.stamp; - pose_with_covariance_stamped.header.frame_id = "map"; - pose_with_covariance_stamped.pose.pose = curr_pose; + // parse + std::vector landmarks; + + for (aruco::Marker & marker : markers) { + // convert marker pose to tf + const cv::Quat q = cv::Quat::createFromRvec(marker.Rvec); + Pose pose; + pose.position.x = marker.Tvec.at(0, 0); + pose.position.y = marker.Tvec.at(1, 0); + pose.position.z = marker.Tvec.at(2, 0); + pose.orientation.x = q.x; + pose.orientation.y = q.y; + pose.orientation.z = q.z; + pose.orientation.w = q.w; + const double distance = std::hypot(pose.position.x, pose.position.y, pose.position.z); + if (distance <= distance_threshold_) { + tf2::doTransform(pose, pose, transform_sensor_to_base_link); + landmarks.push_back(landmark_manager::Landmark{std::to_string(marker.id), pose}); + } - // ~5[m]: base_covariance - // 5~[m]: scaling base_covariance by std::pow(distance/5, 3) - const double distance = std::sqrt(distance_squared); - const double scale = distance / 5; - const double coeff = std::max(1.0, std::pow(scale, 3)); - for (int i = 0; i < 36; i++) { - pose_with_covariance_stamped.pose.covariance[i] = coeff * base_covariance_[i]; + // for debug, drawing the detected markers + marker.draw(in_image, cv::Scalar(0, 0, 255), 2); + aruco::CvDrawingUtils::draw3dAxis(in_image, marker, cam_param_); } - pose_pub_->publish(pose_with_covariance_stamped); -} - -tf2::Transform ArTagBasedLocalizer::aruco_marker_to_tf2(const aruco::Marker & marker) -{ - cv::Mat rot(3, 3, CV_64FC1); - cv::Mat r_vec64; - marker.Rvec.convertTo(r_vec64, CV_64FC1); - cv::Rodrigues(r_vec64, rot); - cv::Mat tran64; - marker.Tvec.convertTo(tran64, CV_64FC1); - - tf2::Matrix3x3 tf_rot( - rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), rot.at(1, 0), - rot.at(1, 1), rot.at(1, 2), rot.at(2, 0), rot.at(2, 1), - rot.at(2, 2)); - - tf2::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); + // for debug + if (image_pub_.getNumSubscribers() > 0) { + cv_bridge::CvImage out_msg; + out_msg.header.stamp = sensor_stamp; + out_msg.encoding = sensor_msgs::image_encodings::RGB8; + out_msg.image = in_image; + image_pub_.publish(out_msg.toImageMsg()); + } - return tf2::Transform(tf_rot, tf_orig); + return landmarks; } diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index 889e76eb78ad2..b7dfb45a26ce3 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -46,11 +46,13 @@ #define AR_TAG_BASED_LOCALIZER_HPP_ #include "landmark_manager/landmark_manager.hpp" +#include "localization_util/smart_pose_buffer.hpp" #include #include #include +#include #include #include @@ -59,6 +61,7 @@ #include #include +#include #include #include #include @@ -72,6 +75,7 @@ class ArTagBasedLocalizer : public rclcpp::Node using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using PoseArray = geometry_msgs::msg::PoseArray; using TransformStamped = geometry_msgs::msg::TransformStamped; using MarkerArray = visualization_msgs::msg::MarkerArray; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; @@ -83,23 +87,22 @@ class ArTagBasedLocalizer : public rclcpp::Node private: void map_bin_callback(const HADMapBin::ConstSharedPtr & msg); void image_callback(const Image::ConstSharedPtr & msg); - void cam_info_callback(const CameraInfo & msg); - void ekf_pose_callback(const PoseWithCovarianceStamped & msg); - void publish_pose_as_base_link(const PoseStamped & sensor_to_tag, const std::string & tag_id); - static tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker); + void cam_info_callback(const CameraInfo::ConstSharedPtr & msg); + void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg); + std::vector detect_landmarks(const Image::ConstSharedPtr & msg); // Parameters float marker_size_{}; std::vector target_tag_ids_; std::vector base_covariance_; - double distance_threshold_squared_{}; + double distance_threshold_{}; + bool consider_orientation_{}; double ekf_time_tolerance_{}; double ekf_position_tolerance_{}; // tf std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; - std::unique_ptr tf_broadcaster_; // image transport std::unique_ptr it_; @@ -112,6 +115,7 @@ class ArTagBasedLocalizer : public rclcpp::Node // Publishers rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Publisher::SharedPtr pose_array_pub_; image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; rclcpp::Publisher::SharedPtr diag_pub_; @@ -120,8 +124,8 @@ class ArTagBasedLocalizer : public rclcpp::Node aruco::MarkerDetector detector_; aruco::CameraParameters cam_param_; bool cam_info_received_; - PoseWithCovarianceStamped latest_ekf_pose_{}; - std::map landmark_map_; + std::unique_ptr ekf_pose_buffer_; + landmark_manager::LandmarkManager landmark_manager_; }; #endif // AR_TAG_BASED_LOCALIZER_HPP_ diff --git a/localization/landmark_based_localizer/doc_image/consider_orientation.drawio.svg b/localization/landmark_based_localizer/doc_image/consider_orientation.drawio.svg new file mode 100644 index 0000000000000..4e548d08623ae --- /dev/null +++ b/localization/landmark_based_localizer/doc_image/consider_orientation.drawio.svg @@ -0,0 +1,76 @@ + + + + + + + Self pose + + + + + + relative pose + + + + Detected + Landmark + + + + Mapped + Landmark + + + + New self pose + + + + + + relative pose + + + + consider_orientation = true + + + + + Self pose + + + + Detected + Landmark + + + + Mapped + Landmark + + + + New self pose + + + + consider_orientation = false + + + + + + + diff --git a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp index 9a22ee13f60ae..fba419f746b5e 100644 --- a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp +++ b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -34,12 +35,25 @@ struct Landmark geometry_msgs::msg::Pose pose; }; -std::vector parse_landmarks( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, - const std::string & target_subtype, const rclcpp::Logger & logger); - -visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg( - const std::vector & landmarks); +class LandmarkManager +{ +public: + void parse_landmarks( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const std::string & target_subtype, const rclcpp::Logger & logger); + + [[nodiscard]] visualization_msgs::msg::MarkerArray get_landmarks_as_marker_array_msg() const; + + [[nodiscard]] geometry_msgs::msg::Pose calculate_new_self_pose( + const std::vector & detected_landmarks, + const geometry_msgs::msg::Pose & self_pose, const bool consider_orientation) const; + +private: + // To allow multiple landmarks with the same id to be registered on a vector_map, + // manage vectors by having them in a std::map. + // landmarks_map_[""] = [pose0, pose1, ...] + std::map> landmarks_map_; +}; } // namespace landmark_manager diff --git a/localization/landmark_based_localizer/landmark_manager/package.xml b/localization/landmark_based_localizer/landmark_manager/package.xml index 1a35ae6a338d1..2e0e252386d1f 100644 --- a/localization/landmark_based_localizer/landmark_manager/package.xml +++ b/localization/landmark_based_localizer/landmark_manager/package.xml @@ -18,6 +18,7 @@ autoware_auto_mapping_msgs geometry_msgs lanelet2_extension + localization_util rclcpp tf2_eigen diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index 6d1698daf5eae..875f04edd8c47 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -15,6 +15,8 @@ #include "landmark_manager/landmark_manager.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" +#include "localization_util/util_func.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -25,7 +27,7 @@ namespace landmark_manager { -std::vector parse_landmarks( +void LandmarkManager::parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, const std::string & target_subtype, const rclcpp::Logger & logger) { @@ -36,8 +38,6 @@ std::vector parse_landmarks( lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); - std::vector landmarks; - for (const auto & poly : lanelet_map_ptr->polygonLayer) { const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; if (type != "pose_marker") { @@ -48,8 +48,8 @@ std::vector parse_landmarks( continue; } - // Get marker_id - const std::string marker_id = poly.attributeOr("marker_id", "none"); + // Get landmark_id + const std::string landmark_id = poly.attributeOr("marker_id", "none"); // Get 4 vertices const auto & vertices = poly.basicPolygon(); @@ -58,7 +58,7 @@ std::vector parse_landmarks( continue; } - // 4 vertices represent the marker vertices in counterclockwise order + // 4 vertices represent the landmark vertices in counterclockwise order // Calculate the volume by considering it as a tetrahedron const auto & v0 = vertices[0]; const auto & v1 = vertices[1]; @@ -98,8 +98,8 @@ std::vector parse_landmarks( pose.orientation.w = q.w(); // Add - landmarks.push_back(Landmark{marker_id, pose}); - RCLCPP_INFO_STREAM(logger, "id: " << marker_id); + landmarks_map_[landmark_id].push_back(pose); + RCLCPP_INFO_STREAM(logger, "id: " << landmark_id); RCLCPP_INFO_STREAM( logger, "(x, y, z) = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z); @@ -107,54 +107,117 @@ std::vector parse_landmarks( logger, "q = " << pose.orientation.x << ", " << pose.orientation.y << ", " << pose.orientation.z << ", " << pose.orientation.w); } - - return landmarks; } -visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg( - const std::vector & landmarks) +visualization_msgs::msg::MarkerArray LandmarkManager::get_landmarks_as_marker_array_msg() const { int32_t id = 0; visualization_msgs::msg::MarkerArray marker_array; - for (const auto & [id_str, pose] : landmarks) { - // publish cube as a thin board - visualization_msgs::msg::Marker cube_marker; - cube_marker.header.frame_id = "map"; - cube_marker.header.stamp = rclcpp::Clock().now(); - cube_marker.ns = "landmark_cube"; - cube_marker.id = id; - cube_marker.type = visualization_msgs::msg::Marker::CUBE; - cube_marker.action = visualization_msgs::msg::Marker::ADD; - cube_marker.pose = pose; - cube_marker.scale.x = 1.0; - cube_marker.scale.y = 2.0; - cube_marker.scale.z = 0.1; - cube_marker.color.a = 0.5; - cube_marker.color.r = 0.0; - cube_marker.color.g = 1.0; - cube_marker.color.b = 0.0; - marker_array.markers.push_back(cube_marker); - - // publish text - visualization_msgs::msg::Marker text_marker; - text_marker.header.frame_id = "map"; - text_marker.header.stamp = rclcpp::Clock().now(); - text_marker.ns = "landmark_text"; - text_marker.id = id; - text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - text_marker.action = visualization_msgs::msg::Marker::ADD; - text_marker.pose = pose; - text_marker.text = "(" + id_str + ")"; - text_marker.scale.z = 0.5; - text_marker.color.a = 1.0; - text_marker.color.r = 1.0; - text_marker.color.g = 0.0; - text_marker.color.b = 0.0; - marker_array.markers.push_back(text_marker); - - id++; + for (const auto & [landmark_id_str, landmark_poses] : landmarks_map_) { + for (const auto & pose : landmark_poses) { + // publish cube as a thin board + visualization_msgs::msg::Marker cube_marker; + cube_marker.header.frame_id = "map"; + cube_marker.header.stamp = rclcpp::Clock().now(); + cube_marker.ns = "landmark_cube"; + cube_marker.id = id; + cube_marker.type = visualization_msgs::msg::Marker::CUBE; + cube_marker.action = visualization_msgs::msg::Marker::ADD; + cube_marker.pose = pose; + cube_marker.scale.x = 1.0; + cube_marker.scale.y = 2.0; + cube_marker.scale.z = 0.1; + cube_marker.color.a = 0.5; + cube_marker.color.r = 0.0; + cube_marker.color.g = 1.0; + cube_marker.color.b = 0.0; + marker_array.markers.push_back(cube_marker); + + // publish text + visualization_msgs::msg::Marker text_marker; + text_marker.header.frame_id = "map"; + text_marker.header.stamp = rclcpp::Clock().now(); + text_marker.ns = "landmark_text"; + text_marker.id = id; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.action = visualization_msgs::msg::Marker::ADD; + text_marker.pose = pose; + text_marker.text = "(" + landmark_id_str + ")"; + text_marker.scale.z = 0.5; + text_marker.color.a = 1.0; + text_marker.color.r = 1.0; + text_marker.color.g = 0.0; + text_marker.color.b = 0.0; + marker_array.markers.push_back(text_marker); + + id++; + } } return marker_array; } +geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( + const std::vector & detected_landmarks, + const geometry_msgs::msg::Pose & self_pose, const bool consider_orientation) const +{ + using Pose = geometry_msgs::msg::Pose; + + Pose min_new_self_pose; + double min_distance = std::numeric_limits::max(); + + for (const landmark_manager::Landmark & landmark : detected_landmarks) { + // Firstly, landmark pose is base_link + const Pose & detected_landmark_on_base_link = landmark.pose; + + // convert base_link to map + const Pose detected_landmark_on_map = + tier4_autoware_utils::transformPose(detected_landmark_on_base_link, self_pose); + + // match to map + if (landmarks_map_.count(landmark.id) == 0) { + continue; + } + + // 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( + mapped_landmark_on_map.position, detected_landmark_on_map.position); + if (curr_distance > min_distance) { + continue; + } + + if (consider_orientation) { + const Eigen::Affine3d landmark_pose = pose_to_affine3d(mapped_landmark_on_map); + const Eigen::Affine3d landmark_to_base_link = + pose_to_affine3d(detected_landmark_on_base_link).inverse(); + const Eigen::Affine3d new_self_pose_eigen = landmark_pose * landmark_to_base_link; + + const Pose new_self_pose = matrix4f_to_pose(new_self_pose_eigen.matrix().cast()); + + // update + min_distance = curr_distance; + min_new_self_pose = new_self_pose; + } else { + const double diff_x = + mapped_landmark_on_map.position.x - detected_landmark_on_map.position.x; + const double diff_y = + mapped_landmark_on_map.position.y - detected_landmark_on_map.position.y; + const double diff_z = + mapped_landmark_on_map.position.z - detected_landmark_on_map.position.z; + Pose new_self_pose = self_pose; + new_self_pose.position.x += diff_x; + new_self_pose.position.y += diff_y; + new_self_pose.position.z += diff_z; + + // update + min_distance = curr_distance; + min_new_self_pose = new_self_pose; + } + } + } + + return min_new_self_pose; +} + } // namespace landmark_manager diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt index 9490ffb67723b..ade446020d101 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/localization_util/CMakeLists.txt @@ -6,10 +6,18 @@ autoware_package() ament_auto_add_library(localization_util SHARED src/util_func.cpp - src/pose_array_interpolator.cpp src/tf2_listener_module.cpp + src/smart_pose_buffer.cpp ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_smart_pose_buffer + test/test_smart_pose_buffer.cpp + src/smart_pose_buffer.cpp + ) +endif() + ament_auto_package( INSTALL_TO_SHARE ) diff --git a/localization/localization_util/include/localization_util/pose_array_interpolator.hpp b/localization/localization_util/include/localization_util/smart_pose_buffer.hpp similarity index 55% rename from localization/localization_util/include/localization_util/pose_array_interpolator.hpp rename to localization/localization_util/include/localization_util/smart_pose_buffer.hpp index 998d8465733f5..2a2a775a9280c 100644 --- a/localization/localization_util/include/localization_util/pose_array_interpolator.hpp +++ b/localization/localization_util/include/localization_util/smart_pose_buffer.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ -#define LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ +#ifndef LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ +#define LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ #include "localization_util/util_func.hpp" @@ -23,33 +23,39 @@ #include -class PoseArrayInterpolator +class SmartPoseBuffer { private: using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; public: - PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time & target_ros_time, - const std::deque & pose_msg_ptr_array, - const double & pose_timeout_sec, const double & pose_distance_tolerance_meters); + struct InterpolateResult + { + PoseWithCovarianceStamped old_pose; + PoseWithCovarianceStamped new_pose; + PoseWithCovarianceStamped interpolated_pose; + }; - PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time & target_ros_time, - const std::deque & pose_msg_ptr_array); + SmartPoseBuffer() = delete; + SmartPoseBuffer( + const rclcpp::Logger & logger, const double & pose_timeout_sec, + const double & pose_distance_tolerance_meters); - PoseWithCovarianceStamped get_current_pose(); - PoseWithCovarianceStamped get_old_pose(); - PoseWithCovarianceStamped get_new_pose(); - [[nodiscard]] bool is_success() const; + std::optional interpolate(const rclcpp::Time & target_ros_time); + + void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr); + + void pop_old(const rclcpp::Time & target_ros_time); + + void clear(); private: rclcpp::Logger logger_; - rclcpp::Clock clock_; - const PoseWithCovarianceStamped::SharedPtr current_pose_ptr_; - PoseWithCovarianceStamped::SharedPtr old_pose_ptr_; - PoseWithCovarianceStamped::SharedPtr new_pose_ptr_; - bool success_; + std::deque pose_buffer_; + std::mutex mutex_; // This mutex is for pose_buffer_ + + const double pose_timeout_sec_; + const double pose_distance_tolerance_meters_; [[nodiscard]] bool validate_time_stamp_difference( const rclcpp::Time & target_time, const rclcpp::Time & reference_time, @@ -59,4 +65,4 @@ class PoseArrayInterpolator const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; }; -#endif // LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ +#endif // LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ diff --git a/localization/localization_util/include/localization_util/util_func.hpp b/localization/localization_util/include/localization_util/util_func.hpp index bd9a2b9305e25..0b31333da44d3 100644 --- a/localization/localization_util/include/localization_util/util_func.hpp +++ b/localization/localization_util/include/localization_util/util_func.hpp @@ -46,16 +46,14 @@ geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose); geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose); geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); +geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( + const double r_rad, const double p_rad, const double y_rad); +geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( + const double r_deg, const double p_deg, const double y_deg); + geometry_msgs::msg::Twist calc_twist( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b); -void get_nearest_timestamp_pose( - const std::deque & - pose_cov_msg_ptr_array, - const rclcpp::Time & time_stamp, - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_old_pose_cov_msg_ptr, - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_new_pose_cov_msg_ptr); - geometry_msgs::msg::PoseStamped interpolate_pose( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, const rclcpp::Time & time_stamp); @@ -64,19 +62,11 @@ geometry_msgs::msg::PoseStamped interpolate_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp); -void pop_old_pose( - std::deque & - pose_cov_msg_ptr_array, - const rclcpp::Time & time_stamp); - Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose); Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose); geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix); Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos); -std::vector create_random_pose_array( - const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num); - template T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform) { @@ -88,7 +78,7 @@ T transform(const T & input, const geometry_msgs::msg::TransformStamped & transf double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); void output_pose_with_cov_to_log( - const rclcpp::Logger logger, const std::string & prefix, + const rclcpp::Logger & logger, const std::string & prefix, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); #endif // LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/localization_util/src/pose_array_interpolator.cpp b/localization/localization_util/src/pose_array_interpolator.cpp deleted file mode 100644 index ebc904fcf8d38..0000000000000 --- a/localization/localization_util/src/pose_array_interpolator.cpp +++ /dev/null @@ -1,109 +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 "localization_util/pose_array_interpolator.hpp" - -PoseArrayInterpolator::PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time & target_ros_time, - const std::deque & pose_msg_ptr_array) -: logger_(node->get_logger()), - clock_(*node->get_clock()), - current_pose_ptr_(new PoseWithCovarianceStamped), - old_pose_ptr_(new PoseWithCovarianceStamped), - new_pose_ptr_(new PoseWithCovarianceStamped), - success_(true) -{ - get_nearest_timestamp_pose(pose_msg_ptr_array, target_ros_time, old_pose_ptr_, new_pose_ptr_); - const geometry_msgs::msg::PoseStamped interpolated_pose_msg = - interpolate_pose(*old_pose_ptr_, *new_pose_ptr_, target_ros_time); - current_pose_ptr_->header = interpolated_pose_msg.header; - current_pose_ptr_->pose.pose = interpolated_pose_msg.pose; - current_pose_ptr_->pose.covariance = old_pose_ptr_->pose.covariance; -} - -PoseArrayInterpolator::PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time & target_ros_time, - const std::deque & pose_msg_ptr_array, - const double & pose_timeout_sec, const double & pose_distance_tolerance_meters) -: PoseArrayInterpolator(node, target_ros_time, pose_msg_ptr_array) -{ - // check the time stamp - bool is_old_pose_valid = - validate_time_stamp_difference(old_pose_ptr_->header.stamp, target_ros_time, pose_timeout_sec); - bool is_new_pose_valid = - validate_time_stamp_difference(new_pose_ptr_->header.stamp, target_ros_time, pose_timeout_sec); - - // check the position jumping (ex. immediately after the initial pose estimation) - bool is_pose_diff_valid = validate_position_difference( - old_pose_ptr_->pose.pose.position, new_pose_ptr_->pose.pose.position, - pose_distance_tolerance_meters); - - // all validations must be true - if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { - success_ = false; - RCLCPP_WARN(logger_, "Validation error."); - } -} - -geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_current_pose() -{ - return *current_pose_ptr_; -} - -geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_old_pose() -{ - return *old_pose_ptr_; -} - -geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_new_pose() -{ - return *new_pose_ptr_; -} - -bool PoseArrayInterpolator::is_success() const -{ - return success_; -} - -bool PoseArrayInterpolator::validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const -{ - const double dt = std::abs((target_time - reference_time).seconds()); - bool success = dt < time_tolerance_sec; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " - "difference is %lf[sec] (the tolerance is %lf[sec]).", - reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); - } - return success; -} - -bool PoseArrayInterpolator::validate_position_difference( - const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, - const double distance_tolerance_m_) const -{ - double distance = norm(target_point, reference_point); - bool success = distance < distance_tolerance_m_; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The distance from reference position to target position is %lf[m] (the " - "tolerance is %lf[m]).", - distance, distance_tolerance_m_); - } - return success; -} diff --git a/localization/localization_util/src/smart_pose_buffer.cpp b/localization/localization_util/src/smart_pose_buffer.cpp new file mode 100644 index 0000000000000..ba293109dcc4d --- /dev/null +++ b/localization/localization_util/src/smart_pose_buffer.cpp @@ -0,0 +1,148 @@ +// 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 "localization_util/smart_pose_buffer.hpp" + +SmartPoseBuffer::SmartPoseBuffer( + const rclcpp::Logger & logger, const double & pose_timeout_sec, + const double & pose_distance_tolerance_meters) +: logger_(logger), + pose_timeout_sec_(pose_timeout_sec), + pose_distance_tolerance_meters_(pose_distance_tolerance_meters) +{ +} + +std::optional SmartPoseBuffer::interpolate( + const rclcpp::Time & target_ros_time) +{ + InterpolateResult result; + + { + std::lock_guard lock(mutex_); + + if (pose_buffer_.size() < 2) { + RCLCPP_INFO(logger_, "pose_buffer_.size() < 2"); + return std::nullopt; + } + + const rclcpp::Time time_first = pose_buffer_.front()->header.stamp; + const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; + + if (target_ros_time < time_first || time_last < target_ros_time) { + return std::nullopt; + } + + // get the nearest poses + result.old_pose = *pose_buffer_.front(); + for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) { + result.new_pose = *pose_cov_msg_ptr; + const rclcpp::Time pose_time_stamp = result.new_pose.header.stamp; + if (pose_time_stamp > target_ros_time) { + break; + } + result.old_pose = *pose_cov_msg_ptr; + } + } + + // check the time stamp + const bool is_old_pose_valid = validate_time_stamp_difference( + result.old_pose.header.stamp, target_ros_time, pose_timeout_sec_); + const bool is_new_pose_valid = validate_time_stamp_difference( + result.new_pose.header.stamp, target_ros_time, pose_timeout_sec_); + + // check the position jumping (ex. immediately after the initial pose estimation) + const bool is_pose_diff_valid = validate_position_difference( + result.old_pose.pose.pose.position, result.new_pose.pose.pose.position, + pose_distance_tolerance_meters_); + + // all validations must be true + if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { + return std::nullopt; + } + + // interpolate the pose + const geometry_msgs::msg::PoseStamped interpolated_pose_msg = + interpolate_pose(result.old_pose, result.new_pose, target_ros_time); + result.interpolated_pose.header = interpolated_pose_msg.header; + result.interpolated_pose.pose.pose = interpolated_pose_msg.pose; + // This does not interpolate the covariance. + // interpolated_pose.pose.covariance is always set to old_pose.covariance + result.interpolated_pose.pose.covariance = result.old_pose.pose.covariance; + return result; +} + +void SmartPoseBuffer::push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr) +{ + std::lock_guard lock(mutex_); + if (!pose_buffer_.empty()) { + // Check for non-chronological timestamp order + // This situation can arise when replaying a rosbag multiple times + const rclcpp::Time end_time = pose_buffer_.back()->header.stamp; + const rclcpp::Time msg_time = pose_msg_ptr->header.stamp; + if (msg_time < end_time) { + // Clear the buffer if timestamps are reversed to maintain chronological order + pose_buffer_.clear(); + } + } + pose_buffer_.push_back(pose_msg_ptr); +} + +void SmartPoseBuffer::pop_old(const rclcpp::Time & target_ros_time) +{ + std::lock_guard lock(mutex_); + while (!pose_buffer_.empty()) { + if (rclcpp::Time(pose_buffer_.front()->header.stamp) >= target_ros_time) { + break; + } + pose_buffer_.pop_front(); + } +} + +void SmartPoseBuffer::clear() +{ + std::lock_guard lock(mutex_); + pose_buffer_.clear(); +} + +bool SmartPoseBuffer::validate_time_stamp_difference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec) const +{ + const double dt = std::abs((target_time - reference_time).seconds()); + const bool success = dt < time_tolerance_sec; + if (!success) { + RCLCPP_WARN( + logger_, + "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " + "difference is %lf[sec] (the tolerance is %lf[sec]).", + reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); + } + return success; +} + +bool SmartPoseBuffer::validate_position_difference( + const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, + const double distance_tolerance_m_) const +{ + const double distance = norm(target_point, reference_point); + const bool success = distance < distance_tolerance_m_; + if (!success) { + RCLCPP_WARN( + logger_, + "Validation error. The distance from reference position to target position is %lf[m] (the " + "tolerance is %lf[m]).", + distance, distance_tolerance_m_); + } + return success; +} diff --git a/localization/localization_util/src/util_func.cpp b/localization/localization_util/src/util_func.cpp index 865cc02cff293..bb32741067e65 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -16,8 +16,6 @@ #include "localization_util/matrix_type.hpp" -static std::random_device seed_gen; - // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x) { @@ -82,6 +80,28 @@ geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovariance return get_rpy(pose.pose.pose); } +geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( + const double r_rad, const double p_rad, const double y_rad) +{ + tf2::Quaternion q; + q.setRPY(r_rad, p_rad, y_rad); + geometry_msgs::msg::Quaternion quaternion_msg; + quaternion_msg.x = q.x(); + quaternion_msg.y = q.y(); + quaternion_msg.z = q.z(); + quaternion_msg.w = q.w(); + return quaternion_msg; +} + +geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( + const double r_deg, const double p_deg, const double y_deg) +{ + const double r_rad = r_deg * M_PI / 180.0; + const double p_rad = p_deg * M_PI / 180.0; + const double y_rad = y_deg * M_PI / 180.0; + return rpy_rad_to_quaternion(r_rad, p_rad, y_rad); +} + geometry_msgs::msg::Twist calc_twist( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) { @@ -116,29 +136,6 @@ geometry_msgs::msg::Twist calc_twist( return twist; } -void get_nearest_timestamp_pose( - const std::deque & - pose_cov_msg_ptr_array, - const rclcpp::Time & time_stamp, - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_old_pose_cov_msg_ptr, - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_new_pose_cov_msg_ptr) -{ - for (const auto & pose_cov_msg_ptr : pose_cov_msg_ptr_array) { - output_new_pose_cov_msg_ptr = - std::const_pointer_cast(pose_cov_msg_ptr); - const rclcpp::Time pose_time_stamp = output_new_pose_cov_msg_ptr->header.stamp; - if (pose_time_stamp > time_stamp) { - // TODO(Tier IV): refactor - const rclcpp::Time old_pose_time_stamp = output_old_pose_cov_msg_ptr->header.stamp; - if (old_pose_time_stamp.seconds() == 0.0) { - output_old_pose_cov_msg_ptr = output_new_pose_cov_msg_ptr; - } - break; - } - output_old_pose_cov_msg_ptr = output_new_pose_cov_msg_ptr; - } -} - geometry_msgs::msg::PoseStamped interpolate_pose( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, const rclcpp::Time & time_stamp) @@ -193,19 +190,6 @@ geometry_msgs::msg::PoseStamped interpolate_pose( return interpolate_pose(tmp_pose_a, tmp_pose_b, time_stamp); } -void pop_old_pose( - std::deque & - pose_cov_msg_ptr_array, - const rclcpp::Time & time_stamp) -{ - while (!pose_cov_msg_ptr_array.empty()) { - if (rclcpp::Time(pose_cov_msg_ptr_array.front()->header.stamp) >= time_stamp) { - break; - } - pose_cov_msg_ptr_array.pop_front(); - } -} - Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose) { Eigen::Affine3d eigen_pose; @@ -237,49 +221,6 @@ geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_mat return ros_pose; } -std::vector create_random_pose_array( - const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num) -{ - std::default_random_engine engine(seed_gen()); - const Eigen::Map covariance = - make_eigen_covariance(base_pose_with_cov.pose.covariance); - - std::normal_distribution<> x_distribution(0.0, std::sqrt(covariance(0, 0))); - std::normal_distribution<> y_distribution(0.0, std::sqrt(covariance(1, 1))); - std::normal_distribution<> z_distribution(0.0, std::sqrt(covariance(2, 2))); - std::normal_distribution<> roll_distribution(0.0, std::sqrt(covariance(3, 3))); - std::normal_distribution<> pitch_distribution(0.0, std::sqrt(covariance(4, 4))); - std::normal_distribution<> yaw_distribution(0.0, std::sqrt(covariance(5, 5))); - - const auto base_rpy = get_rpy(base_pose_with_cov); - - std::vector poses; - for (int i = 0; i < particle_num; ++i) { - geometry_msgs::msg::Vector3 xyz; - geometry_msgs::msg::Vector3 rpy; - - xyz.x = base_pose_with_cov.pose.pose.position.x + x_distribution(engine); - xyz.y = base_pose_with_cov.pose.pose.position.y + y_distribution(engine); - xyz.z = base_pose_with_cov.pose.pose.position.z + z_distribution(engine); - rpy.x = base_rpy.x + roll_distribution(engine); - rpy.y = base_rpy.y + pitch_distribution(engine); - rpy.z = base_rpy.z + yaw_distribution(engine); - - tf2::Quaternion tf_quaternion; - tf_quaternion.setRPY(rpy.x, rpy.y, rpy.z); - - geometry_msgs::msg::Pose pose; - pose.position.x = xyz.x; - pose.position.y = xyz.y; - pose.position.z = xyz.z; - pose.orientation = tf2::toMsg(tf_quaternion); - - poses.push_back(pose); - } - - return poses; -} - double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) { const double dx = p1.x - p2.x; @@ -289,7 +230,7 @@ double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Poin } void output_pose_with_cov_to_log( - const rclcpp::Logger logger, const std::string & prefix, + const rclcpp::Logger & logger, const std::string & prefix, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov) { const Eigen::Map covariance = diff --git a/localization/localization_util/test/test_smart_pose_buffer.cpp b/localization/localization_util/test/test_smart_pose_buffer.cpp new file mode 100644 index 0000000000000..2520e458f2d33 --- /dev/null +++ b/localization/localization_util/test/test_smart_pose_buffer.cpp @@ -0,0 +1,114 @@ +// 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 "localization_util/smart_pose_buffer.hpp" +#include "localization_util/util_func.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + +bool compare_pose( + const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b) +{ + return pose_a.header.stamp == pose_b.header.stamp && + pose_a.header.frame_id == pose_b.header.frame_id && + pose_a.pose.covariance == pose_b.pose.covariance && + pose_a.pose.pose.position.x == pose_b.pose.pose.position.x && + pose_a.pose.pose.position.y == pose_b.pose.pose.position.y && + pose_a.pose.pose.position.z == pose_b.pose.pose.position.z && + pose_a.pose.pose.orientation.x == pose_b.pose.pose.orientation.x && + pose_a.pose.pose.orientation.y == pose_b.pose.pose.orientation.y && + pose_a.pose.pose.orientation.z == pose_b.pose.pose.orientation.z && + 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 +{ + rclcpp::Logger logger = rclcpp::get_logger("test_logger"); + const double pose_timeout_sec = 10.0; + const double pose_distance_tolerance_meters = 100.0; + SmartPoseBuffer smart_pose_buffer(logger, pose_timeout_sec, pose_distance_tolerance_meters); + + // first data + PoseWithCovarianceStamped::SharedPtr old_pose_ptr = std::make_shared(); + old_pose_ptr->header.stamp.sec = 10; + old_pose_ptr->header.stamp.nanosec = 0; + old_pose_ptr->pose.pose.position.x = 10.0; + old_pose_ptr->pose.pose.position.y = 20.0; + old_pose_ptr->pose.pose.position.z = 0.0; + old_pose_ptr->pose.pose.orientation = rpy_deg_to_quaternion(0.0, 0.0, 0.0); + smart_pose_buffer.push_back(old_pose_ptr); + + // second data + PoseWithCovarianceStamped::SharedPtr new_pose_ptr = std::make_shared(); + new_pose_ptr->header.stamp.sec = 20; + new_pose_ptr->header.stamp.nanosec = 0; + new_pose_ptr->pose.pose.position.x = 20.0; + new_pose_ptr->pose.pose.position.y = 40.0; + new_pose_ptr->pose.pose.position.z = 0.0; + new_pose_ptr->pose.pose.orientation = rpy_deg_to_quaternion(0.0, 0.0, 90.0); + smart_pose_buffer.push_back(new_pose_ptr); + + // interpolate + builtin_interfaces::msg::Time target_ros_time_msg; + target_ros_time_msg.sec = 15; + target_ros_time_msg.nanosec = 0; + const std::optional & interpolate_result = + smart_pose_buffer.interpolate(target_ros_time_msg); + ASSERT_TRUE(interpolate_result.has_value()); + const SmartPoseBuffer::InterpolateResult result = interpolate_result.value(); + + // check old + EXPECT_TRUE(compare_pose(result.old_pose, *old_pose_ptr)); + + // check new + EXPECT_TRUE(compare_pose(result.new_pose, *new_pose_ptr)); + + // check interpolated + EXPECT_EQ(result.interpolated_pose.header.stamp.sec, 15); + EXPECT_EQ(result.interpolated_pose.header.stamp.nanosec, static_cast(0)); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0); + const auto rpy = get_rpy(result.interpolated_pose.pose.pose); + EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6); + EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6); + EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 4beecc2625fe3..2ad4a47b833d1 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -27,7 +27,7 @@ find_package(PCL REQUIRED COMPONENTS common io registration) include_directories(${PCL_INCLUDE_DIRS}) ament_auto_add_executable(ndt_scan_matcher - src/debug.cpp + src/particle.cpp src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp src/map_module.cpp 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 78052cb8198a3..da0ae27a7808e 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 @@ -17,7 +17,6 @@ #include "localization_util/tf2_listener_module.hpp" #include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/debug.hpp" #include "ndt_scan_matcher/particle.hpp" #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 6f9420f5bc71a..7f4a17a65a7e1 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 @@ -17,6 +17,7 @@ #define FMT_HEADER_ONLY +#include "localization_util/smart_pose_buffer.hpp" #include "localization_util/tf2_listener_module.hpp" #include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" @@ -128,8 +129,6 @@ class NDTScanMatcher : public rclcpp::Node const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time); - std::optional interpolate_regularization_pose( - const rclcpp::Time & sensor_ros_time); void add_regularization_pose(const rclcpp::Time & sensor_ros_time); void publish_diagnostic(); @@ -187,8 +186,8 @@ class NDTScanMatcher : public rclcpp::Node double converged_param_transform_probability_; double converged_param_nearest_voxel_transformation_likelihood_; - int initial_estimate_particles_num_; - int n_startup_trials_; + int64_t initial_estimate_particles_num_; + int64_t n_startup_trials_; double lidar_topic_timeout_sec_; double initial_pose_timeout_sec_; double initial_pose_distance_tolerance_m_; @@ -198,16 +197,12 @@ class NDTScanMatcher : public rclcpp::Node std::vector initial_pose_offset_model_; std::array output_pose_covariance_; - std::deque - initial_pose_msg_ptr_array_; std::mutex ndt_ptr_mtx_; - std::mutex initial_pose_array_mtx_; + std::unique_ptr initial_pose_buffer_; // variables for regularization const bool regularization_enabled_; // whether to use longitudinal regularization - std::deque - regularization_pose_msg_ptr_array_; // queue for storing regularization base poses - std::mutex regularization_mutex_; // mutex for regularization_pose_msg_ptr_array_ + std::unique_ptr regularization_pose_buffer_; bool is_activated_; std::shared_ptr tf2_listener_module_; @@ -222,7 +217,7 @@ class NDTScanMatcher : public rclcpp::Node bool use_dynamic_map_loading_; // The execution time which means probably NDT cannot matches scans properly - int critical_upper_bound_exe_time_ms_; + int64_t critical_upper_bound_exe_time_ms_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp index 740acf335099c..f1c05bfe98551 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp @@ -16,6 +16,9 @@ #define NDT_SCAN_MATCHER__PARTICLE_HPP_ #include +#include + +#include struct Particle { @@ -31,4 +34,8 @@ struct Particle int iteration; }; +void push_debug_markers( + visualization_msgs::msg::MarkerArray & marker_array, const builtin_interfaces::msg::Time & stamp, + const std::string & map_frame_, const Particle & particle, const size_t i); + #endif // NDT_SCAN_MATCHER__PARTICLE_HPP_ 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 1317b8bacf47f..783df34e6c0f8 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -15,7 +15,6 @@ #include "ndt_scan_matcher/ndt_scan_matcher_core.hpp" #include "localization_util/matrix_type.hpp" -#include "localization_util/pose_array_interpolator.hpp" #include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.hpp" #include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" @@ -53,19 +52,6 @@ tier4_debug_msgs::msg::Int32Stamped make_int32_stamped( return tier4_debug_msgs::build().stamp(stamp).data(data); } -std::vector create_initial_pose_offset_model( - const std::vector & x, const std::vector & y) -{ - int size = x.size(); - std::vector initial_pose_offset_model(size); - for (int i = 0; i < size; i++) { - initial_pose_offset_model[i].x() = x[i]; - initial_pose_offset_model[i].y() = y[i]; - } - - return initial_pose_offset_model; -} - Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( const Eigen::Matrix2d & matrix) { @@ -74,9 +60,8 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( const Eigen::Vector2d eigen_vec = eigensolver.eigenvectors().col(0); const double th = std::atan2(eigen_vec.y(), eigen_vec.x()); return Eigen::Rotation2Dd(th).toRotationMatrix(); - } else { - throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } + throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } bool validate_local_optimal_solution_oscillation( @@ -115,14 +100,15 @@ NDTScanMatcher::NDTScanMatcher() state_ptr_(new std::map), inversion_vector_threshold_(-0.9), // Not necessary to extract to ndt_scan_matcher.param.yaml oscillation_threshold_(10), // Not necessary to extract to ndt_scan_matcher.param.yaml + output_pose_covariance_({}), regularization_enabled_(declare_parameter("regularization_enabled")) { (*state_ptr_)["state"] = "Initializing"; is_activated_ = false; - int points_queue_size = this->declare_parameter("input_sensor_points_queue_size"); - points_queue_size = std::max(points_queue_size, 0); - RCLCPP_INFO(get_logger(), "points_queue_size: %d", points_queue_size); + int64_t points_queue_size = this->declare_parameter("input_sensor_points_queue_size"); + points_queue_size = std::max(points_queue_size, (int64_t)0); + RCLCPP_INFO(get_logger(), "points_queue_size: %ld", points_queue_size); base_frame_ = this->declare_parameter("base_frame"); RCLCPP_INFO(get_logger(), "base_frame_id: %s", base_frame_.c_str()); @@ -137,8 +123,8 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.trans_epsilon = this->declare_parameter("trans_epsilon"); ndt_params.step_size = this->declare_parameter("step_size"); ndt_params.resolution = this->declare_parameter("resolution"); - ndt_params.max_iterations = this->declare_parameter("max_iterations"); - ndt_params.num_threads = this->declare_parameter("num_threads"); + ndt_params.max_iterations = static_cast(this->declare_parameter("max_iterations")); + ndt_params.num_threads = static_cast(this->declare_parameter("num_threads")); ndt_params.num_threads = std::max(ndt_params.num_threads, 1); ndt_params.regularization_scale_factor = static_cast(this->declare_parameter("regularization_scale_factor")); @@ -149,7 +135,7 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.trans_epsilon, ndt_params.step_size, ndt_params.resolution, ndt_params.max_iterations); - int converged_param_type_tmp = this->declare_parameter("converged_param_type"); + const int64_t converged_param_type_tmp = this->declare_parameter("converged_param_type"); converged_param_type_ = static_cast(converged_param_type_tmp); converged_param_transform_probability_ = @@ -160,13 +146,16 @@ NDTScanMatcher::NDTScanMatcher() lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); critical_upper_bound_exe_time_ms_ = - this->declare_parameter("critical_upper_bound_exe_time_ms"); + this->declare_parameter("critical_upper_bound_exe_time_ms"); initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); initial_pose_distance_tolerance_m_ = this->declare_parameter("initial_pose_distance_tolerance_m"); + initial_pose_buffer_ = std::make_unique( + this->get_logger(), initial_pose_timeout_sec_, initial_pose_distance_tolerance_m_); + use_cov_estimation_ = this->declare_parameter("use_covariance_estimation"); if (use_cov_estimation_) { std::vector initial_pose_offset_model_x = @@ -175,8 +164,12 @@ NDTScanMatcher::NDTScanMatcher() this->declare_parameter>("initial_pose_offset_model_y"); if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { - initial_pose_offset_model_ = - create_initial_pose_offset_model(initial_pose_offset_model_x, initial_pose_offset_model_y); + const size_t size = initial_pose_offset_model_x.size(); + initial_pose_offset_model_.resize(size); + for (size_t i = 0; i < size; i++) { + initial_pose_offset_model_[i].x() = initial_pose_offset_model_x[i]; + initial_pose_offset_model_[i].y() = initial_pose_offset_model_y[i]; + } } else { RCLCPP_WARN( get_logger(), @@ -191,8 +184,9 @@ NDTScanMatcher::NDTScanMatcher() output_pose_covariance_[i] = output_pose_covariance[i]; } - initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num"); - n_startup_trials_ = this->declare_parameter("n_startup_trials"); + initial_estimate_particles_num_ = + this->declare_parameter("initial_estimate_particles_num"); + n_startup_trials_ = this->declare_parameter("n_startup_trials"); estimate_scores_for_degrounded_scan_ = this->declare_parameter("estimate_scores_for_degrounded_scan"); @@ -232,6 +226,9 @@ NDTScanMatcher::NDTScanMatcher() "regularization_pose_with_covariance", 10, std::bind(&NDTScanMatcher::callback_regularization_pose, this, std::placeholders::_1), initial_pose_sub_opt); + const double value_as_unlimited = 1000.0; + regularization_pose_buffer_ = + std::make_unique(this->get_logger(), value_as_unlimited, value_as_unlimited); } sensor_aligned_pose_pub_ = @@ -350,7 +347,8 @@ void NDTScanMatcher::publish_diagnostic() } if ( state_ptr_->count("execution_time") && - std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { + std::stod((*state_ptr_)["execution_time"]) >= + static_cast(critical_upper_bound_exe_time_ms_)) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; @@ -375,42 +373,21 @@ void NDTScanMatcher::callback_initial_pose( { if (!is_activated_) return; - // lock mutex for initial pose - std::lock_guard initial_pose_array_lock(initial_pose_array_mtx_); - // if rosbag restart, clear buffer - if (!initial_pose_msg_ptr_array_.empty()) { - const builtin_interfaces::msg::Time & t_front = - initial_pose_msg_ptr_array_.front()->header.stamp; - const builtin_interfaces::msg::Time & t_msg = initial_pose_msg_ptr->header.stamp; - if (t_front.sec > t_msg.sec || (t_front.sec == t_msg.sec && t_front.nanosec > t_msg.nanosec)) { - initial_pose_msg_ptr_array_.clear(); - } - } - if (initial_pose_msg_ptr->header.frame_id == map_frame_) { - initial_pose_msg_ptr_array_.push_back(initial_pose_msg_ptr); + initial_pose_buffer_->push_back(initial_pose_msg_ptr); } else { - // get TF from pose_frame to map_frame - auto tf_pose_to_map_ptr = std::make_shared(); - tf2_listener_module_->get_transform( - this->now(), map_frame_, initial_pose_msg_ptr->header.frame_id, tf_pose_to_map_ptr); - - // transform pose_frame to map_frame - auto initial_pose_msg_in_map_ptr = - std::make_shared(); - *initial_pose_msg_in_map_ptr = transform(*initial_pose_msg_ptr, *tf_pose_to_map_ptr); - initial_pose_msg_in_map_ptr->header.stamp = initial_pose_msg_ptr->header.stamp; - initial_pose_msg_ptr_array_.push_back(initial_pose_msg_in_map_ptr); + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), *this->get_clock(), 1000, + "Received initial pose message with frame_id " + << initial_pose_msg_ptr->header.frame_id << ", but expected " << map_frame_ + << ". Please check the frame_id in the input topic and ensure it is correct."); } } void NDTScanMatcher::callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr) { - // Get lock for regularization_pose_msg_ptr_array_ - std::lock_guard lock(regularization_mutex_); - regularization_pose_msg_ptr_array_.push_back(pose_conv_msg_ptr); - // Release lock for regularization_pose_msg_ptr_array_ + regularization_pose_buffer_->push_back(pose_conv_msg_ptr); } void NDTScanMatcher::callback_sensor_points( @@ -459,17 +436,15 @@ void NDTScanMatcher::callback_sensor_points( if (!is_activated_) return; // calculate initial pose - std::unique_lock initial_pose_array_lock(initial_pose_array_mtx_); - if (initial_pose_msg_ptr_array_.size() <= 1) { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No Pose!"); + std::optional interpolation_result_opt = + initial_pose_buffer_->interpolate(sensor_ros_time); + if (!interpolation_result_opt) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No interpolated pose!"); return; } - PoseArrayInterpolator interpolator( - this, sensor_ros_time, initial_pose_msg_ptr_array_, initial_pose_timeout_sec_, - initial_pose_distance_tolerance_m_); - if (!interpolator.is_success()) return; - pop_old_pose(initial_pose_msg_ptr_array_, sensor_ros_time); - initial_pose_array_lock.unlock(); + initial_pose_buffer_->pop_old(sensor_ros_time); + const SmartPoseBuffer::InterpolateResult & interpolation_result = + interpolation_result_opt.value(); // if regularization is enabled and available, set pose to NDT for regularization if (regularization_enabled_) { @@ -483,7 +458,7 @@ void NDTScanMatcher::callback_sensor_points( // perform ndt scan matching const Eigen::Matrix4f initial_pose_matrix = - pose_to_matrix4f(interpolator.get_current_pose().pose.pose); + pose_to_matrix4f(interpolation_result.interpolated_pose.pose.pose); auto output_cloud = std::make_shared>(); ndt_ptr_->align(*output_cloud, initial_pose_matrix); const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); @@ -528,7 +503,7 @@ void NDTScanMatcher::callback_sensor_points( const auto exe_time = static_cast(duration_micro_sec) / 1000.0f; // publish - initial_pose_with_covariance_pub_->publish(interpolator.get_current_pose()); + initial_pose_with_covariance_pub_->publish(interpolation_result.interpolated_pose); exe_time_pub_->publish(make_float32_stamped(sensor_ros_time, exe_time)); transform_probability_pub_->publish( make_float32_stamped(sensor_ros_time, ndt_result.transform_probability)); @@ -539,8 +514,8 @@ void NDTScanMatcher::callback_sensor_points( publish_pose(sensor_ros_time, result_pose_msg, ndt_covariance, is_converged); publish_marker(sensor_ros_time, transformation_msg_array); publish_initial_to_result( - sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(), - interpolator.get_new_pose()); + sensor_ros_time, result_pose_msg, interpolation_result.interpolated_pose, + interpolation_result.old_pose, interpolation_result.new_pose); pcl::shared_ptr> sensor_points_in_map_ptr( new pcl::PointCloud); @@ -765,12 +740,12 @@ std::array NDTScanMatcher::estimate_covariance( rot = find_rotation_matrix_aligning_covariance_to_principal_axes( ndt_result.hessian.inverse().block(0, 0, 2, 2)); } catch (const std::exception & e) { - RCLCPP_WARN(get_logger(), e.what()); + RCLCPP_WARN(get_logger(), "Error in Eigen solver: %s", e.what()); return output_pose_covariance_; } // first result is added to mean - const int n = initial_pose_offset_model_.size() + 1; + const int n = static_cast(initial_pose_offset_model_.size()) + 1; const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); Eigen::Vector2d mean = ndt_pose_2d; std::vector ndt_pose_2d_vec; @@ -828,42 +803,20 @@ std::array NDTScanMatcher::estimate_covariance( return ndt_covariance; } -std::optional NDTScanMatcher::interpolate_regularization_pose( - const rclcpp::Time & sensor_ros_time) -{ - std::shared_ptr interpolator = nullptr; - { - // Get lock for regularization_pose_msg_ptr_array_ - std::lock_guard lock(regularization_mutex_); - - if (regularization_pose_msg_ptr_array_.empty()) { - return std::nullopt; - } - - interpolator = std::make_shared( - this, sensor_ros_time, regularization_pose_msg_ptr_array_); - - // Remove old poses to make next interpolation more efficient - pop_old_pose(regularization_pose_msg_ptr_array_, sensor_ros_time); - - // Release lock for regularization_pose_msg_ptr_array_ - } - - if (!interpolator || !interpolator->is_success()) { - return std::nullopt; - } - - return pose_to_matrix4f(interpolator->get_current_pose().pose.pose); -} - void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_time) { ndt_ptr_->unsetRegularizationPose(); - std::optional pose_opt = interpolate_regularization_pose(sensor_ros_time); - if (pose_opt.has_value()) { - ndt_ptr_->setRegularizationPose(pose_opt.value()); - RCLCPP_DEBUG_STREAM(get_logger(), "Regularization pose is set to NDT"); + std::optional interpolation_result_opt = + regularization_pose_buffer_->interpolate(sensor_ros_time); + if (!interpolation_result_opt) { + return; } + regularization_pose_buffer_->pop_old(sensor_ros_time); + const SmartPoseBuffer::InterpolateResult & interpolation_result = + interpolation_result_opt.value(); + const Eigen::Matrix4f pose = pose_to_matrix4f(interpolation_result.interpolated_pose.pose.pose); + ndt_ptr_->setRegularizationPose(pose); + RCLCPP_DEBUG_STREAM(get_logger(), "Regularization pose is set to NDT"); } void NDTScanMatcher::service_trigger_node( @@ -872,8 +825,7 @@ void NDTScanMatcher::service_trigger_node( { is_activated_ = req->data; if (is_activated_) { - std::lock_guard initial_pose_array_lock(initial_pose_array_mtx_); - initial_pose_msg_ptr_array_.clear(); + initial_pose_buffer_->clear(); } res->success = true; } @@ -936,16 +888,16 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( // The range taken by 2 * phi(x) - 1 is [-1, 1], so it can be used as a uniform distribution in // TPE. Let u = 2 * phi(x) - 1, then x = sqrt(2) * erf_inv(u). Computationally, it is not a good // to give erf_inv -1 and 1, so it is rounded off at (-1 + eps, 1 - eps). - const double SQRT2 = std::sqrt(2); - auto uniform_to_normal = [&SQRT2](const double uniform) { + const double sqrt2 = std::sqrt(2); + auto uniform_to_normal = [&sqrt2](const double uniform) { assert(-1.0 <= uniform && uniform <= 1.0); constexpr double epsilon = 1.0e-6; const double clamped = std::clamp(uniform, -1.0 + epsilon, 1.0 - epsilon); - return boost::math::erf_inv(clamped) * SQRT2; + return boost::math::erf_inv(clamped) * sqrt2; }; - auto normal_to_uniform = [&SQRT2](const double normal) { - return boost::math::erf(normal / SQRT2); + auto normal_to_uniform = [&sqrt2](const double normal) { + return boost::math::erf(normal / sqrt2); }; // Optimizing (x, y, z, roll, pitch, yaw) 6 dimensions. @@ -960,7 +912,12 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( std::vector particle_array; auto output_cloud = std::make_shared>(); - for (int i = 0; i < initial_estimate_particles_num_; i++) { + // publish the estimated poses in 20 times to see the progress and to avoid dropping data + visualization_msgs::msg::MarkerArray marker_array; + constexpr int64_t publish_num = 20; + const int64_t publish_interval = initial_estimate_particles_num_ / publish_num; + + for (int64_t i = 0; i < initial_estimate_particles_num_; i++) { const TreeStructuredParzenEstimator::Input input = tpe.get_next_input(); geometry_msgs::msg::Pose initial_pose; @@ -986,10 +943,11 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, ndt_result.iteration_num); particle_array.push_back(particle); - const auto marker_array = make_debug_markers( - get_clock()->now(), map_frame_, tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1), - particle, i); - ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + push_debug_markers(marker_array, get_clock()->now(), map_frame_, particle, i); + if ((i + 1) % publish_interval == 0 || (i + 1) == initial_estimate_particles_num_) { + ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + marker_array.markers.clear(); + } const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose); const geometry_msgs::msg::Vector3 rpy = get_rpy(pose); diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp index d5ea544d3c5e5..6e2ed022f531e 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp @@ -20,7 +20,7 @@ int main(int argc, char ** argv) { - google::InitGoogleLogging(argv[0]); + google::InitGoogleLogging(argv[0]); // NOLINT google::InstallFailureSignalHandler(); rclcpp::init(argc, argv); diff --git a/localization/ndt_scan_matcher/src/debug.cpp b/localization/ndt_scan_matcher/src/particle.cpp similarity index 83% rename from localization/ndt_scan_matcher/src/debug.cpp rename to localization/ndt_scan_matcher/src/particle.cpp index 941f682de5803..b30703c2761e8 100644 --- a/localization/ndt_scan_matcher/src/debug.cpp +++ b/localization/ndt_scan_matcher/src/particle.cpp @@ -12,24 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/debug.hpp" +#include "ndt_scan_matcher/particle.hpp" #include "localization_util/util_func.hpp" -visualization_msgs::msg::MarkerArray make_debug_markers( - const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, - const geometry_msgs::msg::Vector3 & scale, const Particle & particle, const size_t i) +void push_debug_markers( + visualization_msgs::msg::MarkerArray & marker_array, const builtin_interfaces::msg::Time & stamp, + const std::string & map_frame_, const Particle & particle, const size_t i) { - // TODO(Tier IV): getNumSubscribers - // TODO(Tier IV): clear old object - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker marker; marker.header.stamp = stamp; marker.header.frame_id = map_frame_; marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = scale; + marker.scale.x = 0.3; + marker.scale.y = 0.1; + marker.scale.z = 0.1; marker.id = static_cast(i); marker.lifetime = rclcpp::Duration::from_seconds(10.0); // 10.0 is the lifetime in seconds. @@ -62,6 +60,4 @@ visualization_msgs::msg::MarkerArray make_debug_markers( marker.pose = particle.result_pose; marker.color = exchange_color_crc(static_cast(i) / 100.0); marker_array.markers.push_back(marker); - - return marker_array; } diff --git a/localization/stop_filter/README.md b/localization/stop_filter/README.md index 714824b430d74..261b8c9da392c 100644 --- a/localization/stop_filter/README.md +++ b/localization/stop_filter/README.md @@ -25,7 +25,4 @@ This node aims to: ## Parameters -| Name | Type | Description | -| -------------- | ------ | --------------------------------------------------------------------------------------------- | -| `vx_threshold` | double | longitudinal velocity threshold to determine if the vehicle is stopping [m/s] (default: 0.01) | -| `wz_threshold` | double | yaw velocity threshold to determine if the vehicle is stopping [rad/s] (default: 0.01) | +{{ json_to_markdown("localization/stop_filter/schema/stop_filter.schema.json") }} diff --git a/localization/stop_filter/schema/stop_filter.schema.json b/localization/stop_filter/schema/stop_filter.schema.json new file mode 100644 index 0000000000000..ce6d4b8a2bb72 --- /dev/null +++ b/localization/stop_filter/schema/stop_filter.schema.json @@ -0,0 +1,40 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Stop Filter Node", + "type": "object", + "definitions": { + "stop_filter": { + "type": "object", + "properties": { + "vx_threshold": { + "type": "number", + "description": "Longitudinal velocity threshold to determine if the vehicle is stopping. [m/s]", + "default": "0.01", + "minimum": 0.0 + }, + "wz_threshold": { + "type": "number", + "description": "Yaw velocity threshold to determine if the vehicle is stopping. [rad/s]", + "default": "0.01", + "minimum": 0.0 + } + }, + "required": ["vx_threshold", "wz_threshold"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/stop_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/bytetrack/CMakeLists.txt b/perception/bytetrack/CMakeLists.txt index e481464455a55..c020843af56b5 100644 --- a/perception/bytetrack/CMakeLists.txt +++ b/perception/bytetrack/CMakeLists.txt @@ -29,7 +29,10 @@ target_include_directories(bytetrack_lib $ $ ) -target_link_libraries(bytetrack_lib Eigen3::Eigen) +target_link_libraries(bytetrack_lib + Eigen3::Eigen + yaml-cpp +) # # ROS node @@ -91,4 +94,5 @@ rclcpp_components_register_node(${PROJECT_NAME}_visualizer ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/bytetrack/README.md b/perception/bytetrack/README.md index e58f4bb5892aa..cabbdd2f05836 100644 --- a/perception/bytetrack/README.md +++ b/perception/bytetrack/README.md @@ -6,7 +6,7 @@ The core algorithm, named `ByteTrack`, mainly aims to perform multi-object track Because the algorithm associates almost every detection box including ones with low detection scores, the number of false negatives is expected to decrease by using it. -[demo video](https://user-images.githubusercontent.com/3022416/225920856-745a3bb7-6b35-403d-87b0-6e5085952d70.mp4) +[demo video](https://github.com/YoshiRi/autoware.universe/assets/3022416/40f4c158-657e-48e1-81c2-8ac39152892d) ## Inner-workings / Algorithms @@ -20,6 +20,16 @@ the number of false negatives is expected to decrease by using it. - This package is ported version toward Autoware from [this repository](https://github.com/ifzhang/ByteTrack/tree/main/deploy/TensorRT/cpp) (The C++ implementation by the ByteTrack's authors) +### 2d tracking modification from original codes + +The paper just says that the 2d tracking algorithm is a simple Kalman filter. +Original codes use the `top-left-corner` and `aspect ratio` and `size` as the state vector. + +This is sometimes unstable because the aspectratio can be changed by the occlusion. +So, we use the `top-left` and `size` as the state vector. + +Kalman filter settings can be controlled by the parameters in `config/bytetrack_node.param.yaml`. + ## Inputs / Outputs ### bytetrack_node diff --git a/perception/bytetrack/config/bytetracker.param.yaml b/perception/bytetrack/config/bytetracker.param.yaml new file mode 100644 index 0000000000000..7078f190c87e6 --- /dev/null +++ b/perception/bytetrack/config/bytetracker.param.yaml @@ -0,0 +1,14 @@ +# Kalman filter parameters for 2d tracking +dt: 0.1 # time step[s] +dim_x: 8 # tlbr + velocity +dim_z: 4 # tlbr +q_cov_x: 0.1 +q_cov_y: 0.1 +q_cov_vx: 0.1 +q_cov_vy: 0.1 +r_cov_x: 0.1 +r_cov_y: 0.1 +p0_cov_x: 100.0 +p0_cov_y: 100.0 +p0_cov_vx: 100.0 +p0_cov_vy: 100.0 diff --git a/perception/bytetrack/lib/include/byte_tracker.h b/perception/bytetrack/lib/include/byte_tracker.h index 08394e6b0ccdf..60328c5acfd62 100644 --- a/perception/bytetrack/lib/include/byte_tracker.h +++ b/perception/bytetrack/lib/include/byte_tracker.h @@ -96,5 +96,5 @@ class ByteTracker std::vector tracked_stracks; std::vector lost_stracks; std::vector removed_stracks; - byte_kalman::KalmanFilter kalman_filter; + KalmanFilter kalman_filter; }; diff --git a/perception/bytetrack/lib/include/kalman_filter.h b/perception/bytetrack/lib/include/kalman_filter.h deleted file mode 100644 index f2b1e1c7817dd..0000000000000 --- a/perception/bytetrack/lib/include/kalman_filter.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - * MIT License - * - * Copyright (c) 2021 Yifu Zhang - * - * 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. - */ - -// 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. - -#pragma once - -#include "data_type.h" - -#include -namespace byte_kalman -{ -class KalmanFilter -{ -public: - static const double chi2inv95[10]; - KalmanFilter(); - KAL_DATA initiate(const DETECTBOX & measurement); - void predict(KAL_MEAN & mean, KAL_COVA & covariance); - KAL_HDATA project(const KAL_MEAN & mean, const KAL_COVA & covariance); - KAL_DATA update( - const KAL_MEAN & mean, const KAL_COVA & covariance, const DETECTBOX & measurement); - - Eigen::Matrix gating_distance( - const KAL_MEAN & mean, const KAL_COVA & covariance, const std::vector & measurements, - bool only_position = false); - -private: - Eigen::Matrix _motion_mat; - Eigen::Matrix _update_mat; - float _std_weight_position; - float _std_weight_velocity; -}; -} // namespace byte_kalman diff --git a/perception/bytetrack/lib/include/strack.h b/perception/bytetrack/lib/include/strack.h index 2110723e86c58..260f38ea93770 100644 --- a/perception/bytetrack/lib/include/strack.h +++ b/perception/bytetrack/lib/include/strack.h @@ -38,16 +38,18 @@ #pragma once -#include "kalman_filter.h" - +// #include "kalman_filter.h" +#include #include #include +#include #include enum TrackState { New = 0, Tracked, Lost, Removed }; +/** manage one tracklet*/ class STrack { public: @@ -55,8 +57,7 @@ class STrack ~STrack(); std::vector static tlbr_to_tlwh(std::vector & tlbr); - static void multi_predict( - std::vector & stracks, byte_kalman::KalmanFilter & kalman_filter); + static void multi_predict(std::vector & stracks); void static_tlwh(); void static_tlbr(); std::vector tlwh_to_xyah(std::vector tlwh_tmp); @@ -65,10 +66,16 @@ class STrack void mark_removed(); int next_id(); int end_frame(); + void init_kalman_filter(); + void update_kalman_filter(const Eigen::MatrixXd & measurement); + void reflect_state(); - void activate(byte_kalman::KalmanFilter & kalman_filter, int frame_id); + void activate(int frame_id); void re_activate(STrack & new_track, int frame_id, bool new_id = false); void update(STrack & new_track, int frame_id); + void predict(const int frame_id); + + void load_parameters(const std::string & filename); public: bool is_activated; @@ -76,19 +83,41 @@ class STrack boost::uuids::uuid unique_id; int state; - std::vector _tlwh; - std::vector tlwh; - std::vector tlbr; + std::vector original_tlwh; // top left width height + std::vector tlwh; // top left width height + std::vector tlbr; // top left bottom right int frame_id; int tracklet_len; int start_frame; - KAL_MEAN mean; - KAL_COVA covariance; float score; int label; private: - byte_kalman::KalmanFilter kalman_filter; + KalmanFilter kalman_filter_; + struct KfParams + { + // dimension + char dim_x = 8; + char dim_z = 4; + // system noise + float q_cov_x; + float q_cov_y; + float q_cov_vx; + float q_cov_vy; + // measurement noise + float r_cov_x; + float r_cov_y; + // initial state covariance + float p0_cov_x; + float p0_cov_y; + float p0_cov_vx; + float p0_cov_vy; + // other parameters + float dt; // sampling time + }; + static KfParams _kf_parameters; + static bool _parameters_loaded; + enum IDX { X1 = 0, Y1 = 1, X2 = 2, Y2 = 3, VX1 = 4, VY1 = 5, VX2 = 6, VY2 = 7 }; }; diff --git a/perception/bytetrack/lib/src/byte_tracker.cpp b/perception/bytetrack/lib/src/byte_tracker.cpp index 94aae0a5c94cc..a3477e78ad6dd 100644 --- a/perception/bytetrack/lib/src/byte_tracker.cpp +++ b/perception/bytetrack/lib/src/byte_tracker.cpp @@ -106,7 +106,10 @@ std::vector ByteTracker::update(const std::vector & obj ////////////////// Step 2: First association, with IoU ////////////////// strack_pool = joint_stracks(tracked_stracks, this->lost_stracks); - STrack::multi_predict(strack_pool, this->kalman_filter); + // do prediction for each stracks + for (size_t i = 0; i < strack_pool.size(); i++) { + strack_pool[i]->predict(this->frame_id); + } std::vector > dists; int dist_size = 0, dist_size_size = 0; @@ -196,7 +199,7 @@ std::vector ByteTracker::update(const std::vector & obj for (size_t i = 0; i < u_detection.size(); i++) { STrack * track = &detections[u_detection[i]]; if (track->score < this->high_thresh) continue; - track->activate(this->kalman_filter, this->frame_id); + track->activate(this->frame_id); activated_stracks.push_back(*track); } diff --git a/perception/bytetrack/lib/src/kalman_filter.cpp b/perception/bytetrack/lib/src/kalman_filter.cpp deleted file mode 100644 index e515450858d80..0000000000000 --- a/perception/bytetrack/lib/src/kalman_filter.cpp +++ /dev/null @@ -1,175 +0,0 @@ -/* - * MIT License - * - * Copyright (c) 2021 Yifu Zhang - * - * 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. - */ - -// 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 "kalman_filter.h" - -#include - -namespace byte_kalman -{ -const double KalmanFilter::chi2inv95[10] = {0, 3.8415, 5.9915, 7.8147, 9.4877, - 11.070, 12.592, 14.067, 15.507, 16.919}; -KalmanFilter::KalmanFilter() -{ - int ndim = 4; - double dt = 1.; - - _motion_mat = Eigen::MatrixXf::Identity(8, 8); - for (int i = 0; i < ndim; i++) { - _motion_mat(i, ndim + i) = dt; - } - _update_mat = Eigen::MatrixXf::Identity(4, 8); - - this->_std_weight_position = 1. / 20; - this->_std_weight_velocity = 1. / 160; -} - -KAL_DATA KalmanFilter::initiate(const DETECTBOX & measurement) -{ - DETECTBOX mean_pos = measurement; - DETECTBOX mean_vel; - for (int i = 0; i < 4; i++) mean_vel(i) = 0; - - KAL_MEAN mean; - for (int i = 0; i < 8; i++) { - if (i < 4) - mean(i) = mean_pos(i); - else - mean(i) = mean_vel(i - 4); - } - - KAL_MEAN std; - std(0) = 2 * _std_weight_position * measurement[3]; - std(1) = 2 * _std_weight_position * measurement[3]; - std(2) = 1e-2; - std(3) = 2 * _std_weight_position * measurement[3]; - std(4) = 10 * _std_weight_velocity * measurement[3]; - std(5) = 10 * _std_weight_velocity * measurement[3]; - std(6) = 1e-5; - std(7) = 10 * _std_weight_velocity * measurement[3]; - - KAL_MEAN tmp = std.array().square(); - KAL_COVA var = tmp.asDiagonal(); - return std::make_pair(mean, var); -} - -void KalmanFilter::predict(KAL_MEAN & mean, KAL_COVA & covariance) -{ - // revise the data; - DETECTBOX std_pos; - std_pos << _std_weight_position * mean(3), _std_weight_position * mean(3), 1e-2, - _std_weight_position * mean(3); - DETECTBOX std_vel; - std_vel << _std_weight_velocity * mean(3), _std_weight_velocity * mean(3), 1e-5, - _std_weight_velocity * mean(3); - KAL_MEAN tmp; - tmp.block<1, 4>(0, 0) = std_pos; - tmp.block<1, 4>(0, 4) = std_vel; - tmp = tmp.array().square(); - KAL_COVA motion_cov = tmp.asDiagonal(); - KAL_MEAN mean1 = this->_motion_mat * mean.transpose(); - KAL_COVA covariance1 = this->_motion_mat * covariance * (_motion_mat.transpose()); - covariance1 += motion_cov; - - mean = mean1; - covariance = covariance1; -} - -KAL_HDATA KalmanFilter::project(const KAL_MEAN & mean, const KAL_COVA & covariance) -{ - DETECTBOX std; - std << _std_weight_position * mean(3), _std_weight_position * mean(3), 1e-1, - _std_weight_position * mean(3); - KAL_HMEAN mean1 = _update_mat * mean.transpose(); - KAL_HCOVA covariance1 = _update_mat * covariance * (_update_mat.transpose()); - Eigen::Matrix diag = std.asDiagonal(); - diag = diag.array().square().matrix(); - covariance1 += diag; - // covariance1.diagonal() << diag; - return std::make_pair(mean1, covariance1); -} - -KAL_DATA -KalmanFilter::update( - const KAL_MEAN & mean, const KAL_COVA & covariance, const DETECTBOX & measurement) -{ - KAL_HDATA pa = project(mean, covariance); - KAL_HMEAN projected_mean = pa.first; - KAL_HCOVA projected_cov = pa.second; - - // chol_factor, lower = - // scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False) - // kalmain_gain = - // scipy.linalg.cho_solve((cho_factor, lower), - // np.dot(covariance, self._upadte_mat.T).T, - // check_finite=False).T - Eigen::Matrix B = (covariance * (_update_mat.transpose())).transpose(); - Eigen::Matrix kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4 - Eigen::Matrix innovation = measurement - projected_mean; // eg.1x4 - auto tmp = innovation * (kalman_gain.transpose()); - KAL_MEAN new_mean = (mean.array() + tmp.array()).matrix(); - KAL_COVA new_covariance = covariance - kalman_gain * projected_cov * (kalman_gain.transpose()); - return std::make_pair(new_mean, new_covariance); -} - -Eigen::Matrix KalmanFilter::gating_distance( - const KAL_MEAN & mean, const KAL_COVA & covariance, const std::vector & measurements, - bool only_position) -{ - KAL_HDATA pa = this->project(mean, covariance); - if (only_position) { - printf("not implement!"); - exit(0); - } - KAL_HMEAN mean1 = pa.first; - KAL_HCOVA covariance1 = pa.second; - - // Eigen::Matrix d(size, 4); - DETECTBOXSS d(measurements.size(), 4); - int pos = 0; - for (DETECTBOX box : measurements) { - d.row(pos++) = box - mean1; - } - Eigen::Matrix factor = covariance1.llt().matrixL(); - Eigen::Matrix z = - factor.triangularView().solve(d).transpose(); - auto zz = ((z.array()) * (z.array())).matrix(); - auto square_maha = zz.colwise().sum(); - return square_maha; -} -} // namespace byte_kalman diff --git a/perception/bytetrack/lib/src/strack.cpp b/perception/bytetrack/lib/src/strack.cpp index b06456098cc38..eda11509d2403 100644 --- a/perception/bytetrack/lib/src/strack.cpp +++ b/perception/bytetrack/lib/src/strack.cpp @@ -38,12 +38,20 @@ #include "strack.h" +#include + #include -STrack::STrack(std::vector tlwh_, float score, int label) +#include + +// init static variable +bool STrack::_parameters_loaded = false; +STrack::KfParams STrack::_kf_parameters; + +STrack::STrack(std::vector input_tlwh, float score, int label) { - _tlwh.resize(4); - _tlwh.assign(tlwh_.begin(), tlwh_.end()); + original_tlwh.resize(4); + original_tlwh.assign(input_tlwh.begin(), input_tlwh.end()); is_activated = false; track_id = 0; @@ -52,49 +60,71 @@ STrack::STrack(std::vector tlwh_, float score, int label) tlwh.resize(4); tlbr.resize(4); - static_tlwh(); - static_tlbr(); + static_tlwh(); // update object size + static_tlbr(); // update object size frame_id = 0; tracklet_len = 0; this->score = score; start_frame = 0; this->label = label; + + // load static kf parameters: initialized once in program + const std::string package_share_directory = + ament_index_cpp::get_package_share_directory("bytetrack"); + const std::string default_config_path = + package_share_directory + "/config/bytetracker.param.yaml"; + if (!_parameters_loaded) { + load_parameters(default_config_path); + _parameters_loaded = true; + } } STrack::~STrack() { } -void STrack::activate(byte_kalman::KalmanFilter & kalman_filter, int frame_id) +void STrack::init_kalman_filter() +{ + // assert parameter is loaded + assert(_parameters_loaded); + + // init kalman filter state + Eigen::MatrixXd X0 = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, 1); + Eigen::MatrixXd P0 = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, _kf_parameters.dim_x); + X0(IDX::X1) = this->original_tlwh[0]; + X0(IDX::Y1) = this->original_tlwh[1]; + X0(IDX::X2) = this->original_tlwh[2]; + X0(IDX::Y2) = this->original_tlwh[3]; + P0(IDX::X1, IDX::X1) = _kf_parameters.p0_cov_x; + P0(IDX::Y1, IDX::Y1) = _kf_parameters.p0_cov_y; + P0(IDX::X2, IDX::X2) = _kf_parameters.p0_cov_x; + P0(IDX::Y2, IDX::Y2) = _kf_parameters.p0_cov_y; + P0(IDX::VX1, IDX::VX1) = _kf_parameters.p0_cov_vx; + P0(IDX::VY1, IDX::VY1) = _kf_parameters.p0_cov_vy; + P0(IDX::VX2, IDX::VX2) = _kf_parameters.p0_cov_vx; + P0(IDX::VY2, IDX::VY2) = _kf_parameters.p0_cov_vy; + this->kalman_filter_.init(X0, P0); +} + +/** init a tracklet */ +void STrack::activate(int frame_id) { - this->kalman_filter = kalman_filter; this->track_id = this->next_id(); this->unique_id = boost::uuids::random_generator()(); std::vector _tlwh_tmp(4); - _tlwh_tmp[0] = this->_tlwh[0]; - _tlwh_tmp[1] = this->_tlwh[1]; - _tlwh_tmp[2] = this->_tlwh[2]; - _tlwh_tmp[3] = this->_tlwh[3]; + _tlwh_tmp[0] = this->original_tlwh[0]; + _tlwh_tmp[1] = this->original_tlwh[1]; + _tlwh_tmp[2] = this->original_tlwh[2]; + _tlwh_tmp[3] = this->original_tlwh[3]; std::vector xyah = tlwh_to_xyah(_tlwh_tmp); - DETECTBOX xyah_box; - xyah_box[0] = xyah[0]; - xyah_box[1] = xyah[1]; - xyah_box[2] = xyah[2]; - xyah_box[3] = xyah[3]; - auto mc = this->kalman_filter.initiate(xyah_box); - this->mean = mc.first; - this->covariance = mc.second; - - static_tlwh(); - static_tlbr(); + // init kf + init_kalman_filter(); + // reflect state + reflect_state(); this->tracklet_len = 0; this->state = TrackState::Tracked; - // if (frame_id == 1) - // { - // this->is_activated = true; - // } this->is_activated = true; this->frame_id = frame_id; this->start_frame = frame_id; @@ -103,17 +133,12 @@ void STrack::activate(byte_kalman::KalmanFilter & kalman_filter, int frame_id) void STrack::re_activate(STrack & new_track, int frame_id, bool new_id) { std::vector xyah = tlwh_to_xyah(new_track.tlwh); - DETECTBOX xyah_box; - xyah_box[0] = xyah[0]; - xyah_box[1] = xyah[1]; - xyah_box[2] = xyah[2]; - xyah_box[3] = xyah[3]; - auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box); - this->mean = mc.first; - this->covariance = mc.second; + // TODO(me): write kf update + Eigen::MatrixXd measurement = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, 1); + measurement << new_track.tlwh[0], new_track.tlwh[1], new_track.tlwh[2], new_track.tlwh[3]; + update_kalman_filter(measurement); - static_tlwh(); - static_tlbr(); + reflect_state(); this->tracklet_len = 0; this->state = TrackState::Tracked; @@ -132,18 +157,14 @@ void STrack::update(STrack & new_track, int frame_id) this->tracklet_len++; std::vector xyah = tlwh_to_xyah(new_track.tlwh); - DETECTBOX xyah_box; - xyah_box[0] = xyah[0]; - xyah_box[1] = xyah[1]; - xyah_box[2] = xyah[2]; - xyah_box[3] = xyah[3]; - auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box); - this->mean = mc.first; - this->covariance = mc.second; + // update + // TODO(me): write update + Eigen::MatrixXd measurement = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, 1); + measurement << new_track.tlwh[0], new_track.tlwh[1], new_track.tlwh[2], new_track.tlwh[3]; + update_kalman_filter(measurement); - static_tlwh(); - static_tlbr(); + reflect_state(); this->state = TrackState::Tracked; this->is_activated = true; @@ -151,24 +172,31 @@ void STrack::update(STrack & new_track, int frame_id) this->score = new_track.score; } +/** reflect kalman filter state to current object variables*/ +void STrack::reflect_state() +{ + // update tlwh + static_tlwh(); + // update tlbr + static_tlbr(); +} + void STrack::static_tlwh() { if (this->state == TrackState::New) { - tlwh[0] = _tlwh[0]; - tlwh[1] = _tlwh[1]; - tlwh[2] = _tlwh[2]; - tlwh[3] = _tlwh[3]; + tlwh[0] = original_tlwh[0]; + tlwh[1] = original_tlwh[1]; + tlwh[2] = original_tlwh[2]; + tlwh[3] = original_tlwh[3]; return; } - - tlwh[0] = mean[0]; - tlwh[1] = mean[1]; - tlwh[2] = mean[2]; - tlwh[3] = mean[3]; - - tlwh[2] *= tlwh[3]; - tlwh[0] -= tlwh[2] / 2; - tlwh[1] -= tlwh[3] / 2; + // put kf state to tlwh + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, 1); + this->kalman_filter_.getX(X); + tlwh[0] = X(IDX::X1); + tlwh[1] = X(IDX::Y1); + tlwh[2] = X(IDX::X2); + tlwh[3] = X(IDX::Y2); } void STrack::static_tlbr() @@ -222,15 +250,102 @@ int STrack::end_frame() return this->frame_id; } -void STrack::multi_predict( - std::vector & stracks, byte_kalman::KalmanFilter & kalman_filter) +void STrack::multi_predict(std::vector & stracks) { for (size_t i = 0; i < stracks.size(); i++) { if (stracks[i]->state != TrackState::Tracked) { - stracks[i]->mean[7] = 0; + // not tracked } - kalman_filter.predict(stracks[i]->mean, stracks[i]->covariance); + // prediction + stracks[i]->predict(stracks[i]->frame_id + 1); + // TODO(me): write prediction stracks[i]->static_tlwh(); stracks[i]->static_tlbr(); } } + +void STrack::update_kalman_filter(const Eigen::MatrixXd & measurement) +{ + // assert parameter is loaded + assert(_parameters_loaded); + + // get C matrix + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, _kf_parameters.dim_x); + C(IDX::X1, IDX::X1) = 1; + C(IDX::Y1, IDX::Y1) = 1; + C(IDX::X2, IDX::X2) = 1; + C(IDX::Y2, IDX::Y2) = 1; + + // get R matrix + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, _kf_parameters.dim_z); + R(IDX::X1, IDX::X1) = _kf_parameters.r_cov_x; + R(IDX::Y1, IDX::Y1) = _kf_parameters.r_cov_y; + R(IDX::X2, IDX::X2) = _kf_parameters.r_cov_x; + R(IDX::Y2, IDX::Y2) = _kf_parameters.r_cov_y; + + // update + if (!this->kalman_filter_.update(measurement, C, R)) { + std::cerr << "Cannot update" << std::endl; + } +} + +void STrack::predict(const int frame_id) +{ + // check state is Tracked + if (this->state != TrackState::Tracked) { + // not tracked + return; + } + + // else do prediction + float time_elapsed = _kf_parameters.dt * (frame_id - this->frame_id); + // A matrix + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(_kf_parameters.dim_x, _kf_parameters.dim_x); + A(IDX::X1, IDX::VX1) = time_elapsed; + A(IDX::Y1, IDX::VY1) = time_elapsed; + A(IDX::X2, IDX::VX2) = time_elapsed; + A(IDX::Y2, IDX::VY2) = time_elapsed; + + // u and B matrix + Eigen::MatrixXd u = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, 1); + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, _kf_parameters.dim_x); + + // get P_t + Eigen::MatrixXd P_t = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, _kf_parameters.dim_x); + this->kalman_filter_.getP(P_t); + + // Q matrix + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, _kf_parameters.dim_x); + Q(IDX::X1, IDX::X1) = _kf_parameters.q_cov_x; + Q(IDX::Y1, IDX::Y1) = _kf_parameters.q_cov_y; + Q(IDX::VX1, IDX::VX1) = _kf_parameters.q_cov_vx; + Q(IDX::VY1, IDX::VY1) = _kf_parameters.q_cov_vy; + Q(IDX::X2, IDX::X2) = _kf_parameters.q_cov_x; + Q(IDX::Y2, IDX::Y2) = _kf_parameters.q_cov_y; + Q(IDX::VX2, IDX::VX2) = _kf_parameters.q_cov_vx; + Q(IDX::VY2, IDX::VY2) = _kf_parameters.q_cov_vy; + + // prediction + if (!this->kalman_filter_.predict(u, A, B, Q)) { + std::cerr << "Cannot predict" << std::endl; + } +} + +void STrack::load_parameters(const std::string & path) +{ + YAML::Node config = YAML::LoadFile(path); + // initialize ekf params + _kf_parameters.dim_x = config["dim_x"].as(); + _kf_parameters.dim_z = config["dim_z"].as(); + _kf_parameters.q_cov_x = config["q_cov_x"].as(); + _kf_parameters.q_cov_y = config["q_cov_y"].as(); + _kf_parameters.q_cov_vx = config["q_cov_vx"].as(); + _kf_parameters.q_cov_vy = config["q_cov_vy"].as(); + _kf_parameters.r_cov_x = config["r_cov_x"].as(); + _kf_parameters.r_cov_y = config["r_cov_y"].as(); + _kf_parameters.p0_cov_x = config["p0_cov_x"].as(); + _kf_parameters.p0_cov_y = config["p0_cov_y"].as(); + _kf_parameters.p0_cov_vx = config["p0_cov_vx"].as(); + _kf_parameters.p0_cov_vy = config["p0_cov_vy"].as(); + _kf_parameters.dt = config["dt"].as(); +} diff --git a/perception/bytetrack/package.xml b/perception/bytetrack/package.xml index 6b3891438c179..da1768e1bf7ea 100644 --- a/perception/bytetrack/package.xml +++ b/perception/bytetrack/package.xml @@ -19,6 +19,7 @@ cv_bridge eigen image_transport + kalman_filter libboost-system-dev libopencv-dev rclcpp @@ -26,6 +27,7 @@ sensor_msgs tensorrt_common tier4_perception_msgs + yaml-cpp ament_lint_auto autoware_lint_common diff --git a/perception/bytetrack/src/bytetrack_node.cpp b/perception/bytetrack/src/bytetrack_node.cpp index 358a5053b5d4a..eb5e73312a890 100644 --- a/perception/bytetrack/src/bytetrack_node.cpp +++ b/perception/bytetrack/src/bytetrack_node.cpp @@ -83,10 +83,18 @@ void ByteTrackNode::on_rect( bytetrack::ObjectArray objects = bytetrack_->update_tracker(object_array); for (const auto & tracked_object : objects) { tier4_perception_msgs::msg::DetectedObjectWithFeature object; - object.feature.roi.x_offset = tracked_object.x_offset; - object.feature.roi.y_offset = tracked_object.y_offset; - object.feature.roi.width = tracked_object.width; - object.feature.roi.height = tracked_object.height; + // fit xy offset to 0 if roi is outside of image + const int outside_x = std::max(-tracked_object.x_offset, 0); + const int outside_y = std::max(-tracked_object.y_offset, 0); + const int32_t output_x = std::max(tracked_object.x_offset, 0); + const int32_t output_y = std::max(tracked_object.y_offset, 0); + const int32_t output_width = tracked_object.width - outside_x; + const int32_t output_height = tracked_object.height - outside_y; + // convert int32 to uint32 + object.feature.roi.x_offset = static_cast(output_x); + object.feature.roi.y_offset = static_cast(output_y); + object.feature.roi.width = static_cast(output_width); + 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